update some motor defs

.vscode/launch.json

@@ -2,7 +2,7 @@
     "version": "0.2.0",
     "configurations": [
         {
-            "name": "Cortex Debug",
+            "name": "Debug-dap",
             "cwd": "${workspaceRoot}",
             "executable": "${workspaceRoot}\\build\\basic_framework.elf",
             "request": "launch",
@@ -17,6 +17,20 @@
             "armToolchainPath": "D:\\gcc-arm-none-eabi\\bin",
             // path to your gcc-arm-none-eabi/arm-none-eabi-gdb.exe
             "gdbPath": "D:\\gcc-arm-none-eabi\\bin\\arm-none-eabi-gdb.exe" 
+        },
+        {
+            "name": "Debug-jlink",
+            "cwd": "${workspaceFolder}",
+            "executable": "${workspaceRoot}\\build\\basic_framework.elf",
+            "request": "launch",
+            "type": "cortex-debug",
+            "device": "STM32F407IG",
+            "runToEntryPoint": "main",
+            "showDevDebugOutput": "none",
+            "servertype": "jlink",
+            "interface": "swd",
+            "armToolchainPath": "D:\\gcc-arm-none-eabi\\bin",
+            "gdbPath": "D:\\gcc-arm-none-eabi\\bin\\arm-none-eabi-gdb.exe" 
         }
     ]
 }

Src/main.c

@@ -37,6 +37,7 @@
 #include "bsp_can.h"
 #include "can.h"
 #include "LK9025.h"
+#include "dji_motor.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -114,7 +115,6 @@ int main(void)
   MX_USART1_UART_Init();
   MX_USART6_UART_Init();
   /* USER CODE BEGIN 2 */
-  
   /* USER CODE END 2 */
 
   /* Call init function for freertos objects (in freertos.c) */

bsp/bsp_can.c

@@ -3,6 +3,7 @@
 #include <stdlib.h>
 #include "memory.h"
 
+/* can instance ptrs storage, used for recv callback */
 static can_instance* instance[MX_REGISTER_DEVICE_CNT];
 
 /**
@@ -52,7 +53,11 @@ static void CANServiceInit()
     HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);
 }
 
-can_instance* CANRegister(uint8_t tx_id,uint8_t rx_id,CAN_HandleTypeDef* can_handle,can_callback module_callback)
+
+/* -----------------------two callable function -----------------------*/
+
+
+can_instance* CANRegister(can_instance_config config)
 {
     static uint8_t idx;
     if(!idx)
@@ -60,25 +65,33 @@ can_instance* CANRegister(uint8_t tx_id,uint8_t rx_id,CAN_HandleTypeDef* can_han
         CANServiceInit();
     }
     instance[idx]=(can_instance*)malloc(sizeof(can_instance));
-    instance[idx]->can_handle=can_handle;
+
-    instance[idx]->tx_id=tx_id;
+    instance[idx]->txconf.StdId=config.tx_id;
-    instance[idx]->rx_id=rx_id;
+    instance[idx]->txconf.IDE=CAN_ID_STD;
-    instance[idx]->can_module_callback=module_callback;
+    instance[idx]->txconf.RTR=CAN_RTR_DATA;
+    instance[idx]->txconf.DLC=0x08;
+
+    instance[idx]->can_handle=config.can_handle;
+    instance[idx]->tx_id=config.tx_id;
+    instance[idx]->rx_id=config.rx_id;
+    instance[idx]->can_module_callback=config.can_module_callback;
+
     CANAddFilter(instance[idx]);
+
     return instance[idx++];
 }
 
+
 void CANTransmit(can_instance* _instance)
 {
-    CAN_TxHeaderTypeDef txconf;
+    while(HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0);
-    txconf.StdId=_instance->tx_id;
+    HAL_CAN_AddTxMessage(_instance->can_handle, &_instance->txconf, _instance->tx_buff, &_instance->tx_mailbox);
-    txconf.IDE=CAN_ID_STD;
-    txconf.RTR=CAN_RTR_DATA;
-    txconf.DLC=0x08;
-    while (HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0);
-    HAL_CAN_AddTxMessage(_instance->can_handle, &txconf, _instance->tx_buff, &_instance->tx_mailbox);
 }
 
