Änderung CYCLE_COUNTER_MAX für Testzwecke

can/can_client.c

@@ -1,10 +1,35 @@
 
+#include "main.h"
+
+#include <settings/settings.h>
 #include <can/can_client.h>
 #include <io/io.h>
 #include <mqtt/mqtt_client.h>
 
-struct MOTOR_CONTROL_DATA motctrl[MOTOR_COUNT];
-struct CAN_INTERFACE_DATA intf_data[MOTOR_COUNT];
+struct MOTOR_CONTROL_DATA motctrl[MAX_MOTOR_COUNT];
+struct CAN_INTERFACE_DATA intf_data[MAX_MOTOR_COUNT];
+
+
+int iBusTimeoutCounter = 0;
+/// @brief Increase counter for cycles without received telegram
+/// @param iMotorIndex 
+void IncBusTimeoutCounter(int iMotorIndex)
+{
+    if (iBusTimeoutCounter < 2000)
+    {
+        iBusTimeoutCounter++;
+        if (iBusTimeoutCounter >= 2000)
+        {
+            // long time no telegram received -> motor is not connected anymore
+            motctrl[iMotorIndex].nSwitchState = 0;
+            motctrl[iMotorIndex].nDriveConnected = 0;
+            motctrl[iMotorIndex].nDriveReady = 0;
+            Can_SetMotorGear(iMotorIndex, 0);
+            MqttClient_Publish_MotorSwitchState(iMotorIndex, motctrl[iMotorIndex].nSwitchState);
+        }
+    }
+}
+
 
 /// @brief Open socket of CAN interface for the given motor
 /// @param iMotorIndex 
@@ -12,14 +37,36 @@ struct CAN_INTERFACE_DATA intf_data[MOTOR_COUNT];
 /// @return 
 int Can_OpenInterface(int iMotorIndex, const char * ifacename)
 {
+    // Init control data
+    motctrl[iMotorIndex].iActualMotorPowerW = 0;
+    motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_NEUTRAL;
+    motctrl[iMotorIndex].iMotorPowerRaw = 0;
+    motctrl[iMotorIndex].iMotorPowerSteps = 0;
+    motctrl[iMotorIndex].nSwitchState = 0;
+    motctrl[iMotorIndex].nDriveConnected = 0;
+    motctrl[iMotorIndex].nDriveReady = 0;
+
+    mylog(LOG_INFO, "CAN: PWR_MIN_RAW=%d PWR_MAX_RAW=%d PWR_STEP_COUNT=%d", 
+        settings.iMotorPwrMinRaw, settings.iMotorPwrMaxRaw, settings.iMotorPwrStepCount);
+    
     strcpy(intf_data[iMotorIndex].iface_name, ifacename);
-    Can_SetMotorGear(iMotorIndex, 0);
+    Can_SetMotorGear(iMotorIndex, MOTOR_GEAR_NEUTRAL);
     Can_SetMotorPower(iMotorIndex, 0);
 
+    if (settings.iCanSimu)
+    {
+        mylog(LOG_WARNING, "CAN: Using simulation mode (motor %d).", iMotorIndex);
+        intf_data[iMotorIndex].socket = -1;
+        motctrl[iMotorIndex].nSwitchState = 0xF5;
+        motctrl[iMotorIndex].nDriveConnected = 1;
+        motctrl[iMotorIndex].nDriveReady = 1;
+    }
+    else
+    {
         // first we have to create a socket
         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;
         }
 
@@ -27,7 +74,7 @@ int Can_OpenInterface(int iMotorIndex, const char * ifacename)
         strcpy(intf_data[iMotorIndex].ifr.ifr_name, intf_data[iMotorIndex].iface_name);
         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;
         }
 
@@ -37,11 +84,26 @@ int Can_OpenInterface(int iMotorIndex, const char * ifacename)
         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)
         {
-        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;
         }
 
-    printf("Interface %s (motor %d) opened!\n", ifacename, iMotorIndex);
+        // make socket to nonblocking
+        int fcntl_flags = fcntl(intf_data[iMotorIndex].socket, F_GETFL, 0);
+        if (fcntl_flags < 0)
+        {
+            mylog(LOG_ERR, "CAN: Could not get file descriptor flags!");
+            return 4;
+        }
+        fcntl_flags |= O_NONBLOCK;
+        if (fcntl(intf_data[iMotorIndex].socket, F_SETFL, fcntl_flags) < 0)
+        {
+            mylog(LOG_ERR, "CAN: Could not set file descriptor flags (set socket none-blocking)!");
+            return 5;
+        }
+
+        mylog(LOG_INFO, "CAN: Interface %s (motor %d) opened!", ifacename, iMotorIndex);
+    }
 
     return 0;
 }
@@ -50,14 +112,21 @@ int Can_OpenInterface(int iMotorIndex, const char * ifacename)
 /// @brief Close socket of CAN interface for the given motor
 /// @param iMotorIndex 
 void Can_CloseInterface(int iMotorIndex)
+{
+    if (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
         {
-        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);
+        }
+    }
+    else
+    {
+        mylog(LOG_INFO, "CAN: Close simulation mode.");
     }
 }
 
@@ -67,63 +136,90 @@ void Can_CloseInterface(int iMotorIndex)
 /// @param iGear (-1=reverse, 0=neutral, 1=forward)
 void Can_SetMotorGear(int iMotorIndex, int iGear)
 {
-    if (iGear > 0)
+    if ((iGear > 0) && (motctrl[iMotorIndex].nDriveReady != 0))
     {
         if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_FORWARD)
         {
-            MqttClient_Publish_MotorGear(iMotorIndex, iGear);
             motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_FORWARD;
+            // motor is switched to forward -> set min. power
+            Can_SetMotorPower(iMotorIndex, 1);
         }
         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)
         {
-            MqttClient_Publish_MotorGear(iMotorIndex, iGear);
             motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_REVERSE;
+            // motor is switched to reverse -> set min. power
+            Can_SetMotorPower(iMotorIndex, 1);
         }
         WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
-        printf("Motor[%d]: Set gear reverse.\n", iMotorIndex);
+        mylog(LOG_INFO, "CAN: Motor[%d]: Set gear reverse.", iMotorIndex);
     }
     else 
     {
         if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_NEUTRAL)
         {
-            MqttClient_Publish_MotorGear(iMotorIndex, iGear);
             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);
-        printf("Motor[%d]: Set gear neutral.\n", iMotorIndex);
+        mylog(LOG_INFO, "CAN: Motor[%d]: Set gear neutral.", iMotorIndex);
     } 
+    MqttClient_Publish_MotorGear(iMotorIndex, iGear);
 }
 
 
 /// @brief Set power for the given motor
 /// @param iMotorIndex 
