MQTT-Handling optimiert

can/can_client.c

@@ -55,7 +55,7 @@ int Can_OpenInterface(int iMotorIndex, const char * ifacename)
 
     if (settings.iCanSimu)
     {
-        mylog(LOG_INFO, "CAN: Using simulation mode (motor %d).", iMotorIndex);
+        mylog(LOG_WARNING, "CAN: Using simulation mode (motor %d).", iMotorIndex);
         intf_data[iMotorIndex].socket = -1;
         motctrl[iMotorIndex].nSwitchState = 0xF5;
         motctrl[iMotorIndex].nDriveConnected = 1;
@@ -140,7 +140,6 @@ void Can_SetMotorGear(int iMotorIndex, int iGear)
     {
         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);
@@ -152,7 +151,6 @@ void Can_SetMotorGear(int iMotorIndex, int iGear)
     {
         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);
@@ -164,7 +162,6 @@ void Can_SetMotorGear(int iMotorIndex, int iGear)
     {
         if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_NEUTRAL)
         {
-            MqttClient_Publish_MotorGear(iMotorIndex, iGear);
             motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_NEUTRAL;
         }
         // motor is switch to neutral -> set power to 0
@@ -172,6 +169,7 @@ void Can_SetMotorGear(int iMotorIndex, int iGear)
         WriteOutputPin(GPIO_LED_MOTRUN, LOW);
         mylog(LOG_INFO, "CAN: Motor[%d]: Set gear neutral.", iMotorIndex);
     } 
+    MqttClient_Publish_MotorGear(iMotorIndex, iGear);
 }
 
 

io/io.c

@@ -51,7 +51,7 @@ void SetupKeyPin(struct GPIO_KEY_DATA *pdata, int iKeyPin)
 
     if ((pdata->iKeyPin > 0) && nInitialized)
     {
-        //mylog(LOG_INFO, "IO: Config Pin %d as input", iOutPin);
+        mylog(LOG_DEBUG, "IO: Config Pin %d as input", pdata->iKeyPin);
         pinMode(pdata->iKeyPin, INPUT);
         pullUpDnControl(pdata->iKeyPin, PUD_UP);
     }