+
+/* -----------------------belows are callback definitions--------------------------*/
+
+
 /**
  * @brief this func will recv data from @param:fifox to a tmp can_rx_buff
  *        then, all the instances will be polling to check which should recv this pack of data
@@ -102,7 +115,8 @@ static void CANFIFOxCallback(CAN_HandleTypeDef* _hcan,uint32_t fifox)
     }   
 }
 
-/* ATTENTION: two CAN device in STM32 share two FIFO */
+
+/* ATTENTION: two CAN devices in STM32 share two FIFOs */
 /* functions below will call CANFIFOxCallback() to further process message from a specific CAN device */
 /**
  * @brief rx fifo callback. Once FIFO_0 is full,this func would be called
@@ -114,6 +128,7 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
     CANFIFOxCallback(hcan, CAN_RX_FIFO0);
 }
 
+
 /**
  * @brief rx fifo callback. Once FIFO_1 is full,this func would be called
  * 

bsp/bsp_can.h

@@ -9,21 +9,35 @@
 #define MX_CAN_FILTER_CNT (4 * 14) // temporarily useless
 #define DEVICE_CAN_CNT 2           // CAN1,CAN2
 
+
+
 /* can instance typedef, every module registered to CAN should have this variable */
-typedef struct tmp
+typedef struct _
 {
     CAN_HandleTypeDef* can_handle;
+    CAN_TxHeaderTypeDef txconf;
     uint32_t tx_id;
     uint32_t tx_mailbox;
     uint8_t tx_buff[8]; 
     uint8_t rx_buff[8];
     uint32_t rx_id;
-    void (*can_module_callback)(struct tmp*); // callback needs an instance to tell among registered ones
+    void (*can_module_callback)(struct _*); // callback needs an instance to tell among registered ones
 } can_instance;
 
+
+/* this structure is used as initialization*/
+typedef struct 
+{
+    CAN_HandleTypeDef* can_handle;
+    uint32_t tx_id;
+    uint32_t rx_id;
+    void (*can_module_callback)(can_instance*);
+} can_instance_config;
+
 /* module callback,which resolve protocol when new mesg arrives*/
 typedef void (*can_callback)(can_instance*);
 
+
 /**
  * @brief transmit mesg through CAN device
  * 
@@ -31,13 +45,14 @@ typedef void (*can_callback)(can_instance*);
  */
 void CANTransmit(can_instance* _instance);
 
+
 /**
  * @brief Register a module to CAN service,remember to call this before using a CAN device
  * 
- * @param _instance can instance owned by a specific device, remember to initialize it!
+ * @param config init config
- * 
+ * @return can_instance* can instance owned by module
  */
-can_instance* CANRegister(uint8_t tx_id,uint8_t rx_id,CAN_HandleTypeDef* can_handle,can_callback module_callback);
+can_instance* CANRegister(can_instance_config config);
 
 
 #endif

modules/motor/HT04.c

@@ -36,11 +36,11 @@ static void DecodeJoint(can_instance* motor_instance)
     }
 }
 
-joint_instance* HTMotorInit(CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id)
+joint_instance* HTMotorInit(can_instance_config config)
 {
     static uint8_t idx;
     joint_motor_info[idx]=(joint_instance*)malloc(sizeof(joint_instance));
-    joint_motor_info[idx++]->motor_can_instace=CANRegister(tx_id,rx_id,_hcan,DecodeJoint);
+    joint_motor_info[idx++]->motor_can_instace=CANRegister(config);
 }
 
 void JointControl(joint_instance* _instance,float current)

modules/motor/HT04.h

@@ -32,7 +32,7 @@ typedef enum
     CMD_ZERO_POSITION = 0xfe
 } joint_mode;
 
-joint_instance* HTMotorInit(CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id);
+joint_instance* HTMotorInit(can_instance_config config);
 
 void JointControl(joint_instance* _instance,float current);
 

modules/motor/LK9025.h

@@ -11,6 +11,7 @@
 
 #define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))
 
