Korrektur bei Leistungsvorgabe in Schritten

can/can_client.c

@@ -5,6 +5,8 @@
 #include <io/io.h>
 #include <mqtt/mqtt_client.h>
 
+int iCanSimu = 0;
+
 struct MOTOR_CONTROL_DATA motctrl[MOTOR_COUNT];
 struct CAN_INTERFACE_DATA intf_data[MOTOR_COUNT];
 
@@ -37,13 +39,13 @@ void IncBusTimeoutCounter(int iMotorIndex)
 int Can_OpenInterface(int iMotorIndex, const char * ifacename)
 {
     // Init control data
-    motctrl[iMotorIndex].nDriveConnected = 0;
-    motctrl[iMotorIndex].nDriveReady = 0;
     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_MAX_RAW=%d PWR_STEP_COUNT=%d", MOTOR_PWR_MAX_RAW, MOTOR_PWR_STEP_COUNT);
     
@@ -51,6 +53,16 @@ int Can_OpenInterface(int iMotorIndex, const char * ifacename)
     Can_SetMotorGear(iMotorIndex, MOTOR_GEAR_NEUTRAL);
     Can_SetMotorPower(iMotorIndex, 0);
 
+    if (iCanSimu)
+    {
+        mylog(LOG_INFO, "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)
         {
@@ -91,6 +103,7 @@ int Can_OpenInterface(int iMotorIndex, const char * ifacename)
         }
 
         mylog(LOG_INFO, "CAN: Interface %s (motor %d) opened!", ifacename, iMotorIndex);
+    }
 
     return 0;
 }
@@ -99,6 +112,8 @@ 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)
         {
@@ -109,6 +124,11 @@ void Can_CloseInterface(int iMotorIndex)
             mylog(LOG_INFO, "CAN: Interface %s (motor %d) closed.", intf_data[iMotorIndex].iface_name, iMotorIndex);
         }
     }
+    else
+    {
+        mylog(LOG_INFO, "CAN: Close simulation mode.");
+    }
+}
 
 
 /// @brief Set gear for the given motor
@@ -188,7 +208,7 @@ void Can_SetMotorPower(int iMotorIndex, int iPower)
     }
     else
     {
-        motctrl[iMotorIndex].iMotorPowerRaw = (((MOTOR_PWR_MAX_RAW - MOTOR_PWR_MIN_RAW) * motctrl[iMotorIndex].iMotorPowerSteps) / MOTOR_PWR_STEP_COUNT) + MOTOR_PWR_MIN_RAW;
+        motctrl[iMotorIndex].iMotorPowerRaw = (((MOTOR_PWR_MAX_RAW - MOTOR_PWR_MIN_RAW) * (motctrl[iMotorIndex].iMotorPowerSteps - 1)) / (MOTOR_PWR_STEP_COUNT - 1)) + MOTOR_PWR_MIN_RAW;
     }
 
     mylog(LOG_INFO, "CAN: Motor[%d]: Set power to %d -> %d",
@@ -201,6 +221,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;
@@ -220,6 +242,7 @@ void Can_TransmitMotorGear(int iMotorIndex)
 
         }
     }
+}
 
 
 /// @brief  Send CAN protocol for motor power for the given motor
@@ -227,6 +250,8 @@ 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;
@@ -246,11 +271,14 @@ 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;
@@ -288,6 +316,7 @@ void Can_ReadData(int iMotorIndex)
             }
         }
     }
+}
 
 /// @brief Read PGN 65299
 /// @param frame