-/// @param iPower (Range: 0..100)
+/// @param iPower (Range: 0..settings.iMotorPwrStepCount)
 void Can_SetMotorPower(int iMotorIndex, int iPower)
 {
-    if (iPower <= MOTOR_PWR_MIN_PCT)
+    if ((motctrl[iMotorIndex].iMotorGear == MOTOR_GEAR_NEUTRAL) || (motctrl[iMotorIndex].nDriveReady == 0))
     {
-        motctrl[iMotorIndex].iMotorPowerPct = MOTOR_PWR_MIN_PCT;
+        // when motor is neutral or not ready set power to 0
+        motctrl[iMotorIndex].iMotorPowerSteps = 0;
     }
-    else if (iPower >= MOTOR_PWR_MAX_PCT)
+    else if (iPower <= 1)
     {
-        motctrl[iMotorIndex].iMotorPowerPct = MOTOR_PWR_MAX_PCT;
+        // limit to min. power
+        motctrl[iMotorIndex].iMotorPowerSteps = 1;
+    }
+    else if (iPower >= settings.iMotorPwrStepCount)
+    {
+        // limit to max. power
+        motctrl[iMotorIndex].iMotorPowerSteps = settings.iMotorPwrStepCount;
     } 
     else 
     {
-        motctrl[iMotorIndex].iMotorPowerPct = iPower;
+        motctrl[iMotorIndex].iMotorPowerSteps = iPower;
     }
-    MqttClient_Publish_MotorPower(iMotorIndex, motctrl[iMotorIndex].iMotorPowerPct);
+    MqttClient_Publish_MotorPower(iMotorIndex, motctrl[iMotorIndex].iMotorPowerSteps);
 
-    motctrl[iMotorIndex].iMotorPower = 250 * motctrl[iMotorIndex].iMotorPowerPct / 100;
+    // calc value for telegram
+    if (motctrl[iMotorIndex].iMotorPowerSteps <= 0)
+    {
+        motctrl[iMotorIndex].iMotorPowerRaw = 0;
+    }
+    else
+    {
+        // Scale motor power from steps to raw value
+        motctrl[iMotorIndex].iMotorPowerRaw = (((settings.iMotorPwrMaxRaw - settings.iMotorPwrMinRaw) * (motctrl[iMotorIndex].iMotorPowerSteps - 1)) / (settings.iMotorPwrStepCount - 1)) + settings.iMotorPwrMinRaw;
+        if (motctrl[iMotorIndex].iMotorPowerRaw < settings.iMotorPwrMinRaw)
+        {
+            motctrl[iMotorIndex].iMotorPowerRaw = settings.iMotorPwrMinRaw;
+        }
+        else if (motctrl[iMotorIndex].iMotorPowerRaw > settings.iMotorPwrMaxRaw)
+        {
+            motctrl[iMotorIndex].iMotorPowerRaw = settings.iMotorPwrMaxRaw;
+        }
+    }
 
-    printf("Motor[%d]: Set power to %d%% -> %d\n",
-        iMotorIndex, motctrl[iMotorIndex].iMotorPowerPct, motctrl[iMotorIndex].iMotorPower);
+    mylog(LOG_INFO, "CAN: Motor[%d]: Set power to %d -> %d",
+        iMotorIndex, motctrl[iMotorIndex].iMotorPowerSteps, motctrl[iMotorIndex].iMotorPowerRaw);
 }
 
 
@@ -132,6 +228,8 @@ void Can_SetMotorPower(int iMotorIndex, int iPower)
 void Can_TransmitMotorGear(int iMotorIndex)
 {
     // Transmission rate: 100ms
+    if (intf_data[iMotorIndex].socket >= 0)
+    {
         struct can_frame frame;
 
         frame.can_id = 0x18F005D0;
@@ -151,6 +249,7 @@ void Can_TransmitMotorGear(int iMotorIndex)
 
         }
     }
+}
 
 
 /// @brief  Send CAN protocol for motor power for the given motor
@@ -158,13 +257,15 @@ void Can_TransmitMotorGear(int iMotorIndex)
 void Can_TransmitMotorPower(int iMotorIndex)
 {
     // Transmission rate: 50ms
+    if (intf_data[iMotorIndex].socket >= 0)
+    {
         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[1] = motctrl[iMotorIndex].iMotorPowerRaw;   // motor power 0 = 0%, 250 = 100%
         frame.data[2] = 0xFF;
         frame.data[3] = 0xFF;
         frame.data[4] = 0xFF;
@@ -177,3 +278,73 @@ void Can_TransmitMotorPower(int iMotorIndex)
 
         }
     }