@@ -154,7 +154,7 @@ void SetupOutputPin(int iOutPin)
 {
     if ((iOutPin > 0) && nInitialized)
     {
-        //mylog(LOG_INFO, "IO: Config Pin %d as output", iOutPin);
+        mylog(LOG_DEBUG, "IO: Config Pin %d as output", iOutPin);
         pinMode(iOutPin, OUTPUT);
         digitalWrite(iOutPin, LOW);
     }

main.c

@@ -17,7 +17,7 @@ int iLogToConsole = 1;
 /// @param  
 void mylog(int prio, const char *format, ...)
 {
-        if (prio >= settings.iDebugLevel)
+        if (prio <= settings.iDebugLevel)
         {
                 va_list args;
 

mqtt/mqtt_client.c

@@ -8,30 +8,30 @@
 #include "main.h"
 #include <mqtt/mqtt_client.h>
 #include <can/can_client.h>
+#include <settings/settings.h>
 #include <string.h>
 
 #include <mosquitto.h>
 
 
+// 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 nMqttMotor1SwitchState = 0;
+unsigned char nMotor1SwitchState = 255;
 const char* mqtt_topic_motor2_switchstate = "Pool/Motor1/SwitchState";
-unsigned char nMqttMotor2SwitchState = 0;
+unsigned char nMotor2SwitchState = 255;
 const char* mqtt_topic_motor1_actualpowerw = "Pool/Motor1/ActualPowerW";
-int iMqttMotor1ActualPowerW = 0;
+int iMqttMotor1ActualPowerW = -1;
 const char* mqtt_topic_motor2_actualpowerw = "Pool/Motor1/ActualPowerW";
-int iMqttMotor2ActualPowerW = 0;
+int iMqttMotor2ActualPowerW = -1;
 
 const char* mqtt_broker_addr = "127.0.0.1";
 const int mqtt_broker_port = 1883;
@@ -50,60 +50,46 @@ void my_message_callback(struct mosquitto *mosq, void *userdata, const struct mo
     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))
         {
-            mylog(LOG_INFO, "MQTT: Received value for mqtt_topic_motor1_gear: %d", val);
+            mylog(LOG_INFO, "MQTT: Received value for mqtt_topic_motor_gear_request: %d", val);
-            iMqttMotor1Gear = val;
+            if (val == 123456789)
-            Can_SetMotorGear(0, val);
+            {
+                val = 0;
+            }
+
+            for (int i=0; i<settings.iMotorCount; i++)
+            {
+                Can_SetMotorGear(i, val);
+            }
         }
         else
         {
-            mylog(LOG_WARNING, "MQTT: Received mqtt_topic_motor1_gear: %s", 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))
         {
-            mylog(LOG_INFO, "MQTT: Received value for mqtt_topic_motor1_power: %d", val);
+            mylog(LOG_INFO, "MQTT: Received value for mqtt_topic_motor_power_request: %d", val);
-            iMqttMotor1Power = val;
+            if (val == 123456789)
-            Can_SetMotorPower(0, val);
+            {
+                val = 0;
+            }
+            
+            for (int i=0; i<settings.iMotorCount; i++)
+            {
+                Can_SetMotorPower(i, val);
+            }
         }
         else
         {
-            mylog(LOG_WARNING, "MQTT: Received mqtt_topic_motor1_power: %s", topic_value);
+            mylog(LOG_WARNING, "MQTT: Received mqtt_topic_motor_gear_request: %s", topic_value);
-        }
-    }
-    else if (strcmp(message->topic, mqtt_topic_motor2_gear) == 0)
-    {
-        int val = 9999;
-        if (sscanf(topic_value, "%d", &val))
-        {
-            mylog(LOG_INFO, "MQTT: Received value for mqtt_topic_motor2_gear: %d", val);
-            iMqttMotor2Gear = val;
-            Can_SetMotorGear(1, val);
-        }
-        else
-        {
-            mylog(LOG_WARNING, "MQTT: Received mqtt_topic_motor2_gear: %s", topic_value);
-        }
-    }
-    else if (strcmp(message->topic, mqtt_topic_motor2_power) == 0)
-    {
-        int val = 9999;
-        if (sscanf(topic_value, "%d", &val))
-        {
-            mylog(LOG_INFO, "MQTT: Received value for mqtt_topic_motor2_power: %d", val);
-            iMqttMotor2Power = val;
-            Can_SetMotorPower(1, val);
-        }
-        else
-        {
-            mylog(LOG_WARNING, "MQTT: Received mqtt_topic_motor2_power: %s", topic_value);
         }
     }
     else
@@ -114,6 +100,38 @@ void my_message_callback(struct mosquitto *mosq, void *userdata, const struct mo
     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 
@@ -138,6 +156,7 @@ int MqttClient_Connect()
 
     // 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)
@@ -152,81 +171,11 @@ 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)
-    {
-        mylog(LOG_ERR, "MQTT: mosquitto_publish(mqtt_topic_motor1_gear) failed!");
-        MqttClient_Close();
-        iHadConnectError++;
-        return 10;
-    }
-    if (mosquitto_publish(mosq, NULL, mqtt_topic_motor1_power, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS)
-    {
-        mylog(LOG_ERR, "MQTT: mosquitto_publish(mqtt_topic_motor1_power) failed!");
-        MqttClient_Close();
-        iHadConnectError++;
-        return 11;
-    }
-    if (mosquitto_publish(mosq, NULL, mqtt_topic_motor2_gear, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS)
-    {
-        mylog(LOG_ERR, "MQTT: mosquitto_publish(mqtt_topic_motor2_gear) failed!");
-        MqttClient_Close();
-        iHadConnectError++;
-        return 12;
-        MqttClient_Close();
-    }
-    if (mosquitto_publish(mosq, NULL, mqtt_topic_motor2_power, strlen(message), &message, 0, false) != MOSQ_ERR_SUCCESS)
-    {
-        mylog(LOG_ERR, "MQTT: mosquitto_publish(mqtt_topic_motor2_power) failed!");
-        MqttClient_Close();
-        iHadConnectError++;
-        return 13;
-    }
 
-    // subscribe all needed topics
+    mylog(LOG_INFO, "MQTT: Connected successfull after %d errors!", iHadConnectError);
-    if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor1_gear, 0) != MOSQ_ERR_SUCCESS)
-    {
-        mylog(LOG_ERR, "MQTT: mosquitto_subscribe(mqtt_topic_motor1_gear) failed!");
-        iHadConnectError++;
-        return 20;
-    }
-    if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor1_power, 0) != MOSQ_ERR_SUCCESS)
-    {
-        mylog(LOG_ERR, "MQTT: mosquitto_subscribe(mqtt_topic_motor1_power) failed!");
-        MqttClient_Close();
-        iHadConnectError++;
-        return 21;
-    }
-    if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor2_gear, 0) != MOSQ_ERR_SUCCESS)
-    {
-        mylog(LOG_ERR, "MQTT: mosquitto_subscribe(mqtt_topic_motor2_gear) failed!");
-        MqttClient_Close();
-        iHadConnectError++;
-        return 22;
-    }
-    if (mosquitto_subscribe(mosq, NULL, mqtt_topic_motor2_power, 0) != MOSQ_ERR_SUCCESS)
-    {
-        mylog(LOG_ERR, "MQTT: mosquitto_subscribe(mqtt_topic_motor2_power) failed!");
-        MqttClient_Close();
-        iHadConnectError++;
-        return 23;
-    }
-
-    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;
 }
 