+
 typedef struct //9025
 {
     uint16_t last_ecd;

modules/motor/dji_motor.c

@@ -1,7 +1,57 @@
 #include "dji_motor.h"
 
-static dji_motor_instance* dji_motor_info[DJI_MOTOR_CNT];
+static dji_motor_instance* dji_motor_info[DJI_MOTOR_CNT]={NULL};
 
+// can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF
+// can2: [0]:0x1FF,[1]:0x200,[2]:0x2FF
+static can_instance sender_assignment[6]=
+{
+    [0]={.can_handle=&hcan1,.txconf.StdId=0x1ff,.txconf.IDE=CAN_ID_STD,.txconf.RTR=CAN_RTR_DATA,.txconf.DLC=0x08,.tx_buff={0}},
+    [1]={.can_handle=&hcan1,.txconf.StdId=0x200,.txconf.IDE=CAN_ID_STD,.txconf.RTR=CAN_RTR_DATA,.txconf.DLC=0x08,.tx_buff={0}},
+    [2]={.can_handle=&hcan1,.txconf.StdId=0x2ff,.txconf.IDE=CAN_ID_STD,.txconf.RTR=CAN_RTR_DATA,.txconf.DLC=0x08,.tx_buff={0}},
+    [3]={.can_handle=&hcan2,.txconf.StdId=0x1ff,.txconf.IDE=CAN_ID_STD,.txconf.RTR=CAN_RTR_DATA,.txconf.DLC=0x08,.tx_buff={0}},
+    [4]={.can_handle=&hcan2,.txconf.StdId=0x200,.txconf.IDE=CAN_ID_STD,.txconf.RTR=CAN_RTR_DATA,.txconf.DLC=0x08,.tx_buff={0}},
+    [5]={.can_handle=&hcan2,.txconf.StdId=0x2ff,.txconf.IDE=CAN_ID_STD,.txconf.RTR=CAN_RTR_DATA,.txconf.DLC=0x08,.tx_buff={0}},
+};
+
+
+/**
+ * @brief 
+ * 
+ * @param idx 
+ */
+static void MotorSenderGrouping(uint8_t idx,can_instance_config config)
+{
+    uint8_t motor_id=config.tx_id;
+    uint8_t motor_rx_id;
+    uint8_t motor_send_num;
+    uint8_t motor_grouping;
+    switch (dji_motor_info[idx]->motor_type)
+    {
+        case M2006:
+        case M3508:
+            if(motor_id<5)
+            {
+
+            }
+            else
+            {
+
+            }
+            break;
+            
+        case GM6020:
+            if(motor_id<5)
+            {
+                
+            }
+            else
+            {
+
+            }
+            break;
+    }
+}
 
 static void DecodeDJIMotor(can_instance* _instance)
 {
@@ -19,17 +69,44 @@ static void DecodeDJIMotor(can_instance* _instance)
     }
 }
 
-dji_motor_instance* DJIMotorInit(CAN_HandleTypeDef* _hcan,uint16_t tx_id,uint16_t rx_id)
+
+dji_motor_instance* DJIMotorInit(can_instance_config config,
+                                 Motor_Controller_s controller_config,
+                                 Motor_Control_Setting_s motor_setting,
+                                 Motor_Controller_Init_s controller_init,
+                                 Motor_Type_e type)
 {
-    static uint8_t idx;
+    static uint8_t idx; // register idx
     dji_motor_info[idx]=(dji_motor_instance*)malloc(sizeof(dji_motor_instance));
-    dji_motor_info[idx++]->motor_can_instance=CANRegister(tx_id,rx_id,_hcan,DecodeDJIMotor);
+    // motor setting
+    dji_motor_info[idx]->motor_type=type;
+    dji_motor_info[idx]->motor_settings=motor_setting;
+
+    // motor controller init @todo : PID init
+    dji_motor_info[idx]->motor_settings.angle_feedback_source=motor_setting.angle_feedback_source;
+    dji_motor_info[idx]->motor_settings.speed_feedback_source=motor_setting.speed_feedback_source;
+    // group motors, because 4 motors share the same CAN control message 
+    MotorSenderGrouping(idx,config);
+    // register motor to CAN bus
+    dji_motor_info[idx]->motor_can_instance=CANRegister(config);
+    
+    return dji_motor_info[idx++];
 }
 