+}
+
+
+/// @brief Read data from CAN interface
+/// @param iMotorIndex 
+void Can_ReadData(int iMotorIndex)
+{
+    if (intf_data[iMotorIndex].socket >= 0)
+    {
+        ssize_t nbytes = 0;
+        struct can_frame frame;
+
+        // increment cycle counter
+        IncBusTimeoutCounter(iMotorIndex);
+
+        // read one frame
+        if ((nbytes = read(intf_data[iMotorIndex].socket, &frame, sizeof(frame))) > 0)
+        {
+            canid_t pgn = frame.can_id & 0x00FFFF00;
+
+            switch(pgn)
+            {
+                case 0x00EF6400:    // "repeat data"
+                    break;
+
+                case 0x00F00300:    // PGN 61443 "Electronic Engine Controller 2"
+                    // we have sent this -> ignore
+                    break;
+
+                case 0x00F00500:    // PGN 61445 "Electronic Transmission Controller 2"
+                    // we have sent this -> ignore
+                    break;
+
+                case 0x00FF1300:    // PGN 65299 "Manufacturer PGN"
+                    // here we find the states of the switches
+                    Can_Read_Manu_PGN(iMotorIndex, &frame);
+                    break;
+
+                case 0x00FF1400:    // PGN 65300 "Manufacturer PGN 2"
+                    // here we find the actual power of the motor
+                    Can_Read_Manu_PGN2(iMotorIndex, &frame);
+                    break;
+            }
+        }
+    }
+}
+
+/// @brief Read PGN 65299
+/// @param frame 
+void Can_Read_Manu_PGN(int iMotorIndex, struct can_frame *frame)
+{
+    // we received a valid telegram -> set timeout counter to zero
+    iBusTimeoutCounter = 0;
+
+    // get switch states
+    motctrl[iMotorIndex].nSwitchState = frame->data[4];
+    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
+/// @param frame 
+void Can_Read_Manu_PGN2(int iMotorIndex, struct can_frame *frame)
+{
+    motctrl[iMotorIndex].iActualMotorPowerW = (frame->data[6] << 16) | (frame->data[5] << 8) | frame->data[4];
+    MqttClient_Publish_MotorActualPowerW(iMotorIndex, motctrl[iMotorIndex].iActualMotorPowerW);
+}

can/can_client.h

@@ -5,7 +5,9 @@
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
+#include <syslog.h>
 
+#include <fcntl.h>
 #include <net/if.h>
 #include <sys/ioctl.h>
 #include <sys/socket.h>
@@ -13,24 +15,24 @@
 #include <linux/can.h>
 #include <linux/can/raw.h>
 
-#define MOTOR_COUNT 2
+#define MAX_MOTOR_COUNT 2
 
 // motor gear: 0x7C=reverse, 0x7D=neutral, 0x7E=forward
 #define MOTOR_GEAR_REVERSE 0x7C
 #define MOTOR_GEAR_NEUTRAL 0x7D
 #define MOTOR_GEAR_FORWARD 0x7E
 
-#define MOTOR_PWR_MIN_PCT 5
-#define MOTOR_PWR_MAX_PCT 100
-#define MOTOR_PWR_STEP 14
-
 struct MOTOR_CONTROL_DATA
 {
+    char nDriveConnected;
+    char nDriveReady;
     int iMotorGear;
-    int iMotorPower;
-    int iMotorPowerPct;
+    int iMotorPowerRaw;
+    int iMotorPowerSteps;
+    unsigned char nSwitchState;
+    int iActualMotorPowerW;
 };
-extern struct MOTOR_CONTROL_DATA motctrl[MOTOR_COUNT];
+extern struct MOTOR_CONTROL_DATA motctrl[MAX_MOTOR_COUNT];
 
 struct CAN_INTERFACE_DATA
 {
@@ -49,4 +51,8 @@ void Can_SetMotorPower(int iMotorIndex, int iPower);
 void Can_TransmitMotorGear(int iMotorIndex);
 void Can_TransmitMotorPower(int iMotorIndex);
 
+void Can_ReadData(int iMotorIndex);
+void Can_Read_Manu_PGN(int iMotorIndex, struct can_frame *frame);
+void Can_Read_Manu_PGN2(int iMotorIndex, struct can_frame *frame);
+
 #endif

io/io.c

@@ -1,43 +1,48 @@
 
+#include "main.h"
 #include "io.h"
 #include <can/can_client.h>
+#include <settings/settings.h>
+#include <mqtt/mqtt_client.h>
 
 struct GPIO_KEY_DATA gpioKeyStop;
 struct GPIO_KEY_DATA gpioKeyPwrUp;
 struct GPIO_KEY_DATA gpioKeyPwrDown;
 
+char nInitialized = 0;
+int iPowerSupplyOn = 0;
+
+/// @brief Initialize the io pins
+/// @return 
 int IO_Init()
 {
+    iPowerSupplyOn = 0;
+
     if (wiringPiSetupPinType(WPI_PIN_BCM))
     {
-        printf("IO: Set up wiringPi failed!\n");
+        mylog(LOG_ERR, "IO: Set up wiringPi failed!");
         return 1;
     }
+    nInitialized = 1;
 
-    // IO-Pins für Tasten konfigurieren
+    // config IO-pins for the keys
     SetupKeyPin(&gpioKeyStop, GPIO_KEY_STOP);
     SetupKeyPin(&gpioKeyPwrUp, GPIO_KEY_PWRUP);
     SetupKeyPin(&gpioKeyPwrDown, GPIO_KEY_PWRDOWN);
 
-    // IO-Pins für Ausgänge konfigurieren
+    // config IO-pins for outputs
     SetupOutputPin(GPIO_LED_MOTRUN);
+    SetupOutputPin(GPIO_OUT_PWRON);
 
-    // Einschaltsequenz
-    WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
-    delay(500);
-    WriteOutputPin(GPIO_LED_MOTRUN, LOW);
-    delay(500);
-    WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
-    delay(500);
-    WriteOutputPin(GPIO_LED_MOTRUN, LOW);
-    delay(500);
-
-    printf("IO initialized successfull!\n");
+    mylog(LOG_INFO, "IO: Initialized successfull!");
     
     return 0;
 }
 
 
+/// @brief Setup a pin for a key input
+/// @param pdata 
+/// @param iKeyPin 
 void SetupKeyPin(struct GPIO_KEY_DATA *pdata, int iKeyPin)
 {
     pdata->iKeyPin = iKeyPin;
@@ -49,24 +54,26 @@ void SetupKeyPin(struct GPIO_KEY_DATA *pdata, int iKeyPin)
     pdata->iKeyPressedCycleCounter = 0;
     pdata->iKeyRepeatCycleCounter = 0;
 
-    // Wenn Eingang verwendet wird
-    if (pdata->iKeyPin > 0)
+    if ((pdata->iKeyPin > 0) && nInitialized)
     {
+        mylog(LOG_DEBUG, "IO: Config Pin %d as input", pdata->iKeyPin);
         pinMode(pdata->iKeyPin, INPUT);
         pullUpDnControl(pdata->iKeyPin, PUD_UP);
     }
 }
 
 
+/// @brief Read a key input
+/// @param pdata 
 void ReadKey(struct GPIO_KEY_DATA *pdata)
 {
     if (pdata->iKeyPin > 0)
     {
         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)
             {
                 pdata->nLowCycleCounter--;
@@ -77,14 +84,14 @@ void ReadKey(struct GPIO_KEY_DATA *pdata)
                 pdata->nHighCycleCounter++;
                 if (pdata->nHighCycleCounter >= KEY_RISING_FILTERCYCLES)
                 {
-                    // gewünschte Anzahl Zyklen stabil
+                    // key is stable pressed
                     newval = 1;
                 }
             }
         }
         else
         {
-            // Signal liegt nicht an
+            // key is not pressed
             if (pdata->nHighCycleCounter > 0)
             {
                 pdata->nHighCycleCounter--;
@@ -95,7 +102,7 @@ void ReadKey(struct GPIO_KEY_DATA *pdata)
                 pdata->nLowCycleCounter++;
                 if (pdata->nLowCycleCounter >= KEY_FALLING_FILTERCYCLES)
                 {
-                    // gewünschte Anzahl Zyklen stabil
+                    // key is stable not pressed
                     newval = 0;
                 }
             }
@@ -103,7 +110,7 @@ void ReadKey(struct GPIO_KEY_DATA *pdata)
 
         if (newval && !pdata->iKeyValue)
         {
-            // Taster wurde betätigt
+            // key was pressed -> rising edge
             pdata->iKeyRisingEdge = 1;
             pdata->iKeyValue = newval;
             pdata->iKeyPressedCycleCounter = 0;
@@ -111,32 +118,32 @@ void ReadKey(struct GPIO_KEY_DATA *pdata)
         }
         else if (pdata->iKeyValue && !newval)
         {
-            // Taster wurde losgelassen
+            // key was released -> falling edge
             pdata->iKeyFallingEdge = 1;
             pdata->iKeyValue = newval;
         }
         else
         {
-            // Keine Änderung
+            // no change
             pdata->iKeyRisingEdge = 0;
             pdata->iKeyFallingEdge = 0;
 
             if (pdata->iKeyValue)
             {
-                // Wenn Taste gedrückt ist
+                // when key is pressed
                 if (pdata->iKeyPressedCycleCounter < KEY_START_REPEAT_CYCLECOUNT)
                 {
-                    // Zyklen zählen
+                    // count cycles
                     pdata->iKeyPressedCycleCounter++;
                 }
 
                 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++;
                     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->iKeyRepeatCycleCounter = 0;
                     }
@@ -146,66 +153,88 @@ void ReadKey(struct GPIO_KEY_DATA *pdata)
     }
 }
 
-
+/// @brief Config pin for output
+/// @param iOutPin 
 void SetupOutputPin(int iOutPin)
 {
-    if (iOutPin > 0)
+    if ((iOutPin > 0) && nInitialized)
     {
+        mylog(LOG_DEBUG, "IO: Config Pin %d as output", iOutPin);
         pinMode(iOutPin, OUTPUT);
         digitalWrite(iOutPin, LOW);
     }
 }
 
+/// @brief Write an output pin to HIGH or LOW
+/// @param iOutPin 
+/// @param iValue 
 void WriteOutputPin(int iOutPin, int iValue)
 {
-    if (iOutPin > 0)
+    if ((iOutPin > 0) && nInitialized)
     {
         digitalWrite(iOutPin, iValue);
     }
 }
 
 
+/// @brief look cyclic for the keys
 void IO_DoCyclic()
 {
     ReadKey(&gpioKeyStop);
     ReadKey(&gpioKeyPwrUp);
     ReadKey(&gpioKeyPwrDown);
 
-    if (gpioKeyStop.iKeyValue)
+    if (gpioKeyStop.iKeyValue || atomic_load(&abKeyStop))
     {
-        // Stop-Taste betätigt -> hat Vorrang vor den anderen Tasten
-        if (gpioKeyStop.iKeyRisingEdge)
+        // stop key is pressed
+        if (gpioKeyStop.iKeyRisingEdge || atomic_load(&abKeyStop))
         {
+            mylog(LOG_INFO, "IO: KEY-Stop: Stop motor.");
             Can_SetMotorGear(0, 0);
         }
     }
-    else
-    {
-        if (gpioKeyPwrUp.iKeyRisingEdge)
-        {
-            // Leistung erhöhen
-            if (motctrl[0].iMotorGear == MOTOR_GEAR_NEUTRAL)
-            {
-                Can_SetMotorGear(0, 1);
-                Can_SetMotorPower(0, MOTOR_PWR_MIN_PCT);
-            }
-            else
-            {
-                Can_SetMotorPower(0, motctrl[0].iMotorPowerPct + MOTOR_PWR_STEP);
-            }
-        }
 
-        if (gpioKeyPwrDown.iKeyRisingEdge)
+    if (gpioKeyPwrUp.iKeyRisingEdge || atomic_load(&abKeyPlus))
     {
-            // Leistung verringern
-            if (motctrl[0].iMotorPowerPct > MOTOR_PWR_MIN_PCT)
+        if (motctrl[0].nDriveReady)
         {
-                Can_SetMotorPower(0, motctrl[0].iMotorPowerPct - MOTOR_PWR_STEP);
-            }
-            else
+            // when drive is ready to run: plus key is pressed -> increase power
+            if (motctrl[0].iMotorGear == MOTOR_GEAR_NEUTRAL)
             {
-                Can_SetMotorGear(0, 0);
+                mylog(LOG_INFO, "IO: KEY-Plus: Start motor.");
+                Can_SetMotorGear(0, 1);
+                Can_SetMotorPower(0, 1);
+            }
+            else if (motctrl[0].iMotorPowerSteps < settings.iMotorPwrStepCount)
+            {
+                mylog(LOG_INFO, "IO: KEY-Plus: Increase power.");
+                Can_SetMotorPower(0, motctrl[0].iMotorPowerSteps + 1);
             }
         }
+        else if (settings.iShellySupplyCount > 0)
+        {
+            // when drive is not ready and we have to switch the supply
+            MqttClient_SwitchPowerSupply(1);
+            iPowerSupplyOn = 1;
         }
     }
+
+    if (gpioKeyPwrDown.iKeyRisingEdge || atomic_load(&abKeyMinus))
+    {
+        // minus key is pressed -> decrease power
+        if (motctrl[0].iMotorPowerSteps > 1)
+        {
+            mylog(LOG_INFO, "IO: KEY-Minus: Decrease power.");
+            Can_SetMotorPower(0, motctrl[0].iMotorPowerSteps - 1);
+        }
+        // else
+        // {
+        //     mylog(LOG_INFO, "IO: KEY-Minus: Stop motor.");
+        //     Can_SetMotorGear(0, 0);
+        // }
+    }
+
+    atomic_store(&abKeyPlus, false);
+    atomic_store(&abKeyMinus, false);
+    atomic_store(&abKeyStop, false);
+}

io/io.h

@@ -3,19 +3,21 @@
 
 #include <wiringPi.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 for output "Ignition Key"
 
-#define GPIO_KEY_STOP 26        // GPIO Pin fuer Taster Stop
-#define GPIO_KEY_PWRUP 5        // GPIO Pin fuer Taster Leistung-Erhöhen
-#define GPIO_KEY_PWRDOWN 6      // GPIO Pin fuer Taster Leistung-Verringern 
+#define GPIO_KEY_STOP 26        // GPIO Pin for Key "Stop"
+#define GPIO_KEY_PWRUP 5        // GPIO Pin for Key "Increase Power"
+#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_FALLING_FILTERCYCLES 15         // Filterwert für Eingänge (Zyklen-Zähler)
-#define KEY_START_REPEAT_CYCLECOUNT 50      // Anzahl Zyklen, nach denen Wiederholungen beginnen
-#define KEY_REPEAT_CYCLECOUNT 50            // Anzahl Zyklen, nach den wiederholt wird
+#define KEY_RISING_FILTERCYCLES 5           // filter value for input rising edge
+#define KEY_FALLING_FILTERCYCLES 15         // filter value for input falling edge
+#define KEY_START_REPEAT_CYCLECOUNT 50      // number of cycles when to start repeating key presses
+#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
 {
     int iKeyPin;
@@ -28,6 +30,8 @@ struct GPIO_KEY_DATA
     int iKeyRepeatCycleCounter;
 };
 
+extern int iPowerSupplyOn;
+
 int IO_Init();
 void IO_DoCyclic();
 void SetupKeyPin(struct GPIO_KEY_DATA *pdata, int iKeyPin);

main.c

@@ -3,10 +3,51 @@
 #include <mqtt/mqtt_client.h>
 #include <can/can_client.h>
 #include <io/io.h>
+#include <settings/settings.h>
+#include <termios.h>
 
 // Period info of the realtime task
 struct period_info pinfo;
-int iThreadControl = 0;         // 0: thread is running, <0: thread shall exit, >0 thread has exited
+int iLogToConsole = 1;
+atomic_short asThreadControl = ATOMIC_VAR_INIT(0);      // 0: thread is running, <0: thread shall exit, >0 thread has exited
+
+// values for simulation
+atomic_bool abKeyPlus = ATOMIC_VAR_INIT(false);
+atomic_bool abKeyMinus = ATOMIC_VAR_INIT(false);
+atomic_bool abKeyStop = ATOMIC_VAR_INIT(false);
+
+/// @brief send a log message
+/// @param prio 
+/// @param format 
+/// @param  
+void mylog(int prio, const char *format, ...)
+{
+        if (prio <= settings.iDebugLevel)
+        {
+                va_list args;
+
+                // 1. Initialize the argument list with the last fixed argument
+                va_start(args, format);
+
+                // 2. Transfer to vsyslog (instead of syslog)
+                // vsyslog accepts a va_list
+                vsyslog(prio, format, args);
+
+                // 3. Optional: Output additionally to the console
+                // We have to reinitialize the list because va_list is "consumed."
+                if (iLogToConsole)
+                {
+                        va_end(args);
+                        va_start(args, format);
+                        vfprintf(stderr, format, args);
+                        fprintf(stderr, "\n");
+                }
+
+                // 4. Cleanup
+                va_end(args);
+        }
+}
+
 
 /// @brief Initialize period_info with period_ms for cyclic task
 /// @param period_ms 
@@ -55,6 +96,9 @@ static void do_cyclic_1ms(struct period_info *pinfo)
 {
         uint16_t nCalled = 0;
 
+        // read each cycle CAN data
+        Can_ReadData(0);
+
         if ((pinfo->cyclecounter % 10) == 0)
         {
                 // called every 10ms
@@ -74,7 +118,6 @@ static void do_cyclic_1ms(struct period_info *pinfo)
                 // called every 100ms
                 nCalled |= 0x0010;
                 Can_TransmitMotorGear(0);
-                //printf("%.3f: 100ms-Cycle %ld...\n", clock_gettime_s() - pinfo->fStartTime, pinfo->cyclecounter);
         }
 
         if (((pinfo->cyclecounter + 20) % 100) == 0)
@@ -86,14 +129,9 @@ static void do_cyclic_1ms(struct period_info *pinfo)
 
         if (((pinfo->cyclecounter + 30) % 500) == 0)
         {
-                // called every 250ms
+                // called every 500ms
                 MqttClient_Publisher();
         }
-
-        if (nCalled > 0)
-        {
-                //printf("%.3f: Called 0x%.4X at cycle %ld...\n", clock_gettime_s() - pinfo->fStartTime, nCalled, pinfo->cyclecounter);
-        }
 }
  
 
@@ -102,67 +140,85 @@ static void do_cyclic_1ms(struct period_info *pinfo)
 /// @return 
 void *thread_func(void *data)
 {
+        // Initialize IO Ports
+        if (IO_Init())
+        {
+                mylog(LOG_ERR, "IO_Init() failed!");
+                return NULL;
+        }
+
         // Open CAN interface first motor
         if (Can_OpenInterface(0, "can0"))
         {
-                printf("Can_OpenInterface() failed!\n");
+                mylog(LOG_ERR, "Can_OpenInterface() failed!");
                 return NULL;
         }
 
         // Connect to mqtt broker
-        while (MqttClient_Connect() && (iThreadControl == 0))
+        while (MqttClient_Connect() && atomic_load(&asThreadControl) == 0)
         {
-                printf("MqttClient_Connect() failed!\n");
+                mylog(LOG_ERR, "MqttClient_Connect() failed!");
                 sleep(10);
         }
 
-        // Initialize IO Ports
-        if (IO_Init())
-        {
-                printf("IO_Init() failed!\n");
-                return NULL;
-        }
-
         // initialize cyclic task
         periodic_task_init(1, &pinfo);
  
+        // Ignition on
+        WriteOutputPin(GPIO_OUT_PWRON, HIGH);
+
         // cyclic call of do_cyclic_1ms()
-        while (iThreadControl == 0)
+        while (atomic_load(&asThreadControl) == 0)
         {
                 pinfo.cyclecounter++;
-                if (pinfo.cyclecounter >= 86400000)
+                if (pinfo.cyclecounter > CYCLE_COUNTER_MAX)
                 {
                         // Reset cycle counter every 24h
-                        pinfo.cyclecounter = 0;
+                        pinfo.cyclecounter = 1;
                 } 
                 do_cyclic_1ms(&pinfo);
                 wait_rest_of_period(&pinfo);
         }
 
+        // Ignition off
+        WriteOutputPin(GPIO_OUT_PWRON, LOW);
+
         // Disconnect from mqtt broker
         MqttClient_Close();
 
         // Close CAN interface
         Can_CloseInterface(0);
         
-        iThreadControl = 1;
+        // signal thread has finnished
+        atomic_store(&asThreadControl, 1);
         return NULL;
 }
 
 
+// Funktion, um das Terminal in den "Raw Mode" zu versetzen
+void set_conio_terminal_mode() 
+{
+    struct termios new_termios;
+    tcgetattr(0, &new_termios);
+    new_termios.c_lflag &= ~ICANON; // Deaktiviert den zeilenweisen Modus
+    new_termios.c_lflag &= ~ECHO;   // Verhindert, dass die Taste angezeigt wird
+    tcsetattr(0, TCSANOW, &new_termios);
+}
+
+
 /// @brief catch signals and set flag to terminate for the realtime thread
 /// @param signo 
 void sig_handler(int signo)
 {
         if ((signo == SIGINT) || (signo == SIGTERM))
         {
-                printf("Received signal %d\n", signo);
-                iThreadControl = -1;    // signal realtime thread to exit
+                mylog(LOG_INFO, "Received signal %d", signo);
+                atomic_store(&asThreadControl, -1);     // signal realtime thread to exit
         }
 }
 
 
-/// @brief Hauptfunktion Echtzeit-Task erstellen und starten
+/// @brief Main function, create and start realtime task
 /// @param argc 
 /// @param argv 
 /// @return 
@@ -173,22 +229,39 @@ int main(int argc, char* argv[])
         pthread_t thread;
         int ret;
 
+        // First we have to get the default values of our settings
+        Settings_InitDefaultValues();
+
+        openlog("CanRtDriver", LOG_PID | LOG_CONS, LOG_DAEMON);
+        mylog(LOG_INFO, "Service started. PID: %d", getpid());
+
+        // Read the settings file after opening the log
+        Settings_ReadConfFile();
+
+        if (settings.iCanSimu)
+        {
+                // in simulation mode switch terminal to raw mode
+                set_conio_terminal_mode();
+        }
+        else
+        {
                 // catch signals
                 if (signal(SIGTERM, sig_handler) == SIG_ERR)
                 {
-                printf("Can't catch SIGTERM\n");
+                        mylog(LOG_ERR, "Can't catch SIGTERM");
                         exit(-1);
                 }
                 if (signal(SIGINT, sig_handler) == SIG_ERR)
                 {
-                printf("Can't catch SIGINT\n");
+                        mylog(LOG_ERR, "Can't catch SIGINT");
                         exit(-2);
                 }
+        }
 
         /* Lock memory */
         if(mlockall(MCL_CURRENT | MCL_FUTURE) == -1) 
         {
-                printf("mlockall failed: %m\n");
+                mylog(LOG_ERR, "mlockall failed: %m");
                 exit(-3);
         }
 
@@ -196,7 +269,7 @@ int main(int argc, char* argv[])
         ret = pthread_attr_init(&attr);
         if (ret) 
         {
-                printf("init pthread attributes failed\n");
+                mylog(LOG_ERR, "init pthread attributes failed");
                 goto out;
         }
 
@@ -204,7 +277,7 @@ int main(int argc, char* argv[])
         ret = pthread_attr_setstacksize(&attr, PTHREAD_STACK_MIN);
         if (ret) 
         {
-            printf("pthread setstacksize failed\n");
+            mylog(LOG_ERR, "pthread setstacksize failed");
             goto out;
         }
 
@@ -212,42 +285,87 @@ int main(int argc, char* argv[])
         ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
         if (ret) 
         {
-                printf("pthread setschedpolicy failed\n");
+                mylog(LOG_ERR, "pthread setschedpolicy failed");
                 goto out;
         }
         param.sched_priority = 99;  // Priority between 1 (low) and 99() high)
         ret = pthread_attr_setschedparam(&attr, &param);
         if (ret) 
         {
-                printf("pthread setschedparam failed\n");
+                mylog(LOG_ERR, "pthread setschedparam failed");
                 goto out;
         }
         /* Use scheduling parameters of attr */
         ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
         if (ret) 
         {
-                printf("pthread setinheritsched failed\n");
+                mylog(LOG_ERR, "pthread setinheritsched failed");
                 goto out;
         }
 
         
         /* Create a pthread with specified attributes */
-        iThreadControl = 0;
+        atomic_store(&asThreadControl, 0);
         ret = pthread_create(&thread, &attr, thread_func, NULL);
         if (ret) 
         {
-                printf("create pthread failed\n");
+                mylog(LOG_ERR, "create pthread failed");
                 goto out;
         }
 
+        if (settings.iCanSimu)
+        {
+                // in simulation mode we use keys to control the motors
+                mylog(LOG_INFO, "*** CanRtDriver in Simu-Mode ***");
+                while (1)
+                {
+                        char ch = getchar();
+                        mylog(LOG_INFO, "SIMU: Taste gedrückt: %c", ch);
+
+                        if (ch == '+')
+                        {
+                                atomic_store(&abKeyPlus, true);
+                        }
+                        else if (ch == '-')
+                        {
+                                atomic_store(&abKeyMinus, true);
+                        }
+                        else if (ch == 's')
+                        {
+                                atomic_store(&abKeyStop, true);
+                        }
+                        else if (ch == 'p')
+                        {
+                                if (iPowerSupplyOn)
+                                {
+                                        MqttClient_SwitchPowerSupply(0);
+                                        iPowerSupplyOn = 0;
+                                }
+                                else
+                                {
+                                        MqttClient_SwitchPowerSupply(1);
+                                        iPowerSupplyOn = 1;
+                                }
+                        }
+                        else if ( ch == 'q')
+                        {
+                                atomic_store(&asThreadControl, -1);     // signal realtime thread to exit
+                                break;
+                        }
+                }
+        }
+
         // join the thread and wait for it to exit
         ret = pthread_join(thread, NULL);
         if (ret)
         {
-                printf("faild to join thread!\n");
+                mylog(LOG_ERR, "faild to join thread!");
         }
 
 out:
+        mylog(LOG_INFO, "Service quit.");
+        closelog();
+
         return ret;
 }
 

main.h

@@ -11,8 +11,15 @@
 #include <time.h>
 #include <errno.h>
 #include <signal.h>
+#include <syslog.h>
+#include <stdarg.h>
+#include <stdatomic.h>
+#include <stdbool.h>
 
 
+//#define CYCLE_COUNTER_MAX 86400000
+#define CYCLE_COUNTER_MAX 600000        // for testing only
+
 struct period_info 
 {
         struct timespec next_period;
@@ -21,6 +28,11 @@ struct period_info
         float fStartTime; 
 };
 
+extern atomic_bool abKeyPlus;
+extern atomic_bool abKeyMinus;
+extern atomic_bool abKeyStop;
+
 extern struct period_info pinfo;
+extern void mylog(int prio, const char *format, ...);
 
 #endif

makefile

@@ -7,13 +7,14 @@ CFLAGS = -Wextra -Wall -std=gnu99 -I. -Wno-unused-parameter -Wno-unused-variable
 MQTT_SOURCES = mqtt/mqtt_client.c
 CAN_SOURCES = can/can_client.c
 IO_SOURCES = io/io.c
+SETTINGS_SOURCES = settings/settings.c
 PROG = bin/CanRtDriver
 BINDIR = bin
 
 
 all: $(BINDIR) $(PROG) 
 
-bin/CanRtDriver: main.c $(CAN_SOURCES) $(MQTT_SOURCES) $(IO_SOURCES)
+bin/CanRtDriver: main.c $(CAN_SOURCES) $(MQTT_SOURCES) $(IO_SOURCES) $(SETTINGS_SOURCES)
 	$(CC) $(CFLAGS) $^ -lpthread -lmosquitto -lwiringPi -o $@
 
 $(BINDIR):

mqtt/mqtt_client.c

@@ -5,24 +5,33 @@
     https://mosquitto.org/api/files/mosquitto-h.html
 */
 
-#include <main.h>
+#include "main.h"
 #include <mqtt/mqtt_client.h>
 #include <can/can_client.h>
+#include <settings/settings.h>
 #include <string.h>
 
 #include <mosquitto.h>
 
 
-const char* mqtt_topic_status_cyclecounter = "Pool/Status/CycleCounter";
+// Topics to subscribe
+const char* mqtt_topic_motor_gear_request = "Pool/Motor_Gear_Request";
+const char* mqtt_topic_motor_power_request = "Pool/Motor_Power_Request";
 
+// Topics to publish
+const char* mqtt_topic_status_cyclecounter = "Pool/Status/CycleCounter";
 const char* mqtt_topic_motor1_gear = "Pool/Motor1/Gear";
-int iMqttMotor1Gear = 0;
 const char* mqtt_topic_motor1_power = "Pool/Motor1/Power";
-int iMqttMotor1Power = 0;
 const char* mqtt_topic_motor2_gear = "Pool/Motor2/Gear";
-int iMqttMotor2Gear = 0;
 const char* mqtt_topic_motor2_power = "Pool/Motor2/Power";
-int iMqttMotor2Power = 0;
+const char* mqtt_topic_motor1_switchstate = "Pool/Motor1/SwitchState";
+unsigned char nMotor1SwitchState = 255;
+const char* mqtt_topic_motor2_switchstate = "Pool/Motor1/SwitchState";
+unsigned char nMotor2SwitchState = 255;
+const char* mqtt_topic_motor1_actualpowerw = "Pool/Motor1/ActualPowerW";
+int iMqttMotor1ActualPowerW = -1;
+const char* mqtt_topic_motor2_actualpowerw = "Pool/Motor1/ActualPowerW";
+int iMqttMotor2ActualPowerW = -1;
 
 const char* mqtt_broker_addr = "127.0.0.1";
 const int mqtt_broker_port = 1883;
@@ -30,83 +39,108 @@ struct mosquitto *mosq;     /**< Libmosquito MQTT client instance. */
 
 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)
 {
     char* topic_value = (char *)malloc(message->payloadlen + 1);
     memcpy(topic_value, message->payload, message->payloadlen);
     topic_value[message->payloadlen] = '\0';
     
-    if (strcmp(message->topic, mqtt_topic_motor1_gear) == 0)
+    if (strcmp(message->topic, mqtt_topic_motor_gear_request) == 0)
     {
-        int val = 9999;
+        int val = 123456789;
         if (sscanf(topic_value, "%d", &val))
         {
-            printf("%ld: Received value for mqtt_topic_motor1_gear: %d\n", pinfo.cyclecounter, val);
-            iMqttMotor1Gear = val;
-            Can_SetMotorGear(0, val);
+            mylog(LOG_INFO, "MQTT: Received value for mqtt_topic_motor_gear_request: %d", val);
+            if (val == 123456789)
+            {
+                val = 0;
+            }
+
+            for (int i=0; i<settings.iMotorCount; i++)
+            {
+                Can_SetMotorGear(i, val);
+            }
         }
         else
         {
-            printf("%ld: Received mqtt_topic_motor1_gear: %s\n", pinfo.cyclecounter, topic_value);
+            mylog(LOG_WARNING, "MQTT: Received mqtt_topic_motor_gear_request: %s", topic_value);
         }
     }
-    else if (strcmp(message->topic, mqtt_topic_motor1_power) == 0)
+    else if (strcmp(message->topic, mqtt_topic_motor_power_request) == 0)
     {
-        int val = 9999;
+        int val = 123456789;
         if (sscanf(topic_value, "%d", &val))
         {
-            printf("%ld: Received value for mqtt_topic_motor1_power: %d\n", pinfo.cyclecounter, val);
-            iMqttMotor1Power = val;
-            Can_SetMotorPower(0, val);
-        }
-        else
+            mylog(LOG_INFO, "MQTT: Received value for mqtt_topic_motor_power_request: %d", val);
+            if (val == 123456789)
             {
-            printf("%ld: Received mqtt_topic_motor1_power: %s\n", pinfo.cyclecounter, topic_value);
+                val = 0;
             }
-    }
-    else if (strcmp(message->topic, mqtt_topic_motor2_gear) == 0)
+            
+            for (int i=0; i<settings.iMotorCount; i++)
             {
-        int val = 9999;
-        if (sscanf(topic_value, "%d", &val))
-        {
-            printf("%ld: Received value for mqtt_topic_motor2_gear: %d\n", pinfo.cyclecounter, val);
-            iMqttMotor2Gear = val;
-            Can_SetMotorGear(1, val);
-        }
-        else
-        {
-            printf("%ld: Received mqtt_topic_motor2_gear: %s\n", pinfo.cyclecounter, topic_value);
-        }
-    }
-    else if (strcmp(message->topic, mqtt_topic_motor2_power) == 0)
-    {
-        int val = 9999;
-        if (sscanf(topic_value, "%d", &val))
-        {
-            printf("%ld: Received value for mqtt_topic_motor2_power: %d\n", pinfo.cyclecounter, val);
-            iMqttMotor2Power = val;
-            Can_SetMotorPower(1, val);
-        }
-        else
-        {
-            printf("%ld: Received mqtt_topic_motor2_power: %s\n", pinfo.cyclecounter, topic_value);
+                Can_SetMotorPower(i, val);
             }
         }
         else
         {
-        printf("%ld: Received publish('%s'): %s\n", pinfo.cyclecounter, message->topic, topic_value);
+            mylog(LOG_WARNING, "MQTT: Received mqtt_topic_motor_gear_request: %s", topic_value);
+        }
+    }
+    else
+    {
+        mylog(LOG_WARNING, "MQTT: Received publish('%s'): %s", message->topic, topic_value);
     }
 
     free(topic_value);
 }
 