@@ -266,23 +215,15 @@ void MqttClient_Publish_MotorGear(int iMotorIndex, int iGear)
 {
     if (iMotorIndex == 0)
     {
-        if (iGear != iMqttMotor1Gear)
+        char message[100];
-        {
+        snprintf(message, sizeof(message), "%d", iGear);
-            iMqttMotor1Gear = iGear;
+        mosquitto_publish(mosq, NULL, mqtt_topic_motor1_gear, strlen(message), message, 0, false);
-            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)
+        char message[100];
-        {
+        snprintf(message, sizeof(message), "%d", iGear);
-            iMqttMotor2Gear = iGear;
+        mosquitto_publish(mosq, NULL, mqtt_topic_motor2_gear, strlen(message), message, 0, false);
-            char message[100];
-            snprintf(message, sizeof(message), "%d", iGear);
-            mosquitto_publish(mosq, NULL, mqtt_topic_motor2_gear, strlen(message), message, 0, false);
-        }
     }
 }
 
@@ -294,23 +235,15 @@ void MqttClient_Publish_MotorPower(int iMotorIndex, int iPower)
 {
     if (iMotorIndex == 0)
     {
-        if (iPower != iMqttMotor1Power)
+        char message[100];
-        {
+        snprintf(message, sizeof(message), "%d", iPower);
-            iMqttMotor1Power = iPower;
+        mosquitto_publish(mosq, NULL, mqtt_topic_motor1_power, strlen(message), message, 0, false);
-            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)
+        char message[100];
-        {
+        snprintf(message, sizeof(message), "%d", iPower);
-            iMqttMotor2Power = iPower;
+        mosquitto_publish(mosq, NULL, mqtt_topic_motor2_power, strlen(message), message, 0, false);
-            char message[100];
-            snprintf(message, sizeof(message), "%d", iPower);
-            mosquitto_publish(mosq, NULL, mqtt_topic_motor2_power, strlen(message), message, 0, false);
-        }
     }
 }
 
