Kommunikation hat funktioniert

can/can_client.c

@@ -0,0 +1,179 @@
+
+#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];
+
+/// @brief Open socket of CAN interface for the given motor
+/// @param iMotorIndex 
+/// @param ifacename 
+/// @return 
+int Can_OpenInterface(int iMotorIndex, const char * ifacename)
+{
+    strcpy(intf_data[iMotorIndex].iface_name, ifacename);
+    Can_SetMotorGear(iMotorIndex, 0);
+    Can_SetMotorPower(iMotorIndex, 0);
+
+    // 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);
+        return 1;
+    }
+
+    // retrieve the interface index
+    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);
+        return 2;
+    }
+
+    // bind the socket to the CAN interface
+    memset(&intf_data[iMotorIndex].addr, 0, sizeof(intf_data[iMotorIndex].addr));
+    intf_data[iMotorIndex].addr.can_family = AF_CAN;
+    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);
+        return 3;
+    }
+
+    printf("Interface %s (motor %d) opened!\n", ifacename, iMotorIndex);
+
+    return 0;
+}
+
+
+/// @brief Close socket of CAN interface for the given motor
+/// @param iMotorIndex 
+void Can_CloseInterface(int iMotorIndex)
+{
+    if (close(intf_data[iMotorIndex].socket) < 0)
+    {
+        printf("Could not close socket of motor %d!\n", iMotorIndex);
+    }
+    else
+    {
+        printf("Interface %s (motor %d) closed.\n", intf_data[iMotorIndex].iface_name, iMotorIndex);
+    }
+}
+
+
+/// @brief Set gear for the given motor
+/// @param iMotorIndex 
+/// @param iGear (-1=reverse, 0=neutral, 1=forward)
+void Can_SetMotorGear(int iMotorIndex, int iGear)
+{
+    if (iGear > 0)
+    {
+        if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_FORWARD)
+        {
+            MqttClient_Publish_MotorGear(iMotorIndex, iGear);
+            motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_FORWARD;
+        }
+        WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
+        printf("Motor[%d]: Set gear forward.\n", iMotorIndex);
+    } 
+    else if (iGear < 0)
+    {
+        if (motctrl[iMotorIndex].iMotorGear != MOTOR_GEAR_REVERSE)
+        {
+            MqttClient_Publish_MotorGear(iMotorIndex, iGear);
+            motctrl[iMotorIndex].iMotorGear = MOTOR_GEAR_REVERSE;
+        }
+        WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
+        printf("Motor[%d]: Set gear reverse.\n", 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);
+        WriteOutputPin(GPIO_LED_MOTRUN, LOW);
+        printf("Motor[%d]: Set gear neutral.\n", iMotorIndex);
+    } 
+}
+
+
+/// @brief Set power for the given motor
+/// @param iMotorIndex 
+/// @param iPower (Range: 0..100)
+void Can_SetMotorPower(int iMotorIndex, int iPower)
+{
+    if (iPower <= MOTOR_PWR_MIN_PCT)
+    {
+        motctrl[iMotorIndex].iMotorPowerPct = MOTOR_PWR_MIN_PCT;
+    }
+    else if (iPower >= MOTOR_PWR_MAX_PCT)
+    {
+        motctrl[iMotorIndex].iMotorPowerPct = MOTOR_PWR_MAX_PCT;
+    } 
+    else 
+    {
+        motctrl[iMotorIndex].iMotorPowerPct = iPower;
+    } 
+    MqttClient_Publish_MotorPower(iMotorIndex, motctrl[iMotorIndex].iMotorPowerPct);
+
+    motctrl[iMotorIndex].iMotorPower = 250 * motctrl[iMotorIndex].iMotorPowerPct / 100;
+
+    printf("Motor[%d]: Set power to %d%% -> %d\n",
+        iMotorIndex, motctrl[iMotorIndex].iMotorPowerPct, motctrl[iMotorIndex].iMotorPower);
+}
+
+
+/// @brief Send CAN protocol for motor gear for the given motor
+/// @param iMotorIndex 
+void Can_TransmitMotorGear(int iMotorIndex)
+{
+    // 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))
+    {
+
+    }
+}
+
+
+/// @brief  Send CAN protocol for motor power for the given motor
+/// @param iMotorIndex 
+void Can_TransmitMotorPower(int iMotorIndex)
+{
+    // 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))
+    {
+
+    }
+}