diff --git a/bsp/bsp_can.c b/bsp/bsp_can.c index 132bd3d..23c4579 100644 --- a/bsp/bsp_can.c +++ b/bsp/bsp_can.c @@ -4,33 +4,35 @@ #include "memory.h" /* can instance ptrs storage, used for recv callback */ -static can_instance* instance[MX_REGISTER_DEVICE_CNT]; +static can_instance *instance[MX_REGISTER_DEVICE_CNT]; + +/* ----------------two static function called by CANRegister()-------------------- */ /** * @brief add filter to receive mesg with specific ID,called by CANRegister() - * + * * @note there are total 28 filter and 2 FIFO in bxCAN of STM32F4 series product. * here, we assign the former 14 to CAN1 and the rest for CAN2 * when initializing, module with odd ID will be assigned to FIFO0 while even one to FIFO1 * those modules which registered in CAN1 would use Filter0-13, while CAN2 use Filter14-27 - * + * * @attention you don't have to fully understand what this function done, cause it is basically * for initialization.Enjoy developing without caring about the infrastructure! * if you really want to know what is happeng, contact author. - * + * * @param _instance can instance owned by specific module */ -static void CANAddFilter(can_instance* _instance) +static void CANAddFilter(can_instance *_instance) { CAN_FilterTypeDef can_filter_conf; - static uint8_t can1_filter_idx=0,can2_filter_idx=14; + static uint8_t can1_filter_idx = 0, can2_filter_idx = 14; can_filter_conf.FilterMode = CAN_FILTERMODE_IDLIST; can_filter_conf.FilterScale = CAN_FILTERSCALE_16BIT; can_filter_conf.FilterFIFOAssignment = (_instance->rx_id & 1) ? CAN_RX_FIFO0 : CAN_RX_FIFO1; can_filter_conf.SlaveStartFilterBank = 14; can_filter_conf.FilterIdLow = _instance->rx_id << 5; - can_filter_conf.FilterBank = _instance->can_handle==&hcan1?(can1_filter_idx++):(can2_filter_idx++); + can_filter_conf.FilterBank = _instance->can_handle == &hcan1 ? (can1_filter_idx++) : (can2_filter_idx++); can_filter_conf.FilterActivation = CAN_FILTER_ENABLE; HAL_CAN_ConfigFilter(_instance->can_handle, &can_filter_conf); @@ -38,10 +40,10 @@ static void CANAddFilter(can_instance* _instance) /** * @brief called by CANRegister before the first module being registered - * + * * @note this func will handle all these thing automatically * there is no need to worry about hardware initialization, we do these for you! - * + * */ static void CANServiceInit() { @@ -53,88 +55,82 @@ static void CANServiceInit() HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING); } +/* -----------------------two extern callable function -----------------------*/ -/* -----------------------two callable function -----------------------*/ - - -can_instance* CANRegister(can_instance_config config) +can_instance *CANRegister(can_instance_config config) { static uint8_t idx; - if(!idx) + if (!idx) { CANServiceInit(); } - instance[idx]=(can_instance*)malloc(sizeof(can_instance)); + instance[idx] = (can_instance *)malloc(sizeof(can_instance)); - 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]->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; + 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) +void CANTransmit(can_instance *_instance) { - while(HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0); + 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 - * - * @param _hcan + * + * @param _hcan * @param fifox passed to HAL_CAN_GetRxMessage() to get mesg from a specific fifo */ -static void CANFIFOxCallback(CAN_HandleTypeDef* _hcan,uint32_t fifox) +static void CANFIFOxCallback(CAN_HandleTypeDef *_hcan, uint32_t fifox) { uint8_t can_rx_buff[8]; CAN_RxHeaderTypeDef rxconf; - HAL_CAN_GetRxMessage(_hcan,fifox,&rxconf,can_rx_buff); + HAL_CAN_GetRxMessage(_hcan, fifox, &rxconf, can_rx_buff); for (size_t i = 0; i < DEVICE_CAN_CNT; i++) { - if(_hcan==instance[i]->can_handle && rxconf.StdId==instance[i]->rx_id) + if (_hcan == instance[i]->can_handle && rxconf.StdId == instance[i]->rx_id) { - memcpy(instance[i]->rx_buff,can_rx_buff,8); + memcpy(instance[i]->rx_buff, can_rx_buff, 8); instance[i]->can_module_callback(instance[i]); break; } - } + } } - /* 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 - * + * * @param hcan CAN handle indicate which device the oddest mesg in FIFO_0 comes from */ -void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) +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 - * + * * @param hcan CAN handle indicate which device the oddest mesg in FIFO_1 comes from */ -void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) +void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) { CANFIFOxCallback(hcan, CAN_RX_FIFO1); } \ No newline at end of file diff --git a/bsp/bsp_usart.c b/bsp/bsp_usart.c index cccec82..59acb60 100644 --- a/bsp/bsp_usart.c +++ b/bsp/bsp_usart.c @@ -2,29 +2,29 @@ #include "stdlib.h" /* usart service instance,modules' info would be recoreded here using ModuleRegister() */ -static usart_instance* instance[DEVICE_USART_CNT]; +static usart_instance *instance[DEVICE_USART_CNT]; /** * @brief usart service will start automatically, after each module registered - * + * * @param _instance instance owned by module */ -static void USARTServiceInit(usart_instance* _instance) +static void USARTServiceInit(usart_instance *_instance) { HAL_UARTEx_ReceiveToIdle_DMA(_instance->usart_handle, _instance->recv_buff, _instance->recv_buff_size); __HAL_DMA_DISABLE_IT(_instance->usart_handle->hdmarx, DMA_IT_HT); } -void USARTRegister(usart_instance* _instance) +void USARTRegister(usart_instance *_instance) { static instance_idx; USARTServiceInit(_instance); - instance[instance_idx++]=_instance; + instance[instance_idx++] = _instance; } -void USARTSend(usart_instance* _instance,uint8_t* send_buf, uint16_t send_size) +void USARTSend(usart_instance *_instance, uint8_t *send_buf, uint16_t send_size) { - HAL_UART_Transmit_DMA(_instance->usart_handle, send_buf,send_size); + HAL_UART_Transmit_DMA(_instance->usart_handle, send_buf, send_size); } /** diff --git a/modules/motor/LK9025.c b/modules/motor/LK9025.c index 272b618..ac8ff7d 100644 --- a/modules/motor/LK9025.c +++ b/modules/motor/LK9025.c @@ -18,11 +18,12 @@ static void DecodeDriven(can_instance* _instance) } } -driven_instance* LKMotroInit(CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id) +driven_instance* LKMotroInit(can_instance_config config) { static uint8_t idx; driven_motor_info[idx]=(driven_instance*)malloc(sizeof(driven_instance)); - driven_motor_info[idx++]->motor_can_instance=CANRegister(tx_id,rx_id,_hcan,DecodeDriven); + config.can_module_callback=DecodeDriven; + driven_motor_info[idx++]->motor_can_instance=CANRegister(config); } void DrivenControl(int16_t motor1_current,int16_t motor2_current) diff --git a/modules/motor/LK9025.h b/modules/motor/LK9025.h index 6a5d083..57e0a59 100644 --- a/modules/motor/LK9025.h +++ b/modules/motor/LK9025.h @@ -30,7 +30,7 @@ typedef enum unused = 0, } driven_mode; -driven_instance* LKMotroInit(CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id); +driven_instance* LKMotroInit(can_instance_config config); void DrivenControl(int16_t motor1_current,int16_t motor2_current); diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index c0de8db..0b1ff35 100644 --- a/modules/motor/dji_motor.c +++ b/modules/motor/dji_motor.c @@ -1,112 +1,173 @@ #include "dji_motor.h" -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}}, -}; - +static uint8_t idx = 0; // register idx +/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */ +static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL}; /** - * @brief - * - * @param idx + * @brief 由于DJI电机发送以四个一组的形式进行,故对其进行特殊处理,用6个(2can*3group)can_instance专门负责发送 + * 该变量将在 DJIMotorControl() 中使用,分组在 MotorSenderGrouping()中进行 + * + * can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF + * can2: [0]:0x1FF,[1]:0x200,[2]:0x2FF */ -static void MotorSenderGrouping(uint8_t idx,can_instance_config config) +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 6个用于确认是否有电机注册到sender_assignment中的标志位,防止发送空帧,此变量将在 DJIMotorControl() 使用 + * flag的初始化在 MotorSenderGrouping()中进行 + * + */ +static uint8_t sender_enable_flag[6] = {0}; + +/** + * @brief 根据电调/拨码开关上的ID,计算发送ID和接收ID,并对电机进行分组以便处理多电机控制命令 + * + * @param config + */ +static void MotorSenderGrouping(can_instance_config *config) { - uint8_t motor_id=config.tx_id; + uint8_t motor_id = config->tx_id - 1; 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) - { + case M2006: + case M3508: + if (motor_id < 4) + { + dji_motor_info[idx]->message_num = motor_id; + dji_motor_info[idx]->sender_group = config->can_handle == &hcan1 ? 1 : 4; + } + else + { + dji_motor_info[idx]->message_num = motor_id - 4; + dji_motor_info[idx]->sender_group = config->can_handle == &hcan1 ? 0 : 3; + } + config->rx_id = 0x200 + motor_id; + sender_enable_flag[dji_motor_info[idx]->sender_group] = 1; + break; - } - else - { - - } - break; - - case GM6020: - if(motor_id<5) - { - - } - else - { - - } - break; + case GM6020: + if (motor_id < 4) + { + dji_motor_info[idx]->message_num = motor_id; + dji_motor_info[idx]->sender_group = config->can_handle == &hcan1 ? 0 : 3; + } + else + { + dji_motor_info[idx]->message_num = motor_id - 4; + dji_motor_info[idx]->sender_group = config->can_handle == &hcan1 ? 2 : 5; + } + config->rx_id = 0x204 + motor_id; + sender_enable_flag[dji_motor_info[idx]->sender_group] = 1; + break; + // other motors should not be registered here + default: + break; } } -static void DecodeDJIMotor(can_instance* _instance) +/** + * @todo 待添加此功能. + * + * @brief 当注册的电机id冲突时,会进入这个函数并提示冲突的ID + * + */ +static void IDcrash_Handler() { + while (1) + { + }; +} + +/** + * @brief 根据返回的can_instance对反馈报文进行解析 + * + * @param _instance 收到数据的instance,通过遍历与所有电机进行对比 + */ +static void DecodeDJIMotor(can_instance *_instance) +{ + uint8_t *rxbuff = _instance->rx_buff; for (size_t i = 0; i < DJI_MOTOR_CNT; i++) { - if(dji_motor_info[i]->motor_can_instance==_instance) + if (dji_motor_info[i]->motor_can_instance == _instance) { - dji_motor_info[i]->motor_measure.last_ecd = dji_motor_info[i]->motor_measure.ecd; - dji_motor_info[i]->motor_measure.ecd = (uint16_t)(_instance->rx_buff[0] << 8 | _instance->rx_buff[1]); - dji_motor_info[i]->motor_measure.speed_rpm = (uint16_t)(_instance->rx_buff[2] << 8 | _instance->rx_buff[3]); - dji_motor_info[i]->motor_measure.given_current = (uint16_t)(_instance->rx_buff[4] << 8 | _instance->rx_buff[5]); - dji_motor_info[i]->motor_measure.temperate = _instance->rx_buff[6]; + dji_motor_info[i]->motor_measure.last_ecd = dji_motor_info[i]->motor_measure.ecd; + dji_motor_info[i]->motor_measure.ecd = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]); + dji_motor_info[i]->motor_measure.speed_rpm = (uint16_t)(rxbuff[2] << 8 | rxbuff[3]); + dji_motor_info[i]->motor_measure.given_current = (uint16_t)(rxbuff[4] << 8 | rxbuff[5]); + dji_motor_info[i]->motor_measure.temperate = rxbuff[6]; break; } } } - -dji_motor_instance* DJIMotorInit(can_instance_config config, - Motor_Controller_s controller_config, +dji_motor_instance *DJIMotorInit(can_instance_config config, Motor_Control_Setting_s motor_setting, Motor_Controller_Init_s controller_init, Motor_Type_e type) { - static uint8_t idx; // register idx - dji_motor_info[idx]=(dji_motor_instance*)malloc(sizeof(dji_motor_instance)); + dji_motor_info[idx] = (dji_motor_instance *)malloc(sizeof(dji_motor_instance)); // motor setting - dji_motor_info[idx]->motor_type=type; - dji_motor_info[idx]->motor_settings=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); + // motor controller init + // @todo : PID init + dji_motor_info[idx]->motor_controller.other_angle_feedback_ptr = controller_init.other_angle_feedback_ptr; + dji_motor_info[idx]->motor_controller.other_speed_feedback_ptr = controller_init.other_speed_feedback_ptr; + // group motors, because 4 motors share the same CAN control message + MotorSenderGrouping(&config); // register motor to CAN bus - dji_motor_info[idx]->motor_can_instance=CANRegister(config); - + config.can_module_callback = DecodeDJIMotor; + dji_motor_info[idx]->motor_can_instance = CANRegister(config); + return dji_motor_info[idx++]; } - -void DJIMotorSetRef() +void DJIMotorSetRef(dji_motor_instance *motor, float ref) { - - + motor->motor_controller.pid_ref = ref; } - void DJIMotorControl() { - + static uint8_t group, num, set; + // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值 for (size_t i = 0; i < DJI_MOTOR_CNT; i++) { - + if (dji_motor_info[i]) + { + // @todo: 计算PID + // calculate pid output + // ... + group = dji_motor_info[i]->sender_group; + num = dji_motor_info[i]->message_num; + set = (int16_t)dji_motor_info[i]->motor_controller.pid_output; + // sender_assignment[group].rx_buff[num]= 0xff & PIDoutPIDoutput>>8; + // sender_assignment[group].rx_buff[num]= 0xff & PIDoutput; + } + else + break; + } + + // 遍历flag,检查是否要发送这一帧报文 + for (size_t i = 0; i < 6; i++) + { + if (sender_enable_flag[i]) + { + CANTransmit(&sender_assignment[i]); + } } - } \ No newline at end of file diff --git a/modules/motor/dji_motor.h b/modules/motor/dji_motor.h index 53a22a1..5d5b57a 100644 --- a/modules/motor/dji_motor.h +++ b/modules/motor/dji_motor.h @@ -1,7 +1,7 @@ #ifndef DJI_MOTOR_H #define DJI_MOTOR_H -#define DJI_MOTOR_CNT 8 +#define DJI_MOTOR_CNT 12 #include "bsp_can.h" #include "controller.h" @@ -9,12 +9,12 @@ /** * @brief DJI intelligent motor typedef - * + * */ typedef struct { /* motor measurement recv from CAN feedback */ - struct + struct { uint16_t ecd; uint16_t last_ecd; @@ -43,17 +43,54 @@ typedef struct } dji_motor_instance; -dji_motor_instance* DJIMotorInit(can_instance_config config, - Motor_Controller_s controller_config, +/** + * @todo 加入ID冲突的检查机制,如果发现注册的ID冲突,进入IDcrash_Handler()的死循环中 + * + * @brief 调用此函数注册一个DJI智能电机,需要传递较多的初始化参数,请在application初始化的时候调用此函数 + * 推荐传参时像标准库一样构造initStructure然后传入此函数. + * recommend: type xxxinitStructure = { + * .member1=xx, + * .member2=xx, + * ....}; + * 请注意不要在一条总线上挂载过多的电机(超过6个),若一定要这么做,请降低每个电机的反馈频率(设为500Hz), + * 并减小DJIMotorControl()任务的运行频率. + * + * @attention 当前并没有对电机的ID冲突进行检查,请保证在注册电机的时候,他们的反馈ID不会产生冲突! + * M3508和M2006的反馈报文都是0x200+id,而GM6020的反馈是0x204+id,请注意前两者和后者的id不要冲突. + * + * @param config 电机can设置,对于DJI电机仅需要将tx_id设置为电调闪动次数(c620,615,610)或拨码开关的值(GM6020) + * 你不需要自己查表计算发送和接收id,我们会帮你处理好! + * + * @param motor_setting 电机的控制设置,包括是否反转,闭环类型和是否使用编码器之外的反馈值 + * + * @param controller_init 电机控制器的参数设置,包括其他的反馈来源数据指针和三环PID的参数. + * 如果不需要其他数据来源或不需要三个环,将不需要指针置为NULL即可 + * + * @param type 电机的类型枚举,包括m2006,m3508和gm6020 + * + * @return dji_motor_instance* 返回一个电机实例指针给应用,方便其对电机的参考值进行设置,即调用DJIMotorSetRef() + */ +dji_motor_instance *DJIMotorInit(can_instance_config config, Motor_Control_Setting_s motor_setting, Motor_Controller_Init_s controller_init, Motor_Type_e type); -void DJIMotorSetRef(); +/** + * @brief 被application层的应用调用,给电机设定参考值. + * 对于应用,可以将电机视为传递函数为1的设备,不需要关心底层的闭环 + * + * @param motor 要设置的电机 + * @param ref 设定参考值 + */ +void DJIMotorSetRef(dji_motor_instance *motor, float ref); + + +/** + * @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率 + * + */ void DJIMotorControl(); - - #endif // !DJI_MOTOR_H diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index 0e437da..10ed417 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -40,6 +40,9 @@ typedef struct PID_t* speed_PID; PID_t* angle_PID; + float pid_ref; + float pid_output; + } Motor_Controller_s; typedef enum