From 6441982964ba46c3f28360ba5a87afefbe8416c9 Mon Sep 17 00:00:00 2001 From: NeoZng Date: Sun, 30 Oct 2022 22:19:13 +0800 Subject: [PATCH] update some motor defs --- .vscode/launch.json | 16 ++++++- Src/main.c | 2 +- bsp/bsp_can.c | 41 ++++++++++++------ bsp/bsp_can.h | 25 ++++++++--- modules/motor/HT04.c | 4 +- modules/motor/HT04.h | 2 +- modules/motor/LK9025.h | 1 + modules/motor/dji_motor.c | 87 +++++++++++++++++++++++++++++++++++--- modules/motor/dji_motor.h | 37 ++++++++++------ modules/motor/motor_def.h | 52 +++++++++++++++++++++-- modules/motor/motor_task.h | 10 +---- 11 files changed, 224 insertions(+), 53 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index f901619..b7051ea 100644 --- a/.vscode/launch.json +++ b/.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" } ] } \ No newline at end of file diff --git a/Src/main.c b/Src/main.c index 18dcf28..b278330 100644 --- a/Src/main.c +++ b/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) */ diff --git a/bsp/bsp_can.c b/bsp/bsp_can.c index 279c054..132bd3d 100644 --- a/bsp/bsp_can.c +++ b/bsp/bsp_can.c @@ -3,6 +3,7 @@ #include #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]->rx_id=rx_id; - instance[idx]->can_module_callback=module_callback; + + instance[idx]->txconf.StdId=config.tx_id; + instance[idx]->txconf.IDE=CAN_ID_STD; + 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; - txconf.StdId=_instance->tx_id; - 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); + while(HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0); + HAL_CAN_AddTxMessage(_instance->can_handle, &_instance->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 * diff --git a/bsp/bsp_can.h b/bsp/bsp_can.h index d92fe09..cbb25d3 100644 --- a/bsp/bsp_can.h +++ b/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 diff --git a/modules/motor/HT04.c b/modules/motor/HT04.c index 35399f8..031d590 100644 --- a/modules/motor/HT04.c +++ b/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) diff --git a/modules/motor/HT04.h b/modules/motor/HT04.h index cb1ddcd..09965a8 100644 --- a/modules/motor/HT04.h +++ b/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); diff --git a/modules/motor/LK9025.h b/modules/motor/LK9025.h index 3ad9529..6a5d083 100644 --- a/modules/motor/LK9025.h +++ b/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; diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index 3c7364f..c0de8db 100644 --- a/modules/motor/dji_motor.c +++ b/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++) + { + + } + } \ No newline at end of file diff --git a/modules/motor/dji_motor.h b/modules/motor/dji_motor.h index 7c50d04..53a22a1 100644 --- a/modules/motor/dji_motor.h +++ b/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 - { - closeloop_e close_loop_type; - reverse_flag_e reverse_flag; - feedback_source_e angle_feedback_source; - feedback_source_e speed_feedback_source; - } controll_state; + /* basic config of a motor*/ + Motor_Control_Setting_s motor_settings; - float* other_angle_feedback_ptr; - float* other_speed_feedback_ptr; - - PID_t* current_PID; - PID_t* speed_PID; - PID_t* angle_PID; + /* controller used in the motor (3 loops)*/ + Motor_Controller_s motor_controller; + /* 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(); diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index 357b8c7..0e437da 100644 --- a/modules/motor/motor_def.h +++ b/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; diff --git a/modules/motor/motor_task.h b/modules/motor/motor_task.h index 1613c0d..eddf921 100644 --- a/modules/motor/motor_task.h +++ b/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();