-void DJIMotorSetRef();
+
+void DJIMotorSetRef()
+{
+
+
+}
 
 
 void DJIMotorControl()
 {
     
+    for (size_t i = 0; i < DJI_MOTOR_CNT; i++)
+    {
+
+    }
+    
 }

modules/motor/dji_motor.h

@@ -7,8 +7,13 @@
 #include "controller.h"
 #include "motor_def.h"
 
+/**
+ * @brief DJI intelligent motor typedef
+ * 
+ */
 typedef struct
 {
+    /* motor measurement recv from CAN feedback */
     struct 
     {
         uint16_t ecd;
@@ -20,25 +25,29 @@ typedef struct
         int32_t total_angle;
     } motor_measure;
 
-    struct 
+    /* basic config of a motor*/
-    {
+    Motor_Control_Setting_s motor_settings;
-        closeloop_e close_loop_type;
-        reverse_flag_e reverse_flag;
-        feedback_source_e angle_feedback_source;
-        feedback_source_e speed_feedback_source;
-    } controll_state;
 
-    float* other_angle_feedback_ptr;
+    /* controller used in the motor (3 loops)*/
-    float* other_speed_feedback_ptr;
+    Motor_Controller_s motor_controller;
-
-    PID_t* current_PID;
-    PID_t* speed_PID;
-    PID_t* angle_PID;
 
+    /* the CAN instance own by motor instance*/
     can_instance *motor_can_instance;
+
+    /* sender assigment*/
+    uint8_t sender_group;
+    uint8_t message_num;
+
+    Motor_Type_e motor_type;
+
 } dji_motor_instance;
 
-dji_motor_instance* DJIMotorInit(CAN_HandleTypeDef* _hcan,uint16_t tx_id,uint16_t rx_id);
+
+dji_motor_instance* DJIMotorInit(can_instance_config config,
+                                 Motor_Controller_s controller_config,
+                                 Motor_Control_Setting_s motor_setting,
+                                 Motor_Controller_Init_s controller_init,
+                                 Motor_Type_e type);
 
 void DJIMotorSetRef();
 

modules/motor/motor_def.h

@@ -6,19 +6,65 @@ typedef enum
     CURRENT=0,
     SPEED=1,
     ANGLE=2
-} closeloop_e;
+} Closeloop_Type_e;
 
 typedef enum
 {
     MOTOR=0,
     OTHER=1
-} feedback_source_e;
+} Feedback_Source_e;
 
 typedef enum
 {
     CLOCKWISE=0,
     COUNTER_CLOCKWISE=1
-} reverse_flag_e;
+} Reverse_Flag_e;
+
+
+typedef struct 
+{
+    Closeloop_Type_e close_loop_type;
+    Reverse_Flag_e reverse_flag;
+    Feedback_Source_e angle_feedback_source;
+    Feedback_Source_e speed_feedback_source;
+
+} Motor_Control_Setting_s;
+
+
+typedef struct
+{
+    float* other_angle_feedback_ptr;
+    float* other_speed_feedback_ptr;
+
+    PID_t* current_PID;
+    PID_t* speed_PID;
+    PID_t* angle_PID;
+
+} Motor_Controller_s;
+
+typedef enum 
+{
+    GM6020=0,
+    M3508=1,
+    M2006=2,
+    LK9025=3,
+    HT04=4
+} Motor_Type_e;
+
+typedef struct 
+{
+    /* data */
+} PID_config_s;
+
+typedef struct 
+{
+    float* other_angle_feedback_ptr;
+    float* other_speed_feedback_ptr;
+
+    PID_config_s current_PID;
+    PID_config_s speed_PID;
+    PID_config_s angle_PID;
+} Motor_Controller_Init_s;
 
 
 

modules/motor/motor_task.h

@@ -4,15 +4,9 @@
 #include "LK9025.h"
 #include "HT04.h"
 #include "dji_motor.h"
+#include "motor_def.h"
+
 
-typedef enum 
-{
-    GM6020=0,
-    M3508=1,
-    M2006=2,
-    LK9025=3,
-    HT04=4
-} Motor_Type_e;
 
 void MotorControlTask();