@@ -322,9 +255,9 @@ void MqttClient_Publish_MotorSwitchState(int iMotorIndex, unsigned char nSwitchS
 {
     if (iMotorIndex == 0)
     {
-        if (nSwitchState != nMqttMotor1SwitchState)
+        if (nSwitchState != nMotor1SwitchState)
         {
-            nMqttMotor1SwitchState = nSwitchState;
+            nMotor1SwitchState = nSwitchState;
             char message[100];
             snprintf(message, sizeof(message), "%2X", nSwitchState);
             mosquitto_publish(mosq, NULL, mqtt_topic_motor1_switchstate, strlen(message), message, 0, false);
@@ -332,9 +265,9 @@ void MqttClient_Publish_MotorSwitchState(int iMotorIndex, unsigned char nSwitchS
     }
     else if (iMotorIndex == 1)
     {
-        if (nSwitchState != nMqttMotor2SwitchState)
+        if (nSwitchState != nMotor2SwitchState)
         {
-            nMqttMotor2SwitchState = nSwitchState;
+            nMotor2SwitchState = nSwitchState;
             char message[100];
             snprintf(message, sizeof(message), "%2X", nSwitchState);
             mosquitto_publish(mosq, NULL, mqtt_topic_motor2_switchstate, strlen(message), message, 0, false);

settings/settings.c

@@ -1,7 +1,7 @@
 
 #include <main.h>
 #include <settings/settings.h>
-
+#include <ctype.h>
 
 struct APP_SETTINGS settings;
 
@@ -16,7 +16,7 @@ void Settings_InitDefaultValues()
 // #define        LOG_INFO         6        /* informational */
 // #define        LOG_DEBUG        7        /* debug-level messages */
     settings.iDebugLevel = LOG_INFO;
-    settings.iCanSimu = 0;
+    settings.iCanSimu = 1;
     
     settings.iMotorCount = 1;
     settings.iMotorPwrMinRaw = 38;
@@ -38,17 +38,58 @@ void Settings_InitDefaultValues()
 }
 
 
+
+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";
+    
+    //const char *filename = "/etc/CanRtDriver.conf";
 
+    char filename[MAX_PATH + 50];
+    sprintf(filename, "%s.conf", settings.sExePath);
     FILE *file = fopen(filename, "r");
     if (file == NULL) 
     {
-        mylog(LOG_ERR, "Failed to open settings file %s", filename);
+        mylog(LOG_INFO, "SETTINGS: File %s noch found", filename);
-        return;
+        sprintf(filename, "/etc/CanRtDriver.conf");
+        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)) 
     {
@@ -62,13 +103,42 @@ void Settings_ReadConfFile()
         line[strcspn(line, "\r\n")] = 0;
 
         // 3. Split line to key and value
-        char *key = strtok(line, "=");
+        char *key = trim_str(strtok(line, "="));
-        char *value = strtok(NULL, "=");
+        char *value = trim_str(strtok(NULL, "="));
 
         if ((key != NULL) && (value != NULL)) 
         {
-            mylog(LOG_DEBUG, "SETTINGS: Found key: '%s' | value: '%s'\n", key, value);
+            //mylog(LOG_INFO, "SETTINGS: Found key: '%s' | value: '%s'", key, value);
             
+            if (strcmp(key, "DebugLevel") == 0)
+            {
+                settings.iDebugLevel = atoi(value);
+                mylog(LOG_DEBUG, "SETTINGS: %s = %d", key, settings.iDebugLevel);
+            }
+            else if (strcmp(key, "CanSimu") == 0)
+            {
+                settings.iCanSimu = atoi(value);
+                mylog(LOG_DEBUG, "SETTINGS: %s = %d", key, settings.iCanSimu);
+            }
+            else if (strcmp(key, "MotorPowerMinRaw") == 0)
+            {
+                settings.iMotorPwrMinRaw = atoi(value);
+                mylog(LOG_DEBUG, "SETTINGS: %s = %d", key, settings.iMotorPwrMinRaw);
+            }
+            else if (strcmp(key, "MotorPowerMaxRaw") == 0)
+            {
+                settings.iMotorPwrMaxRaw = atoi(value);
+                mylog(LOG_DEBUG, "SETTINGS: %s = %d", key, settings.iMotorPwrMaxRaw);
+            }
+            else if (strcmp(key, "MotorPowerStepCount") == 0)
+            {
+                settings.iMotorPwrStepCount = atoi(value);
+                mylog(LOG_DEBUG, "SETTINGS: %s = %d", key, settings.iMotorPwrStepCount);
+            }
+            else
+            {
+                mylog(LOG_WARNING, "SETTING: Unknown key: %s", key);
+            }
             // // Beispiel: Wert verarbeiten
             // if (strcmp(key, "port") == 0) 
             // {

settings/settings.h

@@ -8,12 +8,13 @@
 #include <string.h>
 
 #define MAX_LINE_LENGTH 256
+#define MAX_PATH 256
 
 struct APP_SETTINGS
 {
     int iDebugLevel;            // Level of debug messages
     char sExePath[MAX_PATH];    // Path of the executable
-    int iCanSimu = 0;           // Simulate CAN if 1
+    int iCanSimu;               // Simulate CAN if 1
 
     int iMotorCount;            // Number of used motors (1 or 2)
     int iMotorPwrMinRaw;        // Minimum power value for motor (raw value)