(#32) motor task thread safe

src/app/src/motor_task.c

@@ -1,7 +1,11 @@
 #include "motor_task.h"
 #include "dji_motor.h"
+#include "dm_motor.h"
+#include "mf_motor.h"
 
 void Motor_Task_Loop() {
     DJI_Motor_Send();
+    MF_Motor_Send();
+    DM_Motor_Send();
 }
 

src/devices/inc/dm_motor.h

@@ -19,6 +19,9 @@
 #define DM_MOTOR_POS_VEL (1)
 #define DM_MOTOR_VEL (2)
 
+#define DM_MOTOR_ZERO_CURRENT (0)
+#define DM_MOTOR_HARDWARE_DISABLE (1)
+
 typedef struct
 {
     uint8_t id;
@@ -42,7 +45,11 @@ typedef struct _DM_Motor_Config {
     uint8_t control_mode;
     uint32_t rx_id;
     uint32_t tx_id;
+    uint8_t disable_behavior; /* DM_MOTOR_ZERO_CURRENT or DM_MOTOR_HARDWARE_DISABLE
+    DM_MOTOR_ZERO_CURRENT: set a zero current to the motor
+    DM_MOTOR_HARDWARE_DISABLE: disable the motor by sending disable signal to motor driver
     
+    */
     float pos_offset;
     float kp;
     float kd;
@@ -52,8 +59,10 @@ typedef struct _DM_Motor {
     /* CAN Information */
     uint8_t can_bus;
     uint8_t control_mode;
+    uint8_t send_pending_flag;
     uint16_t tx_id;
     uint16_t rx_id;
+    uint8_t disable_behavior;
     CAN_Instance_t *can_instance;
 
     /* Motor Target */
@@ -73,4 +82,8 @@ DM_Motor_Handle_t* DM_Motor_Init(DM_Motor_Config_t *config);
 void DM_Motor_Ctrl_MIT(DM_Motor_Handle_t *motor, float target_pos, float target_vel, float torq);
 void DM_Motor_Set_MIT_PD(DM_Motor_Handle_t *motor, float kp, float kd);
 
+/**
+ * Global function to send the motor control data
+*/
+void DM_Motor_Send(void);
 #endif

src/devices/inc/mf_motor.h

@@ -42,6 +42,7 @@ typedef struct _MF_Motor
     /* CAN Information */
     uint8_t can_bus;
     uint8_t control_mode;
+    uint8_t send_pending_flag;
     uint16_t tx_id;
     uint16_t rx_id;
     CAN_Instance_t *can_instance;
@@ -62,22 +63,22 @@ void MF_Motor_Decode(CAN_Instance_t *can_instance);
 MF_Motor_Handle_t *MF_Motor_Init(MF_Motor_Config_t config);
 
 // TODO: document functions
-HAL_StatusTypeDef MF_Motor_GetPIDParam(MF_Motor_Handle_t *motor);
+void MF_Motor_GetPIDParam(MF_Motor_Handle_t *motor);
 
 // TODO: document functions
-HAL_StatusTypeDef MF_Motor_PIDToRam(MF_Motor_Handle_t *motor,
+void MF_Motor_PIDToRam(MF_Motor_Handle_t *motor,
                        uint8_t kp_ang, uint8_t ki_ang,
                        uint8_t kp_vel, uint8_t ki_vel,
                        uint8_t kp_torq, uint8_t ki_torq);
 
 // TODO: document functions
-HAL_StatusTypeDef MF_Motor_EnableMotor(MF_Motor_Handle_t *motor);
+void MF_Motor_EnableMotor(MF_Motor_Handle_t *motor);
 
 // TODO: document functions
-HAL_StatusTypeDef MF_Motor_DisableMotor(MF_Motor_Handle_t *motor);
+void MF_Motor_DisableMotor(MF_Motor_Handle_t *motor);
 
 // TODO: document functions
-HAL_StatusTypeDef MF_Motor_PIDToRam(MF_Motor_Handle_t *motor,
+void MF_Motor_PIDToRam(MF_Motor_Handle_t *motor,
                        uint8_t kp_ang, uint8_t ki_ang,
                        uint8_t kp_vel, uint8_t ki_vel,
                        uint8_t kp_torq, uint8_t ki_torq);
@@ -90,39 +91,40 @@ HAL_StatusTypeDef MF_Motor_PIDToRam(MF_Motor_Handle_t *motor,
  *                      MF Motors: 2048 represents 16.5A
  *                      MG Motors: 2048 represents 33A
  *                  - Check motor datasheet for torque constant
- * @return HAL_StatusTypeDef: HAL_OK if successful, HAL_ERROR otherwise
 */
-HAL_StatusTypeDef MF_Motor_TorqueCtrl(MF_Motor_Handle_t *motor, int16_t torq);
+void MF_Motor_TorqueCtrl(MF_Motor_Handle_t *motor, int16_t torq);
 
 /**
  * @brief Closed loop velocity control for MF motor.
  * @param motor Pointer to the MF motor handle structure.
  * @param vel Desired velocity setpoint for the motor.
  *             - unit: 0.01dps/LSB
- * @return HAL_StatusTypeDef Returns HAL_OK if the command is transmitted successfully, HAL_ERROR otherwise.
- *
+ * 
  * @note The function packages the velocity command into a CAN bus frame.
  *       DATA[0] is set to command identifier 0xA2.
  *       DATA[4] to DATA[7] are assigned the velocity value, split into 4 bytes.
  *       This function uses a local buffer to avoid multiple dereferences of the motor handle.
  * 
  * @warning motor respond velocity in 1dps/LSB
  */
-HAL_StatusTypeDef MF_Motor_VelocityCtrl(MF_Motor_Handle_t *motor, int32_t vel);
+void MF_Motor_VelocityCtrl(MF_Motor_Handle_t *motor, int32_t vel);
 
 /**
  * @brief Closed loop position control for MF motor.
  * @param motor Pointer to the MF motor handle structure.
  * @param pos Desired position setpoint for the motor.
  *             - Unit: 0.01 degree/LSB (36000 represents 360 degrees)
- * @return HAL_StatusTypeDef Returns HAL_OK if the command is transmitted successfully, HAL_ERROR otherwise.
  *
  * @note The function packages the position command into a CAN bus frame.
  *       DATA[0] is set to the command identifier 0xA3.
  *       DATA[4] to DATA[7] are assigned the position value, split into 4 bytes.
  *       This function uses a local buffer to avoid multiple dereferences of the motor handle and initializes
  *       the buffer to zero before setting the command identifier and position value.
  */
-HAL_StatusTypeDef MF_Motor_PositionCtrl(MF_Motor_Handle_t *motor, int32_t pos);
+void MF_Motor_PositionCtrl(MF_Motor_Handle_t *motor, int32_t pos);
 
+/**
+ * @brief Global function to send the motor control data.
+*/
+void MF_Motor_Send(void);
 #endif

src/devices/src/dm_motor.c

@@ -3,6 +3,7 @@
 
 #define DM_MAX_DEVICE (10)
 DM_Motor_Handle_t *g_dm_motors[DM_MAX_DEVICE] = {NULL};
+uint8_t g_dm_motor_num = 0;
 
 /**
  * int float_to_uint(float x, float x_min, float x_max, int bits)
@@ -44,9 +45,9 @@ void DM_Motor_Decode(CAN_Instance_t *motor_can_instance)
     data_frame->pos_int = (data[1] << 8) | data[2];
     data_frame->vel_int = (data[3] << 4) | (data[4] >> 4);
     data_frame->torq_int = ((data[4] & 0xF) << 8) | data[5];
-    data_frame->pos_raw = uint_to_float(data_frame->pos_int, P_MIN, P_MAX, 16);   // (-12.5,12.5)
-    data_frame->vel = uint_to_float(data_frame->vel_int, V_MIN, V_MAX, 12);   // (-45.0,45.0)
-    data_frame->torq = uint_to_float(data_frame->torq_int, T_MIN, T_MAX, 12); // (-18.0,18.0)
+    data_frame->pos_raw = uint_to_float(data_frame->pos_int, P_MIN, P_MAX, 16); // (-12.5,12.5)
+    data_frame->vel = uint_to_float(data_frame->vel_int, V_MIN, V_MAX, 12);     // (-45.0,45.0)
+    data_frame->torq = uint_to_float(data_frame->torq_int, T_MIN, T_MAX, 12);   // (-18.0,18.0)
     data_frame->t_mos = (float)(data[6]);
     data_frame->t_rotor = (float)(data[7]);
 
@@ -65,22 +66,50 @@ void DM_Motor_Enable_Motor(DM_Motor_Handle_t *motor)
     data[5] = 0xFF;
     data[6] = 0xFF;
     data[7] = 0xFC;
-    CAN_Transmit(motor_can_instance);
+
+    // set the flag to send the data
+    motor->send_pending_flag = 1;
 }
 
 void DM_Motor_Disable_Motor(DM_Motor_Handle_t *motor)
 {
-    CAN_Instance_t *motor_can_instance = motor->can_instance;
-    uint8_t *data = motor_can_instance->tx_buffer;
-    data[0] = 0xFF;
-    data[1] = 0xFF;
-    data[2] = 0xFF;
-    data[3] = 0xFF;
-    data[4] = 0xFF;
-    data[5] = 0xFF;
-    data[6] = 0xFF;
-    data[7] = 0xFD;
-    CAN_Transmit(motor_can_instance);
+    uint8_t *data = motor->can_instance->tx_buffer;
+    switch (motor->disable_behavior)
+    {
+    case DM_MOTOR_ZERO_CURRENT:
+        uint16_t pos_temp, vel_temp, kp_temp, kd_temp, torq_temp;
+
+        pos_temp = float_to_uint(0, P_MIN, P_MAX, 16);
+        vel_temp = float_to_uint(0, V_MIN, V_MAX, 12);
+        kp_temp = float_to_uint(0, KP_MIN, KP_MAX, 12);
+        kd_temp = float_to_uint(0, KD_MIN, KD_MAX, 12);
+        torq_temp = float_to_uint(0, T_MIN, T_MAX, 12);
+
+        data[0] = (pos_temp >> 8);
+        data[1] = pos_temp;
+        data[2] = (vel_temp >> 4);
+        data[3] = ((vel_temp & 0xF) << 4) | (kp_temp >> 8);
+        data[4] = kp_temp;
+        data[5] = (kd_temp >> 4);
+        data[6] = ((kd_temp & 0xF) << 4) | (torq_temp >> 8);
+        data[7] = torq_temp;
+        break;
+    case DM_MOTOR_HARDWARE_DISABLE:
+        data[0] = 0xFF;
+        data[1] = 0xFF;
+        data[2] = 0xFF;
+        data[3] = 0xFF;
+        data[4] = 0xFF;
+        data[5] = 0xFF;
+        data[6] = 0xFF;
+        data[7] = 0xFE;
+        break;
+    default:
+        break;
+    }
+
+    // set the flag to send the data
+    motor->send_pending_flag = 1;
 }
 
 void DM_Motor_Ctrl_MIT(DM_Motor_Handle_t *motor, float target_pos, float target_vel, float torq)
@@ -106,7 +135,8 @@ void DM_Motor_Ctrl_MIT(DM_Motor_Handle_t *motor, float target_pos, float target_
     data[6] = ((kd_temp & 0xF) << 4) | (torq_temp >> 8);
     data[7] = torq_temp;
 
-    CAN_Transmit(motor_can_instance);
+    // set the flag to send the data
+    motor->send_pending_flag = 1;
 }
 
 void DM_Motor_Set_MIT_PD(DM_Motor_Handle_t *motor, float kp, float kd)
@@ -115,43 +145,45 @@ void DM_Motor_Set_MIT_PD(DM_Motor_Handle_t *motor, float kp, float kd)
     motor->kd = kd;
 }
 
-DM_Motor_Handle_t* DM_Motor_Init(DM_Motor_Config_t *config)
+DM_Motor_Handle_t *DM_Motor_Init(DM_Motor_Config_t *config)
 {
     DM_Motor_Handle_t *motor = malloc(sizeof(DM_Motor_Handle_t));
     motor->can_bus = config->can_bus;
     motor->control_mode = config->control_mode;
     motor->tx_id = config->tx_id;
     motor->rx_id = config->rx_id;
-    motor->target_pos = 0.0f;
-    motor->target_vel = 0.0f;
+    motor->disable_behavior = config->disable_behavior; // by defualt set to zero current
+
     motor->kp = config->kp;
     motor->kd = config->kd;
-    motor->torq = 0.0f;
     motor->stats = calloc(sizeof(DM_Motor_Stats_t), 1);
     motor->stats->pos_offset = config->pos_offset;
-    
+
     motor->can_instance = CAN_Device_Register(motor->can_bus, motor->tx_id, motor->rx_id, DM_Motor_Decode);
-    motor->can_instance->binding_motor_stats =(void*) motor->stats;
+    motor->can_instance->binding_motor_stats = (void *)motor->stats;
 
-    for (int i = 0; i < DM_MAX_DEVICE; i++)
-    {
-        if (g_dm_motors[i] == NULL)
-        {
-            g_dm_motors[i] = motor;
-            break;
-        }
-    }
+    g_dm_motors[g_dm_motor_num++] = motor;
     return motor;
 }
 
-
-
 void DM_Motor_CtrlPosVel()
 {
-    //TODO:
+    // TODO:
 }
 
 void DM_Motor_CtrlVel()
 {
     // TODO:
 }
+
+void DM_Motor_Send()
+{
+    for (int i = 0; i < g_dm_motor_num; i++) // loop through all the motors
+    {
+        if (g_dm_motors[i]->send_pending_flag)
+        {                                               // check if the flag is set
+            CAN_Transmit(g_dm_motors[i]->can_instance); // send the data
+            g_dm_motors[i]->send_pending_flag = 0;      // clear the flag
+        }
+    }
+}