bernhard/CanRtDriver
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

./git_commit_push.sh

Browse Source
This commit is contained in:
Bernhard
2025-12-17 19:19:44 +01:00
parent c393706088
commit dcb267df89
7 changed files with 278 additions and 152 deletions
Download Patch File Download Diff File Expand all files Collapse all files

155
can/can_client.c
View File

@@ -1,4 +1,6 @@
#include "main.h"
#include <can/can_client.h> #include <can/can_client.h>
#include <io/io.h> #include <io/io.h>
#include <mqtt/mqtt_client.h> #include <mqtt/mqtt_client.h>
@@ -6,7 +8,10 @@
struct MOTOR_CONTROL_DATA motctrl[MOTOR_COUNT]; struct MOTOR_CONTROL_DATA motctrl[MOTOR_COUNT];
struct CAN_INTERFACE_DATA intf_data[MOTOR_COUNT]; struct CAN_INTERFACE_DATA intf_data[MOTOR_COUNT];
int iBusTimeoutCounter = 0; int iBusTimeoutCounter = 0;
/// @brief Increase counter for cycles without received telegram
/// @param iMotorIndex
void IncBusTimeoutCounter(int iMotorIndex) void IncBusTimeoutCounter(int iMotorIndex)
{ {
if (iBusTimeoutCounter < 2000) if (iBusTimeoutCounter < 2000)
@@ -14,11 +19,17 @@ void IncBusTimeoutCounter(int iMotorIndex)
iBusTimeoutCounter++; iBusTimeoutCounter++;
if (iBusTimeoutCounter >= 2000) if (iBusTimeoutCounter >= 2000)
{ {
// long time no telegram received -> motor is not connected anymore
motctrl[iMotorIndex].nSwitchState = 0; motctrl[iMotorIndex].nSwitchState = 0;
motctrl[iMotorIndex].nDriveConnected = 0;
motctrl[iMotorIndex].nDriveReady = 0;
Can_SetMotorGear(iMotorIndex, 0);
MqttClient_Publish_MotorSwitchState(iMotorIndex, motctrl[iMotorIndex].nSwitchState); MqttClient_Publish_MotorSwitchState(iMotorIndex, motctrl[iMotorIndex].nSwitchState);
} }
} }
} }
/// @brief Open socket of CAN interface for the given motor /// @brief Open socket of CAN interface for the given motor
/// @param iMotorIndex /// @param iMotorIndex
/// @param ifacename /// @param ifacename
@@ -26,12 +37,16 @@ void IncBusTimeoutCounter(int iMotorIndex)
int Can_OpenInterface(int iMotorIndex, const char * ifacename) int Can_OpenInterface(int iMotorIndex, const char * ifacename)
{ {
// Init control data // Init control data
motctrl[iMotorIndex].nDriveConnected = 0;
motctrl[iMotorIndex].nDriveReady = 0;
motctrl[iMotorIndex].iActualMotorPowerW = 0; motctrl[iMotorIndex].iActualMotorPowerW = 0;
motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_NEUTRAL; motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_NEUTRAL;
motctrl[iMotorIndex].iMotorPower = 0; motctrl[iMotorIndex].iMotorPower = 0;
motctrl[iMotorIndex].iMotorPowerPct = 0; motctrl[iMotorIndex].iMotorPowerPct = 0;
motctrl[iMotorIndex].nSwitchState = 0; motctrl[iMotorIndex].nSwitchState = 0;
mylog(LOG_INFO, "CAN: PWR_MIN_PCT=%d PWR_MAX_PCT=%d PWR_STEP=%d", MOTOR_PWR_MIN_PCT, MOTOR_PWR_MAX_PCT, MOTOR_PWR_STEP);
strcpy(intf_data[iMotorIndex].iface_name, ifacename); strcpy(intf_data[iMotorIndex].iface_name, ifacename);
Can_SetMotorGear(iMotorIndex, MOTOR_GEAR_NEUTRAL); Can_SetMotorGear(iMotorIndex, MOTOR_GEAR_NEUTRAL);
Can_SetMotorPower(iMotorIndex, 0); Can_SetMotorPower(iMotorIndex, 0);
@@ -39,7 +54,7 @@ int Can_OpenInterface(int iMotorIndex, const char * ifacename)
// first we have to create a socket // first we have to create a socket
if ((intf_data[iMotorIndex].socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) if ((intf_data[iMotorIndex].socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0)
{ {
printf("Could not create socket for motor %d!\n", iMotorIndex); mylog(LOG_ERR, "CAN: Could not create socket for motor %d!", iMotorIndex);
return 1; return 1;
} }
@@ -47,7 +62,7 @@ int Can_OpenInterface(int iMotorIndex, const char * ifacename)
strcpy(intf_data[iMotorIndex].ifr.ifr_name, intf_data[iMotorIndex].iface_name); strcpy(intf_data[iMotorIndex].ifr.ifr_name, intf_data[iMotorIndex].iface_name);
if (ioctl(intf_data[iMotorIndex].socket, SIOCGIFINDEX, &intf_data[iMotorIndex].ifr) < 0) if (ioctl(intf_data[iMotorIndex].socket, SIOCGIFINDEX, &intf_data[iMotorIndex].ifr) < 0)
{ {
printf("Could not get interface index for motor %d!\n", iMotorIndex); mylog(LOG_ERR, "CAN: Could not get interface index for motor %d!", iMotorIndex);
return 2; return 2;
} }
@@ -57,25 +72,25 @@ int Can_OpenInterface(int iMotorIndex, const char * ifacename)
intf_data[iMotorIndex].addr.can_ifindex = intf_data[iMotorIndex].ifr.ifr_ifindex; intf_data[iMotorIndex].addr.can_ifindex = intf_data[iMotorIndex].ifr.ifr_ifindex;
if (bind(intf_data[iMotorIndex].socket, (struct sockaddr *)&intf_data[iMotorIndex].addr, sizeof(intf_data[iMotorIndex].addr)) < 0) if (bind(intf_data[iMotorIndex].socket, (struct sockaddr *)&intf_data[iMotorIndex].addr, sizeof(intf_data[iMotorIndex].addr)) < 0)
{ {
printf("Could not bind socket to inteface for motor %d!\n", iMotorIndex); mylog(LOG_ERR, "CAN: Could not bind socket to inteface for motor %d!", iMotorIndex);
return 3; return 3;
} }
// socket auf nicht-blockierend umstellen // make socket to nonblocking
int fcntl_flags = fcntl(intf_data[iMotorIndex].socket, F_GETFL, 0); int fcntl_flags = fcntl(intf_data[iMotorIndex].socket, F_GETFL, 0);
if (fcntl_flags < 0) if (fcntl_flags < 0)
{ {
printf("Could not get file descriptor flags!\n"); mylog(LOG_ERR, "CAN: Could not get file descriptor flags!");
return 4; return 4;
} }
fcntl_flags |= O_NONBLOCK; fcntl_flags |= O_NONBLOCK;
if (fcntl(intf_data[iMotorIndex].socket, F_SETFL, fcntl_flags) < 0) if (fcntl(intf_data[iMotorIndex].socket, F_SETFL, fcntl_flags) < 0)
{ {
printf("Could not set file descriptor flags (set socket none-blocking)!\n"); mylog(LOG_ERR, "CAN: Could not set file descriptor flags (set socket none-blocking)!");
return 5; return 5;
} }
printf("Interface %s (motor %d) opened!\n", ifacename, iMotorIndex); mylog(LOG_INFO, "CAN: Interface %s (motor %d) opened!", ifacename, iMotorIndex);
return 0; return 0;
} }
@@ -87,11 +102,11 @@ void Can_CloseInterface(int iMotorIndex)
{ {
if (close(intf_data[iMotorIndex].socket) < 0) if (close(intf_data[iMotorIndex].socket) < 0)
{ {
printf("Could not close socket of motor %d!\n", iMotorIndex); mylog(LOG_ERR, "CAN: Could not close socket of motor %d!", iMotorIndex);
} }
else else
{ {
printf("Interface %s (motor %d) closed.\n", intf_data[iMotorIndex].iface_name, iMotorIndex); mylog(LOG_INFO, "CAN: Interface %s (motor %d) closed.", intf_data[iMotorIndex].iface_name, iMotorIndex);
} }
} }
@@ -101,25 +116,29 @@ void Can_CloseInterface(int iMotorIndex)
/// @param iGear (-1=reverse, 0=neutral, 1=forward) /// @param iGear (-1=reverse, 0=neutral, 1=forward)
void Can_SetMotorGear(int iMotorIndex, int iGear) void Can_SetMotorGear(int iMotorIndex, int iGear)
{ {
if (iGear > 0) if ((iGear > 0) && (motctrl[iMotorIndex].nDriveReady != 0))
{ {
if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_FORWARD) if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_FORWARD)
{ {
MqttClient_Publish_MotorGear(iMotorIndex, iGear); MqttClient_Publish_MotorGear(iMotorIndex, iGear);
motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_FORWARD; motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_FORWARD;
// motor is switched to forward -> set min. power
Can_SetMotorPower(iMotorIndex, MOTOR_PWR_MIN_PCT);
} }
WriteOutputPin(GPIO_LED_MOTRUN, HIGH); WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
printf("Motor[%d]: Set gear forward.\n", iMotorIndex); mylog(LOG_INFO, "CAN: Motor[%d]: Set gear forward.", iMotorIndex);
} }
else if (iGear < 0) else if ((iGear < 0) && (motctrl[iMotorIndex].nDriveReady != 0))
{ {
if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_REVERSE) if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_REVERSE)
{ {
MqttClient_Publish_MotorGear(iMotorIndex, iGear); MqttClient_Publish_MotorGear(iMotorIndex, iGear);
motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_REVERSE; motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_REVERSE;
// motor is switched to reverse -> set min. power
Can_SetMotorPower(iMotorIndex, MOTOR_PWR_MIN_PCT);
} }
WriteOutputPin(GPIO_LED_MOTRUN, HIGH); WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
printf("Motor[%d]: Set gear reverse.\n", iMotorIndex); mylog(LOG_INFO, "CAN: Motor[%d]: Set gear reverse.", iMotorIndex);
} }
else else
{ {
@@ -128,9 +147,10 @@ void Can_SetMotorGear(int iMotorIndex, int iGear)
MqttClient_Publish_MotorGear(iMotorIndex, iGear); MqttClient_Publish_MotorGear(iMotorIndex, iGear);
motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_NEUTRAL; motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_NEUTRAL;
} }
Can_SetMotorPower(iMotorIndex, MOTOR_PWR_MIN_PCT); // motor is switch to neutral -> set power to 0
Can_SetMotorPower(iMotorIndex, 0);
WriteOutputPin(GPIO_LED_MOTRUN, LOW); WriteOutputPin(GPIO_LED_MOTRUN, LOW);
printf("Motor[%d]: Set gear neutral.\n", iMotorIndex); mylog(LOG_INFO, "CAN: Motor[%d]: Set gear neutral.", iMotorIndex);
} }
} }
@@ -140,12 +160,19 @@ void Can_SetMotorGear(int iMotorIndex, int iGear)
/// @param iPower (Range: 0..100) /// @param iPower (Range: 0..100)
void Can_SetMotorPower(int iMotorIndex, int iPower) void Can_SetMotorPower(int iMotorIndex, int iPower)
{ {
if (iPower <= MOTOR_PWR_MIN_PCT) if ((motctrl[iMotorIndex].iMotorGear == MOTOR_GEAR_NEUTRAL) || (motctrl[iMotorIndex].nDriveReady == 0))
{ {
// when motor is neutral or not ready set power to 0
motctrl[iMotorIndex].iMotorPowerPct = 0;
}
else if (iPower <= MOTOR_PWR_MIN_PCT)
{
// limit to min. power
motctrl[iMotorIndex].iMotorPowerPct = MOTOR_PWR_MIN_PCT; motctrl[iMotorIndex].iMotorPowerPct = MOTOR_PWR_MIN_PCT;
} }
else if (iPower >= MOTOR_PWR_MAX_PCT) else if (iPower >= MOTOR_PWR_MAX_PCT)
{ {
// limit to max. power
motctrl[iMotorIndex].iMotorPowerPct = MOTOR_PWR_MAX_PCT; motctrl[iMotorIndex].iMotorPowerPct = MOTOR_PWR_MAX_PCT;
} }
else else
@@ -154,9 +181,10 @@ void Can_SetMotorPower(int iMotorIndex, int iPower)
} }
MqttClient_Publish_MotorPower(iMotorIndex, motctrl[iMotorIndex].iMotorPowerPct); MqttClient_Publish_MotorPower(iMotorIndex, motctrl[iMotorIndex].iMotorPowerPct);
// calc value for telegram
motctrl[iMotorIndex].iMotorPower = 250 * motctrl[iMotorIndex].iMotorPowerPct / 100; motctrl[iMotorIndex].iMotorPower = 250 * motctrl[iMotorIndex].iMotorPowerPct / 100;
printf("Motor[%d]: Set power to %d%% -> %d\n", mylog(LOG_INFO, "CAN: Motor[%d]: Set power to %d%% -> %d",
iMotorIndex, motctrl[iMotorIndex].iMotorPowerPct, motctrl[iMotorIndex].iMotorPower); iMotorIndex, motctrl[iMotorIndex].iMotorPowerPct, motctrl[iMotorIndex].iMotorPower);
} }
@@ -165,27 +193,24 @@ void Can_SetMotorPower(int iMotorIndex, int iPower)
/// @param iMotorIndex /// @param iMotorIndex
void Can_TransmitMotorGear(int iMotorIndex) void Can_TransmitMotorGear(int iMotorIndex)
{ {
//if (motctrl[iMotorIndex].nSwitchState > 0) // Transmission rate: 100ms
struct can_frame frame;
frame.can_id = 0x18F005D0;
frame.can_id |= CAN_EFF_FLAG;
frame.can_dlc = 8;
frame.data[0] = motctrl[iMotorIndex].iMotorGear;
frame.data[1] = 0xFF;
frame.data[2] = 0xFF;
frame.data[3] = 0xFF;
frame.data[4] = 0xFF;
frame.data[5] = 0xFF;
frame.data[6] = 0xFF;
frame.data[7] = 0xFF;
if (write(intf_data[iMotorIndex].socket, &frame, sizeof(frame)) != sizeof(frame))
{ {
// Transmission rate: 100ms
struct can_frame frame;
frame.can_id = 0x18F005D0;
frame.can_id |= CAN_EFF_FLAG;
frame.can_dlc = 8;
frame.data[0] = motctrl[iMotorIndex].iMotorGear;
frame.data[1] = 0xFF;
frame.data[2] = 0xFF;
frame.data[3] = 0xFF;
frame.data[4] = 0xFF;
frame.data[5] = 0xFF;
frame.data[6] = 0xFF;
frame.data[7] = 0xFF;
if (write(intf_data[iMotorIndex].socket, &frame, sizeof(frame)) != sizeof(frame))
{
}
} }
} }
@@ -194,27 +219,24 @@ void Can_TransmitMotorGear(int iMotorIndex)
/// @param iMotorIndex /// @param iMotorIndex
void Can_TransmitMotorPower(int iMotorIndex) void Can_TransmitMotorPower(int iMotorIndex)
{ {
//if (motctrl[iMotorIndex].nSwitchState > 0) // Transmission rate: 50ms
struct can_frame frame;
frame.can_id = 0x0CF003D0;
frame.can_id |= CAN_EFF_FLAG;
frame.can_dlc = 8;
frame.data[0] = 0xFF;
frame.data[1] = motctrl[iMotorIndex].iMotorPower; // motor power 0 = 0%, 250 = 100%
frame.data[2] = 0xFF;
frame.data[3] = 0xFF;
frame.data[4] = 0xFF;
frame.data[5] = 0xFF;
frame.data[6] = 0xFF;
frame.data[7] = 0xFF;
if (write(intf_data[iMotorIndex].socket, &frame, sizeof(frame)) != sizeof(frame))
{ {
// Transmission rate: 50ms
struct can_frame frame;
frame.can_id = 0x0CF003D0;
frame.can_id |= CAN_EFF_FLAG;
frame.can_dlc = 8;
frame.data[0] = 0xFF;
frame.data[1] = motctrl[iMotorIndex].iMotorPower; // motor power 0 = 0%, 250 = 100%
frame.data[2] = 0xFF;
frame.data[3] = 0xFF;
frame.data[4] = 0xFF;
frame.data[5] = 0xFF;
frame.data[6] = 0xFF;
frame.data[7] = 0xFF;
if (write(intf_data[iMotorIndex].socket, &frame, sizeof(frame)) != sizeof(frame))
{
}
} }
} }
@@ -226,30 +248,34 @@ void Can_ReadData(int iMotorIndex)
ssize_t nbytes = 0; ssize_t nbytes = 0;
struct can_frame frame; struct can_frame frame;
// increment cycle counter
IncBusTimeoutCounter(iMotorIndex); IncBusTimeoutCounter(iMotorIndex);
// wir wollen immer alle verfügbaren Frames lesen // read one frame
while ((nbytes = read(intf_data[iMotorIndex].socket, &frame, sizeof(frame))) > 0) if ((nbytes = read(intf_data[iMotorIndex].socket, &frame, sizeof(frame))) > 0)
{ {
canid_t pgn = frame.can_id & 0x00FFFF00; canid_t pgn = frame.can_id & 0x00FFFF00;
switch(pgn) switch(pgn)
{ {
case 0x00EF6400: // "repeat data"
break;
case 0x00F00300: // PGN 61443 "Electronic Engine Controller 2" case 0x00F00300: // PGN 61443 "Electronic Engine Controller 2"
// haben wir selber gesendet -> ignorieren // we have sent this -> ignore
break; break;
case 0x00F00500: // PGN 61445 "Electronic Transmission Controller 2" case 0x00F00500: // PGN 61445 "Electronic Transmission Controller 2"
// haben wir selber gesendet -> ignorieren // we have sent this -> ignore
break; break;
case 0x00FF1300: // PGN 65299 "Manufacturer PGN" case 0x00FF1300: // PGN 65299 "Manufacturer PGN"
// hier finden wir die Zustände der Schalter // here we find the states of the switches
Can_Read_Manu_PGN(iMotorIndex, &frame); Can_Read_Manu_PGN(iMotorIndex, &frame);
break; break;
case 0x00FF1400: // PGN 65300 "Manufacturer PGN 2" case 0x00FF1400: // PGN 65300 "Manufacturer PGN 2"
// hier bekommen wir die Leistung des Motors angezeigt // here we find the actual power of the motor
Can_Read_Manu_PGN2(iMotorIndex, &frame); Can_Read_Manu_PGN2(iMotorIndex, &frame);
break; break;
} }
@@ -260,9 +286,16 @@ void Can_ReadData(int iMotorIndex)
/// @param frame /// @param frame
void Can_Read_Manu_PGN(int iMotorIndex, struct can_frame *frame) void Can_Read_Manu_PGN(int iMotorIndex, struct can_frame *frame)
{ {
// we received a valid telegram -> set timeout counter to zero
iBusTimeoutCounter = 0; iBusTimeoutCounter = 0;
// get switch states
motctrl[iMotorIndex].nSwitchState = frame->data[4]; motctrl[iMotorIndex].nSwitchState = frame->data[4];
MqttClient_Publish_MotorSwitchState(iMotorIndex, motctrl[iMotorIndex].nSwitchState); MqttClient_Publish_MotorSwitchState(iMotorIndex, motctrl[iMotorIndex].nSwitchState);
// set flags
motctrl[iMotorIndex].nDriveConnected = 1; // we received a PGN -> so we are connected
motctrl[iMotorIndex].nDriveReady = ((motctrl[iMotorIndex].nSwitchState & 0x80) != 0) ? 1 : 0; // this bit shows if the drive is ready to run
} }
/// @brief Read PGN 65300 /// @brief Read PGN 65300

5
can/can_client.h
View File

@@ -5,6 +5,7 @@
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
#include <unistd.h> #include <unistd.h>
#include <syslog.h>
#include <fcntl.h> #include <fcntl.h>
#include <net/if.h> #include <net/if.h>
@@ -23,10 +24,12 @@
#define MOTOR_PWR_MIN_PCT 15 #define MOTOR_PWR_MIN_PCT 15
#define MOTOR_PWR_MAX_PCT 100 #define MOTOR_PWR_MAX_PCT 100
#define MOTOR_PWR_STEP 12 #define MOTOR_PWR_STEP ((MOTOR_PWR_MAX_PCT - MOTOR_PWR_MIN_PCT) / 7)
struct MOTOR_CONTROL_DATA struct MOTOR_CONTROL_DATA
{ {
char nDriveConnected;
char nDriveReady;
int iMotorGear; int iMotorGear;
int iMotorPower; int iMotorPower;
int iMotorPowerPct; int iMotorPowerPct;

71
io/io.c
View File

@@ -1,4 +1,5 @@
#include "main.h"
#include "io.h" #include "io.h"
#include <can/can_client.h> #include <can/can_client.h>
@@ -6,29 +7,35 @@ struct GPIO_KEY_DATA gpioKeyStop;
struct GPIO_KEY_DATA gpioKeyPwrUp; struct GPIO_KEY_DATA gpioKeyPwrUp;
struct GPIO_KEY_DATA gpioKeyPwrDown; struct GPIO_KEY_DATA gpioKeyPwrDown;
char nInitialized = 0;
int IO_Init() int IO_Init()
{ {
if (wiringPiSetupPinType(WPI_PIN_BCM)) if (wiringPiSetupPinType(WPI_PIN_BCM))
{ {
printf("IO: Set up wiringPi failed!\n"); mylog(LOG_ERR, "IO: Set up wiringPi failed!");
return 1; return 1;
} }
nInitialized = 1;
// IO-Pins für Tasten konfigurieren // config IO-pins for the keys
SetupKeyPin(&gpioKeyStop, GPIO_KEY_STOP); SetupKeyPin(&gpioKeyStop, GPIO_KEY_STOP);
SetupKeyPin(&gpioKeyPwrUp, GPIO_KEY_PWRUP); SetupKeyPin(&gpioKeyPwrUp, GPIO_KEY_PWRUP);
SetupKeyPin(&gpioKeyPwrDown, GPIO_KEY_PWRDOWN); SetupKeyPin(&gpioKeyPwrDown, GPIO_KEY_PWRDOWN);
// IO-Pins für Ausgänge konfigurieren // config IO-pins for outputs
SetupOutputPin(GPIO_LED_MOTRUN); SetupOutputPin(GPIO_LED_MOTRUN);
SetupOutputPin(GPIO_OUT_PWRON); SetupOutputPin(GPIO_OUT_PWRON);
printf("IO initialized successfull!\n"); mylog(LOG_INFO, "IO: Initialized successfull!");
return 0; return 0;
} }
/// @brief Setup a pin for a key input
/// @param pdata
/// @param iKeyPin
void SetupKeyPin(struct GPIO_KEY_DATA *pdata, int iKeyPin) void SetupKeyPin(struct GPIO_KEY_DATA *pdata, int iKeyPin)
{ {
pdata->iKeyPin = iKeyPin; pdata->iKeyPin = iKeyPin;
@@ -40,24 +47,26 @@ void SetupKeyPin(struct GPIO_KEY_DATA *pdata, int iKeyPin)
pdata->iKeyPressedCycleCounter = 0; pdata->iKeyPressedCycleCounter = 0;
pdata->iKeyRepeatCycleCounter = 0; pdata->iKeyRepeatCycleCounter = 0;
// Wenn Eingang verwendet wird if ((pdata->iKeyPin > 0) && nInitialized)
if (pdata->iKeyPin > 0)
{ {
//mylog(LOG_INFO, "IO: Config Pin %d as input", iOutPin);
pinMode(pdata->iKeyPin, INPUT); pinMode(pdata->iKeyPin, INPUT);
pullUpDnControl(pdata->iKeyPin, PUD_UP); pullUpDnControl(pdata->iKeyPin, PUD_UP);
} }
} }
/// @brief Read a key input
/// @param pdata
void ReadKey(struct GPIO_KEY_DATA *pdata) void ReadKey(struct GPIO_KEY_DATA *pdata)
{ {
if (pdata->iKeyPin > 0) if (pdata->iKeyPin > 0)
{ {
int newval = pdata->iKeyValue; int newval = pdata->iKeyValue;
if (digitalRead(pdata->iKeyPin) == LOW) // invertierte Logik weil wir PullUp-Widerstand bei Betätigung auf low ziehen if (digitalRead(pdata->iKeyPin) == LOW) // we use pull-up resistors so we have inverted logic
{ {
// Signal liegt an // key is pressed
if (pdata->nLowCycleCounter > 0) if (pdata->nLowCycleCounter > 0)
{ {
pdata->nLowCycleCounter--; pdata->nLowCycleCounter--;
@@ -68,14 +77,14 @@ void ReadKey(struct GPIO_KEY_DATA *pdata)
pdata->nHighCycleCounter++; pdata->nHighCycleCounter++;
if (pdata->nHighCycleCounter >= KEY_RISING_FILTERCYCLES) if (pdata->nHighCycleCounter >= KEY_RISING_FILTERCYCLES)
{ {
// gewünschte Anzahl Zyklen stabil // key is stable pressed
newval = 1; newval = 1;
} }
} }
} }
else else
{ {
// Signal liegt nicht an // key is not pressed
if (pdata->nHighCycleCounter > 0) if (pdata->nHighCycleCounter > 0)
{ {
pdata->nHighCycleCounter--; pdata->nHighCycleCounter--;
@@ -86,7 +95,7 @@ void ReadKey(struct GPIO_KEY_DATA *pdata)
pdata->nLowCycleCounter++; pdata->nLowCycleCounter++;
if (pdata->nLowCycleCounter >= KEY_FALLING_FILTERCYCLES) if (pdata->nLowCycleCounter >= KEY_FALLING_FILTERCYCLES)
{ {
// gewünschte Anzahl Zyklen stabil // key is stable not pressed
newval = 0; newval = 0;
} }
} }
@@ -94,7 +103,7 @@ void ReadKey(struct GPIO_KEY_DATA *pdata)
if (newval && !pdata->iKeyValue) if (newval && !pdata->iKeyValue)
{ {
// Taster wurde betätigt // key was pressed -> rising edge
pdata->iKeyRisingEdge = 1; pdata->iKeyRisingEdge = 1;
pdata->iKeyValue = newval; pdata->iKeyValue = newval;
pdata->iKeyPressedCycleCounter = 0; pdata->iKeyPressedCycleCounter = 0;
@@ -102,32 +111,32 @@ void ReadKey(struct GPIO_KEY_DATA *pdata)
} }
else if (pdata->iKeyValue && !newval) else if (pdata->iKeyValue && !newval)
{ {
// Taster wurde losgelassen // key was released -> falling edge
pdata->iKeyFallingEdge = 1; pdata->iKeyFallingEdge = 1;
pdata->iKeyValue = newval; pdata->iKeyValue = newval;
} }
else else
{ {
// Keine Änderung // no change
pdata->iKeyRisingEdge = 0; pdata->iKeyRisingEdge = 0;
pdata->iKeyFallingEdge = 0; pdata->iKeyFallingEdge = 0;
if (pdata->iKeyValue) if (pdata->iKeyValue)
{ {
// Wenn Taste gedrückt ist // when key is pressed
if (pdata->iKeyPressedCycleCounter < KEY_START_REPEAT_CYCLECOUNT) if (pdata->iKeyPressedCycleCounter < KEY_START_REPEAT_CYCLECOUNT)
{ {
// Zyklen zählen // count cycles
pdata->iKeyPressedCycleCounter++; pdata->iKeyPressedCycleCounter++;
} }
if (pdata->iKeyPressedCycleCounter >= KEY_START_REPEAT_CYCLECOUNT) if (pdata->iKeyPressedCycleCounter >= KEY_START_REPEAT_CYCLECOUNT)
{ {
// Wenn Taste länger als KEY_START_REPEAT_CYCLECOUNT gedrückt ist // when key is pressed for more then KEY_START_REPEAT_CYCLECOUNT cycles
pdata->iKeyRepeatCycleCounter++; pdata->iKeyRepeatCycleCounter++;
if (pdata->iKeyRepeatCycleCounter >= KEY_REPEAT_CYCLECOUNT) if (pdata->iKeyRepeatCycleCounter >= KEY_REPEAT_CYCLECOUNT)
{ {
// alle KEY_REPEAT_CYCLECOUNT Zyklen Tastendruck signalisieren // signal key press every KEY_REPEAT_CYCLECOUNT cycles
pdata->iKeyRisingEdge = 1; pdata->iKeyRisingEdge = 1;
pdata->iKeyRepeatCycleCounter = 0; pdata->iKeyRepeatCycleCounter = 0;
} }
@@ -137,24 +146,31 @@ void ReadKey(struct GPIO_KEY_DATA *pdata)
} }
} }
/// @brief Config pin for output
/// @param iOutPin
void SetupOutputPin(int iOutPin) void SetupOutputPin(int iOutPin)
{ {
if (iOutPin > 0) if ((iOutPin > 0) && nInitialized)
{ {
//mylog(LOG_INFO, "IO: Config Pin %d as output", iOutPin);
pinMode(iOutPin, OUTPUT); pinMode(iOutPin, OUTPUT);
digitalWrite(iOutPin, LOW); digitalWrite(iOutPin, LOW);
} }
} }
/// @brief Write an output pin to HIGH or LOW
/// @param iOutPin
/// @param iValue
void WriteOutputPin(int iOutPin, int iValue) void WriteOutputPin(int iOutPin, int iValue)
{ {
if (iOutPin > 0) if ((iOutPin > 0) && nInitialized)
{ {
digitalWrite(iOutPin, iValue); digitalWrite(iOutPin, iValue);
} }
} }
/// @brief look cyclic for the keys
void IO_DoCyclic() void IO_DoCyclic()
{ {
ReadKey(&gpioKeyStop); ReadKey(&gpioKeyStop);
@@ -163,37 +179,42 @@ void IO_DoCyclic()
if (gpioKeyStop.iKeyValue) if (gpioKeyStop.iKeyValue)
{ {
// Stop-Taste betätigt -> hat Vorrang vor den anderen Tasten // stop key is pressed
if (gpioKeyStop.iKeyRisingEdge) if (gpioKeyStop.iKeyRisingEdge)
{ {
mylog(LOG_INFO, "IO: KEY-Stop: Stop motor.");
Can_SetMotorGear(0, 0); Can_SetMotorGear(0, 0);
} }
} }
else else if (motctrl[0].nDriveReady) // plus and minus keys only when drive is ready
{ {
if (gpioKeyPwrUp.iKeyRisingEdge) if (gpioKeyPwrUp.iKeyRisingEdge)
{ {
// Leistung erhöhen // plus key is pressed -> increase power
if (motctrl[0].iMotorGear == MOTOR_GEAR_NEUTRAL) if (motctrl[0].iMotorGear == MOTOR_GEAR_NEUTRAL)
{ {
mylog(LOG_INFO, "KEY-Plus: Start motor.");
Can_SetMotorGear(0, 1); Can_SetMotorGear(0, 1);
Can_SetMotorPower(0, MOTOR_PWR_MIN_PCT); Can_SetMotorPower(0, MOTOR_PWR_MIN_PCT);
} }
else else
{ {
mylog(LOG_INFO, "KEY-Plus: Increase power.");
Can_SetMotorPower(0, motctrl[0].iMotorPowerPct + MOTOR_PWR_STEP); Can_SetMotorPower(0, motctrl[0].iMotorPowerPct + MOTOR_PWR_STEP);
} }
} }
if (gpioKeyPwrDown.iKeyRisingEdge) if (gpioKeyPwrDown.iKeyRisingEdge)
{ {
// Leistung verringern // minus key is pressed -> decrease power
if (motctrl[0].iMotorPowerPct > MOTOR_PWR_MIN_PCT) if (motctrl[0].iMotorPowerPct > MOTOR_PWR_MIN_PCT)
{ {
mylog(LOG_INFO, "KEY-Minus: Decrease power.");
Can_SetMotorPower(0, motctrl[0].iMotorPowerPct - MOTOR_PWR_STEP); Can_SetMotorPower(0, motctrl[0].iMotorPowerPct - MOTOR_PWR_STEP);
} }
else else
{ {
mylog(LOG_INFO, "KEY-Minus: Stop motor.");
Can_SetMotorGear(0, 0); Can_SetMotorGear(0, 0);
} }
} }

21
io/io.h
View File

@@ -3,20 +3,21 @@
#include <wiringPi.h> #include <wiringPi.h>
#include <stdio.h> #include <stdio.h>
#include <syslog.h>
#define GPIO_LED_MOTRUN 17 // GPIO Pin fuer LED Motor läuft #define GPIO_LED_MOTRUN 17 // GPIO Pin for LED motor is running
#define GPIO_OUT_PWRON 22 // GPIO Pin für Relais "Zündschlüssel" #define GPIO_OUT_PWRON 22 // GPIO Pin for output "Ignition Key"
#define GPIO_KEY_STOP 26 // GPIO Pin fuer Taster Stop #define GPIO_KEY_STOP 26 // GPIO Pin for Key "Stop"
#define GPIO_KEY_PWRUP 5 // GPIO Pin fuer Taster Leistung-Erhöhen #define GPIO_KEY_PWRUP 5 // GPIO Pin for Key "Increase Power"
#define GPIO_KEY_PWRDOWN 6 // GPIO Pin fuer Taster Leistung-Verringern #define GPIO_KEY_PWRDOWN 6 // GPIO Pin for Key "Decrease Power"
#define KEY_RISING_FILTERCYCLES 5 // Filterwert für Eingänge steigende Flanke #define KEY_RISING_FILTERCYCLES 5 // filter value for input rising edge
#define KEY_FALLING_FILTERCYCLES 15 // Filterwert für Eingänge (Zyklen-Zähler) #define KEY_FALLING_FILTERCYCLES 15 // filter value for input falling edge
#define KEY_START_REPEAT_CYCLECOUNT 50 // Anzahl Zyklen, nach denen Wiederholungen beginnen #define KEY_START_REPEAT_CYCLECOUNT 50 // number of cycles when to start repeating key presses
#define KEY_REPEAT_CYCLECOUNT 50 // Anzahl Zyklen, nach den wiederholt wird #define KEY_REPEAT_CYCLECOUNT 50 // number of cycles how often to repeat key presses
// Datenstruktur für einen Taster // data structure for a key
struct GPIO_KEY_DATA struct GPIO_KEY_DATA
{ {
int iKeyPin; int iKeyPin;

93
main.c
View File

@@ -7,6 +7,38 @@
// Period info of the realtime task // Period info of the realtime task
struct period_info pinfo; struct period_info pinfo;
int iThreadControl = 0; // 0: thread is running, <0: thread shall exit, >0 thread has exited int iThreadControl = 0; // 0: thread is running, <0: thread shall exit, >0 thread has exited
int iLogToConsole = 1;
/// @brief send a log message
/// @param prio
/// @param format
/// @param
void mylog(int prio, const char *format, ...)
{
va_list args;
// 1. Initialisiere die Argumentenliste mit dem letzten festen Parameter
va_start(args, format);
// 2. Übergabe an vsyslog (statt syslog)
// vsyslog nimmt eine va_list entgegen
vsyslog(prio, format, args);
// 3. Optional: Zusätzlich auf die Konsole ausgeben
// Wir müssen die Liste neu initialisieren, da va_list "verbraucht" wird
if (iLogToConsole)
{
va_end(args);
va_start(args, format);
vfprintf(stderr, format, args);
fprintf(stderr, "\n");
}
// 4. Aufräumen
va_end(args);
}
/// @brief Initialize period_info with period_ms for cyclic task /// @brief Initialize period_info with period_ms for cyclic task
/// @param period_ms /// @param period_ms
@@ -77,7 +109,6 @@ static void do_cyclic_1ms(struct period_info *pinfo)
// called every 100ms // called every 100ms
nCalled |= 0x0010; nCalled |= 0x0010;
Can_TransmitMotorGear(0); Can_TransmitMotorGear(0);
//printf("%.3f: 100ms-Cycle %ld...\n", clock_gettime_s() - pinfo->fStartTime, pinfo->cyclecounter);
} }
if (((pinfo->cyclecounter + 20) % 100) == 0) if (((pinfo->cyclecounter + 20) % 100) == 0)
@@ -89,14 +120,9 @@ static void do_cyclic_1ms(struct period_info *pinfo)
if (((pinfo->cyclecounter + 30) % 500) == 0) if (((pinfo->cyclecounter + 30) % 500) == 0)
{ {
// called every 250ms // called every 500ms
MqttClient_Publisher(); MqttClient_Publisher();
} }
if (nCalled > 0)
{
//printf("%.3f: Called 0x%.4X at cycle %ld...\n", clock_gettime_s() - pinfo->fStartTime, nCalled, pinfo->cyclecounter);
}
} }
@@ -105,31 +131,31 @@ static void do_cyclic_1ms(struct period_info *pinfo)
/// @return /// @return
void *thread_func(void *data) void *thread_func(void *data)
{ {
// Initialize IO Ports
if (IO_Init())
{
mylog(LOG_ERR, "IO_Init() failed!");
return NULL;
}
// Open CAN interface first motor // Open CAN interface first motor
if (Can_OpenInterface(0, "can0")) if (Can_OpenInterface(0, "can0"))
{ {
printf("Can_OpenInterface() failed!\n"); mylog(LOG_ERR, "Can_OpenInterface() failed!");
return NULL; return NULL;
} }
// Connect to mqtt broker // Connect to mqtt broker
while (MqttClient_Connect() && (iThreadControl == 0)) while (MqttClient_Connect() && (iThreadControl == 0))
{ {
printf("MqttClient_Connect() failed!\n"); mylog(LOG_ERR, "MqttClient_Connect() failed!");
sleep(10); sleep(10);
} }
// Initialize IO Ports
if (IO_Init())
{
printf("IO_Init() failed!\n");
return NULL;
}
// initialize cyclic task // initialize cyclic task
periodic_task_init(1, &pinfo); periodic_task_init(1, &pinfo);
// "Zündung" ein // Ignition on
WriteOutputPin(GPIO_OUT_PWRON, HIGH); WriteOutputPin(GPIO_OUT_PWRON, HIGH);
// cyclic call of do_cyclic_1ms() // cyclic call of do_cyclic_1ms()
@@ -145,7 +171,7 @@ void *thread_func(void *data)
wait_rest_of_period(&pinfo); wait_rest_of_period(&pinfo);
} }
// "Zündung" aus // Ignition off
WriteOutputPin(GPIO_OUT_PWRON, LOW); WriteOutputPin(GPIO_OUT_PWRON, LOW);
// Disconnect from mqtt broker // Disconnect from mqtt broker
@@ -154,6 +180,7 @@ void *thread_func(void *data)
// Close CAN interface // Close CAN interface
Can_CloseInterface(0); Can_CloseInterface(0);
// signal thread has finnished
iThreadControl = 1; iThreadControl = 1;
return NULL; return NULL;
} }
@@ -165,13 +192,13 @@ void sig_handler(int signo)
{ {
if ((signo == SIGINT) || (signo == SIGTERM)) if ((signo == SIGINT) || (signo == SIGTERM))
{ {
printf("Received signal %d\n", signo); mylog(LOG_INFO, "Received signal %d", signo);
iThreadControl = -1; // signal realtime thread to exit iThreadControl = -1; // signal realtime thread to exit
} }
} }
/// @brief Hauptfunktion Echtzeit-Task erstellen und starten /// @brief Main function, create and start realtime task
/// @param argc /// @param argc
/// @param argv /// @param argv
/// @return /// @return
@@ -182,22 +209,25 @@ int main(int argc, char* argv[])
pthread_t thread; pthread_t thread;
int ret; int ret;
openlog("CanRtDriver", LOG_PID | LOG_CONS, LOG_DAEMON);
mylog(LOG_INFO, "Service started. PID: %d", getpid());
// catch signals // catch signals
if (signal(SIGTERM, sig_handler) == SIG_ERR) if (signal(SIGTERM, sig_handler) == SIG_ERR)
{ {
printf("Can't catch SIGTERM\n"); mylog(LOG_ERR, "Can't catch SIGTERM");
exit(-1); exit(-1);
} }
if (signal(SIGINT, sig_handler) == SIG_ERR) if (signal(SIGINT, sig_handler) == SIG_ERR)
{ {
printf("Can't catch SIGINT\n"); mylog(LOG_ERR, "Can't catch SIGINT");
exit(-2); exit(-2);
} }
/* Lock memory */ /* Lock memory */
if(mlockall(MCL_CURRENT | MCL_FUTURE) == -1) if(mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
{ {
printf("mlockall failed: %m\n"); mylog(LOG_ERR, "mlockall failed: %m");
exit(-3); exit(-3);
} }
@@ -205,7 +235,7 @@ int main(int argc, char* argv[])
ret = pthread_attr_init(&attr); ret = pthread_attr_init(&attr);
if (ret) if (ret)
{ {
printf("init pthread attributes failed\n"); mylog(LOG_ERR, "init pthread attributes failed");
goto out; goto out;
} }
@@ -213,7 +243,7 @@ int main(int argc, char* argv[])
ret = pthread_attr_setstacksize(&attr, PTHREAD_STACK_MIN); ret = pthread_attr_setstacksize(&attr, PTHREAD_STACK_MIN);
if (ret) if (ret)
{ {
printf("pthread setstacksize failed\n"); mylog(LOG_ERR, "pthread setstacksize failed");
goto out; goto out;
} }
@@ -221,21 +251,21 @@ int main(int argc, char* argv[])
ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO); ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
if (ret) if (ret)
{ {
printf("pthread setschedpolicy failed\n"); mylog(LOG_ERR, "pthread setschedpolicy failed");
goto out; goto out;
} }
param.sched_priority = 99; // Priority between 1 (low) and 99() high) param.sched_priority = 99; // Priority between 1 (low) and 99() high)
ret = pthread_attr_setschedparam(&attr, &param); ret = pthread_attr_setschedparam(&attr, &param);
if (ret) if (ret)
{ {
printf("pthread setschedparam failed\n"); mylog(LOG_ERR, "pthread setschedparam failed");
goto out; goto out;
} }
/* Use scheduling parameters of attr */ /* Use scheduling parameters of attr */
ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
if (ret) if (ret)
{ {
printf("pthread setinheritsched failed\n"); mylog(LOG_ERR, "pthread setinheritsched failed");
goto out; goto out;
} }
@@ -245,7 +275,7 @@ int main(int argc, char* argv[])
ret = pthread_create(&thread, &attr, thread_func, NULL); ret = pthread_create(&thread, &attr, thread_func, NULL);
if (ret) if (ret)
{ {
printf("create pthread failed\n"); mylog(LOG_ERR, "create pthread failed");
goto out; goto out;
} }
@@ -253,10 +283,13 @@ int main(int argc, char* argv[])
ret = pthread_join(thread, NULL); ret = pthread_join(thread, NULL);
if (ret) if (ret)
{ {
printf("faild to join thread!\n"); mylog(LOG_ERR, "faild to join thread!");
} }
out: out:
mylog(LOG_INFO, "Service quit.");
closelog();
return ret; return ret;
} }

3
main.h
View File

@@ -11,6 +11,8 @@
#include <time.h> #include <time.h>
#include <errno.h> #include <errno.h>
#include <signal.h> #include <signal.h>
#include <syslog.h>
#include <stdarg.h>
struct period_info struct period_info
@@ -22,5 +24,6 @@ struct period_info
}; };
extern struct period_info pinfo; extern struct period_info pinfo;
extern void mylog(int prio, const char *format, ...);
#endif #endif

80
mqtt/mqtt_client.c
View File

@@ -5,7 +5,7 @@
https://mosquitto.org/api/files/mosquitto-h.html https://mosquitto.org/api/files/mosquitto-h.html
*/ */
#include <main.h> #include "main.h"
#include <mqtt/mqtt_client.h> #include <mqtt/mqtt_client.h>
#include <can/can_client.h> #include <can/can_client.h>
#include <string.h> #include <string.h>
@@ -39,6 +39,11 @@ struct mosquitto *mosq; /**< Libmosquito MQTT client instance. */
int iHadConnectError = 0; int iHadConnectError = 0;
/// @brief callback function for incoming mqtt messages
/// @param mosq
/// @param userdata
/// @param message
void my_message_callback(struct mosquitto *mosq, void *userdata, const struct mosquitto_message *message) void my_message_callback(struct mosquitto *mosq, void *userdata, const struct mosquitto_message *message)
{ {
char* topic_value = (char *)malloc(message->payloadlen + 1); char* topic_value = (char *)malloc(message->payloadlen + 1);
@@ -50,13 +55,13 @@ void my_message_callback(struct mosquitto *mosq, void *userdata, const struct mo
int val = 9999; int val = 9999;
if (sscanf(topic_value, "%d", &val)) if (sscanf(topic_value, "%d", &val))
{ {
printf("%ld: Received value for mqtt_topic_motor1_gear: %d\n", pinfo.cyclecounter, val); mylog(LOG_INFO, "MQTT: Received value for mqtt_topic_motor1_gear: %d", val);
iMqttMotor1Gear = val; iMqttMotor1Gear = val;
Can_SetMotorGear(0, val); Can_SetMotorGear(0, val);
} }
else else
{ {
printf("%ld: Received mqtt_topic_motor1_gear: %s\n", pinfo.cyclecounter, topic_value); mylog(LOG_WARNING, "MQTT: Received mqtt_topic_motor1_gear: %s", topic_value);
} }
} }
else if (strcmp(message->topic, mqtt_topic_motor1_power) == 0) else if (strcmp(message->topic, mqtt_topic_motor1_power) == 0)
@@ -64,13 +69,13 @@ void my_message_callback(struct mosquitto *mosq, void *userdata, const struct mo
int val = 9999; int val = 9999;
if (sscanf(topic_value, "%d", &val)) if (sscanf(topic_value, "%d", &val))
{ {
printf("%ld: Received value for mqtt_topic_motor1_power: %d\n", pinfo.cyclecounter, val); mylog(LOG_INFO, "MQTT: Received value for mqtt_topic_motor1_power: %d", val);
iMqttMotor1Power = val; iMqttMotor1Power = val;
Can_SetMotorPower(0, val); Can_SetMotorPower(0, val);
} }
else else
{ {
printf("%ld: Received mqtt_topic_motor1_power: %s\n", pinfo.cyclecounter, topic_value); mylog(LOG_WARNING, "MQTT: Received mqtt_topic_motor1_power: %s", topic_value);
} }
} }
else if (strcmp(message->topic, mqtt_topic_motor2_gear) == 0) else if (strcmp(message->topic, mqtt_topic_motor2_gear) == 0)
@@ -78,13 +83,13 @@ void my_message_callback(struct mosquitto *mosq, void *userdata, const struct mo
int val = 9999; int val = 9999;
if (sscanf(topic_value, "%d", &val)) if (sscanf(topic_value, "%d", &val))
{ {
printf("%ld: Received value for mqtt_topic_motor2_gear: %d\n", pinfo.cyclecounter, val); mylog(LOG_INFO, "MQTT: Received value for mqtt_topic_motor2_gear: %d", val);
iMqttMotor2Gear = val; iMqttMotor2Gear = val;
Can_SetMotorGear(1, val); Can_SetMotorGear(1, val);
} }
else else
{ {
printf("%ld: Received mqtt_topic_motor2_gear: %s\n", pinfo.cyclecounter, topic_value); mylog(LOG_WARNING, "MQTT: Received mqtt_topic_motor2_gear: %s", topic_value);
} }
} }
else if (strcmp(message->topic, mqtt_topic_motor2_power) == 0) else if (strcmp(message->topic, mqtt_topic_motor2_power) == 0)
@@ -92,30 +97,32 @@ void my_message_callback(struct mosquitto *mosq, void *userdata, const struct mo
int val = 9999; int val = 9999;
if (sscanf(topic_value, "%d", &val)) if (sscanf(topic_value, "%d", &val))
{ {
printf("%ld: Received value for mqtt_topic_motor2_power: %d\n", pinfo.cyclecounter, val); mylog(LOG_INFO, "MQTT: Received value for mqtt_topic_motor2_power: %d", val);
iMqttMotor2Power = val; iMqttMotor2Power = val;
Can_SetMotorPower(1, val); Can_SetMotorPower(1, val);
} }
else else
{ {
printf("%ld: Received mqtt_topic_motor2_power: %s\n", pinfo.cyclecounter, topic_value); mylog(LOG_WARNING, "MQTT: Received mqtt_topic_motor2_power: %s", topic_value);
} }
} }
else else
{ {
printf("%ld: Received publish('%s'): %s\n", pinfo.cyclecounter, message->topic, topic_value); mylog(LOG_WARNING, "MQTT: Received publish('%s'): %s", message->topic, topic_value);
} }
free(topic_value); free(topic_value);
} }
/// @brief connect to mqtt broker
/// @return
int MqttClient_Connect() int MqttClient_Connect()
{ {
int major, minor, revision; int major, minor, revision;
mosquitto_lib_version(&major, &minor, &revision); mosquitto_lib_version(&major, &minor, &revision);
printf("Libmosquitto version: %d.%d.%d\n", major, minor, revision); mylog(LOG_INFO, "MQTT: Libmosquitto version: %d.%d.%d", major, minor, revision);
// libmosquitto initialization // libmosquitto initialization
mosquitto_lib_init(); mosquitto_lib_init();
@@ -124,7 +131,7 @@ int MqttClient_Connect()
mosq = mosquitto_new(NULL, true, NULL); mosq = mosquitto_new(NULL, true, NULL);
if (mosq == NULL) if (mosq == NULL)
{ {
printf("Failed to create mosquitto client!/n"); mylog(LOG_ERR, "MQTT: Failed to create mosquitto client!/n");
iHadConnectError++; iHadConnectError++;
return 1; return 1;
} }
@@ -135,7 +142,7 @@ int MqttClient_Connect()
// Connect to MQTT broker // Connect to MQTT broker
if (mosquitto_connect(mosq, mqtt_broker_addr, mqtt_broker_port, 60) != MOSQ_ERR_SUCCESS) if (mosquitto_connect(mosq, mqtt_broker_addr, mqtt_broker_port, 60) != MOSQ_ERR_SUCCESS)
{ {
printf("Error: connecting to MQTT broker failed\n"); mylog(LOG_ERR, "MQTT: Connecting to MQTT broker failed");
MqttClient_Close(); MqttClient_Close();
iHadConnectError++; iHadConnectError++;
return 2; return 2;
@@ -153,28 +160,29 @@ int MqttClient_Connect()
snprintf(message, sizeof(message), "0"); snprintf(message, sizeof(message), "0");
if (mosquitto_publish(mosq, NULL, mqtt_topic_motor1_gear, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS) if (mosquitto_publish(mosq, NULL, mqtt_topic_motor1_gear, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS)
{ {
printf("mosquitto_publish(mqtt_topic_motor1_gear) fehlgeschlagen!\n"); mylog(LOG_ERR, "MQTT: mosquitto_publish(mqtt_topic_motor1_gear) failed!");
MqttClient_Close(); MqttClient_Close();
iHadConnectError++; iHadConnectError++;
return 10; return 10;
} }
if (mosquitto_publish(mosq, NULL, mqtt_topic_motor1_power, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS) if (mosquitto_publish(mosq, NULL, mqtt_topic_motor1_power, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS)
{ {
printf("mosquitto_publish(mqtt_topic_motor1_power) fehlgeschlagen!\n"); mylog(LOG_ERR, "MQTT: mosquitto_publish(mqtt_topic_motor1_power) failed!");
MqttClient_Close(); MqttClient_Close();
iHadConnectError++; iHadConnectError++;
return 11; return 11;
} }
if (mosquitto_publish(mosq, NULL, mqtt_topic_motor2_gear, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS) if (mosquitto_publish(mosq, NULL, mqtt_topic_motor2_gear, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS)
{ {
printf("mosquitto_publish(mqtt_topic_motor2_gear) fehlgeschlagen!\n"); mylog(LOG_ERR, "MQTT: mosquitto_publish(mqtt_topic_motor2_gear) failed!");
MqttClient_Close(); MqttClient_Close();
iHadConnectError++; iHadConnectError++;
return 12; return 12;
MqttClient_Close();
} }
if (mosquitto_publish(mosq, NULL, mqtt_topic_motor2_power, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS) if (mosquitto_publish(mosq, NULL, mqtt_topic_motor2_power, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS)
{ {
printf("mosquitto_publish(mqtt_topic_motor2_power) fehlgeschlagen!\n"); mylog(LOG_ERR, "MQTT: mosquitto_publish(mqtt_topic_motor2_power) failed!");
MqttClient_Close(); MqttClient_Close();
iHadConnectError++; iHadConnectError++;
return 13; return 13;
@@ -183,54 +191,65 @@ int MqttClient_Connect()
// subscribe all needed topics // subscribe all needed topics
if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor1_gear, 0) != MOSQ_ERR_SUCCESS) if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor1_gear, 0) != MOSQ_ERR_SUCCESS)
{ {
printf("mosquitto_subscribe(mqtt_topic_motor1_gear) fehlgeschlagen!\n"); mylog(LOG_ERR, "MQTT: mosquitto_subscribe(mqtt_topic_motor1_gear) failed!");
MqttClient_Close();
iHadConnectError++; iHadConnectError++;
return 20; return 20;
} }
if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor1_power, 0) != MOSQ_ERR_SUCCESS) if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor1_power, 0) != MOSQ_ERR_SUCCESS)
{ {
printf("mosquitto_subscribe(mqtt_topic_motor1_power) fehlgeschlagen!\n"); mylog(LOG_ERR, "MQTT: mosquitto_subscribe(mqtt_topic_motor1_power) failed!");
MqttClient_Close(); MqttClient_Close();
iHadConnectError++; iHadConnectError++;
return 21; return 21;
} }
if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor2_gear, 0) != MOSQ_ERR_SUCCESS) if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor2_gear, 0) != MOSQ_ERR_SUCCESS)
{ {
printf("mosquitto_subscribe(mqtt_topic_motor2_gear) fehlgeschlagen!\n"); mylog(LOG_ERR, "MQTT: mosquitto_subscribe(mqtt_topic_motor2_gear) failed!");
MqttClient_Close(); MqttClient_Close();
iHadConnectError++; iHadConnectError++;
return 22; return 22;
} }
if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor2_power, 0) != MOSQ_ERR_SUCCESS) if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor2_power, 0) != MOSQ_ERR_SUCCESS)
{ {
printf("mosquitto_subscribe(mqtt_topic_motor2_power) fehlgeschlagen!\n"); mylog(LOG_ERR, "MQTT: mosquitto_subscribe(mqtt_topic_motor2_power) failed!");
MqttClient_Close(); MqttClient_Close();
iHadConnectError++; iHadConnectError++;
return 23; return 23;
} }
printf("MQTT: Connected successfull!\n"); nMqttMotor1SwitchState = 255;
MqttClient_Publish_MotorSwitchState(0, 0);
nMqttMotor2SwitchState = 255;
MqttClient_Publish_MotorSwitchState(1, 0);
iMqttMotor1ActualPowerW = -1;
MqttClient_Publish_MotorActualPowerW(0, 0);
iMqttMotor2ActualPowerW = -1;
MqttClient_Publish_MotorActualPowerW(1, 0);
mylog(LOG_INFO, "MQTT: Connected successfull!");
return 0; return 0;
} }
/// @brief Disconnect from mqtt broker
void MqttClient_Close() void MqttClient_Close()
{ {
//Clean up/destroy objects created by libmosquitto //Clean up/destroy objects created by libmosquitto
mosquitto_destroy(mosq); mosquitto_destroy(mosq);
mosquitto_lib_cleanup(); mosquitto_lib_cleanup();
printf("MQTT: Disconnected!\n"); mylog(LOG_INFO, "MQTT: Disconnected!");
} }
/// @brief Has to be called cyclic, does the work
void MqttClient_Refresher() void MqttClient_Refresher()
{ {
mosquitto_loop(mosq, 0, 1); mosquitto_loop(mosq, 0, 1);
} }
/// @brief publish the cycle counter as a sign of life
void MqttClient_Publisher() void MqttClient_Publisher()
{ {
char message[100]; char message[100];
@@ -240,6 +259,9 @@ void MqttClient_Publisher()
} }
/// @brief Publish the actual motor gear
/// @param iMotorIndex
/// @param iGear
void MqttClient_Publish_MotorGear(int iMotorIndex, int iGear) void MqttClient_Publish_MotorGear(int iMotorIndex, int iGear)
{ {
if (iMotorIndex == 0) if (iMotorIndex == 0)
@@ -265,6 +287,9 @@ void MqttClient_Publish_MotorGear(int iMotorIndex, int iGear)
} }
/// @brief Publish the requested motor power
/// @param iMotorIndex
/// @param iPower
void MqttClient_Publish_MotorPower(int iMotorIndex, int iPower) void MqttClient_Publish_MotorPower(int iMotorIndex, int iPower)
{ {
if (iMotorIndex == 0) if (iMotorIndex == 0)
@@ -289,6 +314,10 @@ void MqttClient_Publish_MotorPower(int iMotorIndex, int iPower)
} }
} }
/// @brief Publish the actual switches states
/// @param iMotorIndex
/// @param nSwitchState
void MqttClient_Publish_MotorSwitchState(int iMotorIndex, unsigned char nSwitchState) void MqttClient_Publish_MotorSwitchState(int iMotorIndex, unsigned char nSwitchState)
{ {
if (iMotorIndex == 0) if (iMotorIndex == 0)
@@ -314,6 +343,9 @@ void MqttClient_Publish_MotorSwitchState(int iMotorIndex, unsigned char nSwitchS
} }
/// @brief Publish the actual real motor power
/// @param iMotorIndex
/// @param iMotorPowerW
void MqttClient_Publish_MotorActualPowerW(int iMotorIndex, int iMotorPowerW) void MqttClient_Publish_MotorActualPowerW(int iMotorIndex, int iMotorPowerW)
{ {
if (iMotorIndex == 0) if (iMotorIndex == 0)