+/// @brief callback function after connection
+void my_connect_callback(struct mosquitto *mosq, void *obj, int rc)
+{
+    if (rc == 0)
+    {
+        if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor_gear_request, 0) != MOSQ_ERR_SUCCESS)
+        {
+            mylog(LOG_ERR, "MQTT: mosquitto_subscribe(mqtt_topic_motor_gear_request) failed!");
+        }
+        if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor_power_request, 0) != MOSQ_ERR_SUCCESS)
+        {
+            mylog(LOG_ERR, "MQTT: mosquitto_subscribe(mqtt_topic_motor_power_request) failed!");
+        }
 
+        char message[10];
+        snprintf(message, sizeof(message), "0");
+        if (mosquitto_publish(mosq, NULL, mqtt_topic_motor_gear_request, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS)
+        {
+            mylog(LOG_ERR, "MQTT: mosquitto_publish(mqtt_topic_motor_gear_request) failed!");
+        }
+        if (mosquitto_publish(mosq, NULL, mqtt_topic_motor_power_request, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS)
+        {
+            mylog(LOG_ERR, "MQTT: mosquitto_publish(mqtt_topic_motor_power_request) failed!");
+        }
+
+        for (int i=0; i<settings.iMotorCount; i++)
+        {
+            MqttClient_Publish_MotorSwitchState(i, 0);
+            MqttClient_Publish_MotorActualPowerW(i, 0);
+        }
+    }
+}
+
+/// @brief connect to mqtt broker
+/// @return 
 int MqttClient_Connect()
 {
     int 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
     mosquitto_lib_init();
@@ -115,18 +149,19 @@ int MqttClient_Connect()
     mosq = mosquitto_new(NULL, true, NULL);
     if (mosq == NULL)
     {
-        printf("Failed to create mosquitto client!/n");
+        mylog(LOG_ERR, "MQTT: Failed to create mosquitto client!/n");
         iHadConnectError++;
         return 1;
     }
 
     // Define a function which will be called by libmosquitto client every time when there is a new MQTT message
     mosquitto_message_callback_set(mosq, my_message_callback);
+    mosquitto_connect_callback_set(mosq, my_connect_callback);
 
     // Connect to MQTT broker
     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();
         iHadConnectError++;
         return 2;
@@ -136,92 +171,34 @@ int MqttClient_Connect()
     {
         /* Wenn wir Verbindungsfehler hatten, dann befinden wir uns wohl in Boot-Prozess und der Mosquitto ist
            gerade erst gestartet. Wir müssen hier etwas warten, sonst funktioniert das Subscriben nicht */
-        sleep(10);
+        sleep(500);
     }
 
-    // publish all topics we want to subscribe
-    char message[10];
-    snprintf(message, sizeof(message), "0");
-    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");
-        MqttClient_Close();
-        iHadConnectError++;
-        return 10;
-    }
-    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");
-        MqttClient_Close();
-        iHadConnectError++;
-        return 11;
-    }
-    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");
-        MqttClient_Close();
-        iHadConnectError++;
-        return 12;
-    }
-    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");
-        MqttClient_Close();
-        iHadConnectError++;
-        return 13;
-    }
 
-    // subscribe all needed topics
-    if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor1_gear, 0) != MOSQ_ERR_SUCCESS)
-    {
-        printf("mosquitto_subscribe(mqtt_topic_motor1_gear) fehlgeschlagen!\n");
-        MqttClient_Close();
-        iHadConnectError++;
-        return 20;
-    }
-    if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor1_power, 0) != MOSQ_ERR_SUCCESS)
-    {
-        printf("mosquitto_subscribe(mqtt_topic_motor1_power) fehlgeschlagen!\n");
-        MqttClient_Close();
-        iHadConnectError++;
-        return 21;
-    }
-    if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor2_gear, 0) != MOSQ_ERR_SUCCESS)
-    {
-        printf("mosquitto_subscribe(mqtt_topic_motor2_gear) fehlgeschlagen!\n");
-        MqttClient_Close();
-        iHadConnectError++;
-        return 22;
-    }
-    if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor2_power, 0) != MOSQ_ERR_SUCCESS)
-    {
-        printf("mosquitto_subscribe(mqtt_topic_motor2_power) fehlgeschlagen!\n");
-        MqttClient_Close();
-        iHadConnectError++;
-        return 23;
-    }
-
-    printf("MQTT: Connected successfull!\n");
+    mylog(LOG_INFO, "MQTT: Connected successfull after %d errors!", iHadConnectError);
     return 0;
 }
 
 
+/// @brief Disconnect from mqtt broker
 void MqttClient_Close()
 {
     //Clean up/destroy objects created by libmosquitto
     mosquitto_destroy(mosq);
     mosquitto_lib_cleanup();
 
-    printf("MQTT: Disconnected!\n");
+    mylog(LOG_INFO, "MQTT: Disconnected!");
 }
 
 
+/// @brief Has to be called cyclic, does the work
 void MqttClient_Refresher()
 {
     mosquitto_loop(mosq, 0, 1);
 }
 
 
+/// @brief publish the cycle counter as a sign of life
 void MqttClient_Publisher()
 {
     char message[100];
@@ -231,51 +208,110 @@ void MqttClient_Publisher()
 }
 
 
+/// @brief Publish the actual motor gear
+/// @param iMotorIndex 
+/// @param iGear 
 void MqttClient_Publish_MotorGear(int iMotorIndex, int iGear)
 {
     if (iMotorIndex == 0)
     {
-        if (iGear != iMqttMotor1Gear)
-        {
-            iMqttMotor1Gear = iGear;
         char message[100];
         snprintf(message, sizeof(message), "%d", iGear);
         mosquitto_publish(mosq, NULL, mqtt_topic_motor1_gear, strlen(message), message, 0, false);
     }
-    }
     else if (iMotorIndex == 1)
     {
-        if (iGear != iMqttMotor2Gear)
-        {
-            iMqttMotor2Gear = iGear;
         char message[100];
         snprintf(message, sizeof(message), "%d", iGear);
         mosquitto_publish(mosq, NULL, mqtt_topic_motor2_gear, strlen(message), message, 0, false);
     }
 }
-}
 
 
+/// @brief Publish the requested motor power
+/// @param iMotorIndex 
+/// @param iPower 
 void MqttClient_Publish_MotorPower(int iMotorIndex, int iPower)
 {
     if (iMotorIndex == 0)
     {
-        if (iPower != iMqttMotor1Power)
-        {
-            iMqttMotor1Power = iPower;
         char message[100];
         snprintf(message, sizeof(message), "%d", iPower);
         mosquitto_publish(mosq, NULL, mqtt_topic_motor1_power, strlen(message), message, 0, false);
     }
-    }
     else if (iMotorIndex == 1)
     {
-        if (iPower != iMqttMotor2Power)
-        {
-            iMqttMotor2Power = iPower;
         char message[100];
         snprintf(message, sizeof(message), "%d", iPower);
         mosquitto_publish(mosq, NULL, mqtt_topic_motor2_power, strlen(message), message, 0, false);
     }
 }
+
+
+/// @brief Publish the actual switches states
+/// @param iMotorIndex 
+/// @param nSwitchState 
+void MqttClient_Publish_MotorSwitchState(int iMotorIndex, unsigned char nSwitchState)
+{
+    if (iMotorIndex == 0)
+    {
+        if (nSwitchState != nMotor1SwitchState)
+        {
+            nMotor1SwitchState = nSwitchState;
+            char message[100];
+            snprintf(message, sizeof(message), "%2X", nSwitchState);
+            mosquitto_publish(mosq, NULL, mqtt_topic_motor1_switchstate, strlen(message), message, 0, false);
+        }
+    }
+    else if (iMotorIndex == 1)
+    {
+        if (nSwitchState != nMotor2SwitchState)
+        {
+            nMotor2SwitchState = nSwitchState;
+            char message[100];
+            snprintf(message, sizeof(message), "%2X", nSwitchState);
+            mosquitto_publish(mosq, NULL, mqtt_topic_motor2_switchstate, strlen(message), message, 0, false);
+        }
+    }
+}
+
+
+/// @brief Publish the actual real motor power
+/// @param iMotorIndex 
+/// @param iMotorPowerW 
+void MqttClient_Publish_MotorActualPowerW(int iMotorIndex, int iMotorPowerW)
+{
+    if (iMotorIndex == 0)
+    {
+        if (iMotorPowerW != iMqttMotor1ActualPowerW)
+        {
+            iMqttMotor1ActualPowerW = iMotorPowerW;
+            char message[100];
+            snprintf(message, sizeof(message), "%d", iMotorPowerW);
+            mosquitto_publish(mosq, NULL, mqtt_topic_motor1_actualpowerw, strlen(message), message, 0, false);
+        }
+    }
+    else if (iMotorIndex == 1)
+    {
+        if (iMotorPowerW != iMqttMotor2ActualPowerW)
+        {
+            iMqttMotor2ActualPowerW = iMotorPowerW;
+            char message[100];
+            snprintf(message, sizeof(message), "%d", iMotorPowerW);
+            mosquitto_publish(mosq, NULL, mqtt_topic_motor2_actualpowerw, strlen(message), message, 0, false);
+        }
+    }
+}
+
+
+void MqttClient_SwitchPowerSupply(int on)
+{
+    char *cmd = on ? "on" : "off";
+
+    for (int i=0; i<settings.iShellySupplyCount; i++)
+    {
+        mosquitto_publish(mosq, NULL, settings.sShellySupplyTopic[i], strlen(cmd), cmd, 0, false);
+    }
+
+    mylog(LOG_INFO, "MQTT: Switch supply-shellies %s", cmd);
 }

mqtt/mqtt_client.h

@@ -1,7 +1,7 @@
 #if !defined(__MQTT_CLIENT_H__)
 #define __MQTT_CLIENT_H__
 
-#define USE_MOSQUITTO_LIB
+//#define USE_MOSQUITTO_LIB
 
 int MqttClient_Connect();
 void MqttClient_Close();
@@ -9,5 +9,8 @@ void MqttClient_Refresher();
 void MqttClient_Publisher();
 void MqttClient_Publish_MotorGear(int iMotorIndex, int iGear);
 void MqttClient_Publish_MotorPower(int iMotorIndex, int iPower);
+void MqttClient_Publish_MotorSwitchState(int iMotorIndex, unsigned char nSwitchState);
+void MqttClient_Publish_MotorActualPowerW(int iMotorIndex, int iMotorPowerW);
+void MqttClient_SwitchPowerSupply(int on);
 
 #endif

scripts/CanRtDriver.service

@@ -2,16 +2,27 @@
 ## Datei gehört in /etc/systemd/system
 ## aktivieren dann mit: 
 ## - sudo systemctl daemon-reload
-## - sudo systemctl enable CanRtDriver.service
-## - sudo systemctl start CanRtDriver.service
+## - sudo systemctl enable CanRtDriver.service NICHT AUSFÜHREN!!!
+## - sudo systemctl start CanRtDriver.service NICHT AUSFÜHREN!!!
+##   => Service wird über ./scripts/can_link_up.sh gestartet
+##
+## Device Unit ermitteln mit: systemctl --type=device | grep can0
+## Ggf. unter 'Requires', 'After' und 'BindsTo' anpassen!
 
 [Unit]
 Description=CAN-Bus Treiber für Gegenstromanlage
-After=network.target
+#After=network.target
+# Stellt sicher, dass die CAN-Device Unit gestartet wird
+Requires=sys-subsystem-net-devices-can0.device
+# Stellt die Reihenfolge sicher (Start nach der Device Unit)
+After=network-online.target sys-subsystem-net-devices-can0.device
+# Optional: Stoppt den Dienst, wenn das Interface offline geht
+BindsTo=sys-subsystem-net-devices-can0.device
 
 [Service]
+# Skript, das wartet, bis das Interface "UP" ist (ersetzen Sie can0 durch Ihren Interface-Namen)
+#ExecStartPre=/bin/sh -c 'while ! ip link show can0 | grep -q "state UP"; do sleep 1; done'
 ExecStart=/home/veltmann/Documents/CanRtDriver/bin/CanRtDriver
-# Oder: ExecStart=/usr/bin/python3 /home/pi/meinprogramm.py
 WorkingDirectory=/home/veltmann/Documents/CanRtDriver/bin
 StandardOutput=inherit
 StandardError=inherit

scripts/can_link_up.sh

@@ -2,6 +2,10 @@
 
 ip link set can0 up type can bitrate 250000
 ip link set can1 up type can bitrate 250000
+sleep 5
 ifconfig can0 txqueuelen 65536
 ifconfig can1 txqueuelen 65536
+sleep 10
+
+systemctl start CanRtDriver
 

scripts/dump_power.sh

@@ -0,0 +1,2 @@
+candump can0,18FF1416:1FFFFFFF
+

scripts/dump_switch.sh

@@ -0,0 +1,2 @@
+candump can0,18FF1316:1FFFFFFF
+

scripts/git_commit_push.sh

@@ -0,0 +1,6 @@
+#!/bin/bash
+
+git add --all
+git commit -am "$1"
+git push
+

scripts/git_undo.sh

@@ -0,0 +1,4 @@
+#!/bin/bash
+
+git restore .
+

scripts/git_update.sh

@@ -0,0 +1,5 @@
+#!/bin/bash
+
+git fetch
+git pull
+

settings/settings.c

@@ -0,0 +1,168 @@
+
+#include <main.h>
+#include <settings/settings.h>
+#include <ctype.h>
+
+struct APP_SETTINGS settings;
+
+void Settings_InitDefaultValues()
+{
+// #define        LOG_EMERG        0        /* system is unusable */
+// #define        LOG_ALERT        1        /* action must be taken immediately */
+// #define        LOG_CRIT         2        /* critical conditions */
+// #define        LOG_ERR          3        /* error conditions */
+// #define        LOG_WARNING      4        /* warning conditions */
+// #define        LOG_NOTICE       5        /* normal but significant condition */
+// #define        LOG_INFO         6        /* informational */
+// #define        LOG_DEBUG        7        /* debug-level messages */
+    settings.iDebugLevel = LOG_INFO;
+    settings.iCanSimu = 1;
+    
+    settings.iMotorCount = 1;
+    settings.iMotorPwrMinRaw = 38;
+    settings.iMotorPwrMaxRaw = 250;
+    settings.iMotorPwrStepCount = 7;
+
+    settings.iShellySupplyCount = 0;
+    for (int i=0; i<MAX_SHELLIES_COUNT; i++)
+    {
+        settings.sShellySupplyTopic[i][0] = '\0';
+    }
+
+    // Get path of the executable itself
+    ssize_t length = readlink("/proc/self/exe", settings.sExePath, sizeof(settings.sExePath) - 1);
+    if (length >= 0)
+    {
+        settings.sExePath[length] = '\0';
+        mylog(LOG_INFO, "SETTINGS: Executable path: %s", settings.sExePath);
+    }
+    else
+    {
+        settings.sExePath[0] = '\0';
+        mylog(LOG_ERR, "SETTINGS: Executable path not found!");
+    }
+}
+
+
+
+char *trim_str(const char *s) 
+{
+    // Führende Leerzeichen finden
+    while (isspace((unsigned char)*s)) 
+        s++;
+
+    // Falls der String leer ist
+    if (*s == 0) 
+        return strdup("");
+
+    // Letztes Zeichen finden
+    const char *end = s + strlen(s) - 1;
+    while (end > s && isspace((unsigned char)*end)) 
+        end--;
+
+    // Länge des neuen Strings berechnen
+    size_t len = (end - s) + 1;
+
+    // Speicher reservieren (+1 für das Null-Byte)
+    char *new_str = malloc(len + 1);
+    if (new_str) 
+    {
+        memcpy(new_str, s, len);
+        new_str[len] = '\0';
+    }
+
+    return new_str;
+}
+
+void Settings_ReadConfFile() 
+{
+    
+    //const char *filename = "/etc/CanRtDriver.conf";
+
+    char filename[MAX_PATH + 50];
+    sprintf(filename, "/etc/CanRtDriver.conf");
+    FILE *file = fopen(filename, "r");
+    if (file == NULL) 
+    {
+        mylog(LOG_INFO, "SETTINGS: File %s noch found", filename);
+        sprintf(filename, "%s.conf", settings.sExePath);
+        file = fopen(filename, "r");
+        if (file == NULL)
+        {
+            mylog(LOG_ERR, "SETTINGS: No conf file found!");
+            return;
+        }
+    }
+
+    mylog(LOG_INFO, "SETTINGS: Reading %s", filename);
+
+    char line[MAX_LINE_LENGTH];
+    while (fgets(line, sizeof(line), file)) 
+    {
+        // 1. Ignore comments and empty lines
+        if (line[0] == '#' || line[0] == '\n' || line[0] == '\r') 
+        {
+            continue;
+        }
+
+        // 2. Remove CRLF at end of line
+        line[strcspn(line, "\r\n")] = 0;
+
+        // 3. Split line to key and value
+        char *key = trim_str(strtok(line, "="));
+        char *value = trim_str(strtok(NULL, "="));
+
+        if ((key != NULL) && (value != NULL)) 
+        {
+            if (strcmp(key, "DebugLevel") == 0)
+            {
+                // how many log messages we want to see
+                settings.iDebugLevel = atoi(value);
+                mylog(LOG_DEBUG, "SETTINGS: %s = %d", key, settings.iDebugLevel);
+            }
+            else if (strcmp(key, "CanSimu") == 0)
+            {
+                // shall we do simulation
+                settings.iCanSimu = atoi(value);
+                mylog(LOG_DEBUG, "SETTINGS: %s = %d", key, settings.iCanSimu);
+            }
+            else if (strcmp(key, "MotorPowerMinRaw") == 0)
+            {
+                // Minimum power value for the motors
+                settings.iMotorPwrMinRaw = atoi(value);
+                mylog(LOG_DEBUG, "SETTINGS: %s = %d", key, settings.iMotorPwrMinRaw);
+            }
+            else if (strcmp(key, "MotorPowerMaxRaw") == 0)
+            {
+                // Maximum power value for the motors
+                settings.iMotorPwrMaxRaw = atoi(value);
+                mylog(LOG_DEBUG, "SETTINGS: %s = %d", key, settings.iMotorPwrMaxRaw);
+            }
+            else if (strcmp(key, "MotorPowerStepCount") == 0)
+            {
+                // How many steps do we want to have switching the power
+                settings.iMotorPwrStepCount = atoi(value);
+                mylog(LOG_DEBUG, "SETTINGS: %s = %d", key, settings.iMotorPwrStepCount);
+            }
+            else if (strcmp(key, "SupplyShellyMqttTopic") == 0)
+            {
+                // MQTT topic for switching power supply
+                if (settings.iShellySupplyCount < MAX_SHELLIES_COUNT)
+                {
+                    strcpy(settings.sShellySupplyTopic[settings.iShellySupplyCount], value);
+                    settings.iShellySupplyCount++;
+                }
+                else
+                {
+                    mylog(LOG_WARNING, "SETTINGS: Too many SupplyShellyMqttTopic!");
+                }
+            }
+            else
+            {
+                mylog(LOG_WARNING, "SETTING: Unknown key: %s", key);
+            }
+        }
+    }
+
+    fclose(file);
+}

settings/settings.h

@@ -0,0 +1,33 @@
+#if !defined(__SETTINGS_H__)
+#define __SETTINGS_H__
+
+#include <stdio.h>
+#include <unistd.h>
+#include <limits.h>
+#include <stdlib.h>
+#include <string.h>
+
+#define MAX_LINE_LENGTH 256
+#define MAX_PATH 256
+#define MAX_SHELLIES_COUNT 5
+
+struct APP_SETTINGS
+{
+    int iDebugLevel;            // Level of debug messages
+    char sExePath[MAX_PATH];    // Path of the executable
+    int iCanSimu;               // Simulate CAN if 1
+
+    int iMotorCount;            // Number of used motors (1 or 2)
+    int iMotorPwrMinRaw;        // Minimum power value for motor (raw value)
+    int iMotorPwrMaxRaw;        // Maximum power value for motor (raw value)
+    int iMotorPwrStepCount;     // Number of power steps
+
+    int iShellySupplyCount;     // How many Shellies we have to switch the power supply
+    char sShellySupplyTopic[MAX_SHELLIES_COUNT][MAX_PATH];
+};
+extern struct APP_SETTINGS settings;
+
+void Settings_InitDefaultValues();
+void Settings_ReadConfFile();
+
+#endif