HFUT_Firmament_EC
/
sentry_chassis_hzz
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

finish basic dji motor

This commit is contained in:
NeoZng 2022-10-31 12:14:47 +08:00
parent 6441982964
commit 3dd4f1066c
7 changed files with 226 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
bsp/bsp_can.c
View File

@ -4,33 +4,35 @@
#include "memory.h" #include "memory.h"
/* can instance ptrs storage, used for recv callback */ /* 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() * @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. * @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 * 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 * 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 * 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 * @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! * for initialization.Enjoy developing without caring about the infrastructure!
* if you really want to know what is happeng, contact author. * if you really want to know what is happeng, contact author.
* *
* @param _instance can instance owned by specific module * @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; 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.FilterMode = CAN_FILTERMODE_IDLIST;
can_filter_conf.FilterScale = CAN_FILTERSCALE_16BIT; can_filter_conf.FilterScale = CAN_FILTERSCALE_16BIT;
can_filter_conf.FilterFIFOAssignment = (_instance->rx_id & 1) ? CAN_RX_FIFO0 : CAN_RX_FIFO1; can_filter_conf.FilterFIFOAssignment = (_instance->rx_id & 1) ? CAN_RX_FIFO0 : CAN_RX_FIFO1;
can_filter_conf.SlaveStartFilterBank = 14; can_filter_conf.SlaveStartFilterBank = 14;
can_filter_conf.FilterIdLow = _instance->rx_id << 5; 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; can_filter_conf.FilterActivation = CAN_FILTER_ENABLE;
HAL_CAN_ConfigFilter(_instance->can_handle, &can_filter_conf); 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 * @brief called by CANRegister before the first module being registered
* *
* @note this func will handle all these thing automatically * @note this func will handle all these thing automatically
* there is no need to worry about hardware initialization, we do these for you! * there is no need to worry about hardware initialization, we do these for you!
* *
*/ */
static void CANServiceInit() static void CANServiceInit()
{ {
@ -53,88 +55,82 @@ static void CANServiceInit()
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING); 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; static uint8_t idx;
if(!idx) if (!idx)
{ {
CANServiceInit(); 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.StdId = config.tx_id;
instance[idx]->txconf.IDE=CAN_ID_STD; instance[idx]->txconf.IDE = CAN_ID_STD;
instance[idx]->txconf.RTR=CAN_RTR_DATA; instance[idx]->txconf.RTR = CAN_RTR_DATA;
instance[idx]->txconf.DLC=0x08; instance[idx]->txconf.DLC = 0x08;
instance[idx]->can_handle=config.can_handle; instance[idx]->can_handle = config.can_handle;
instance[idx]->tx_id=config.tx_id; instance[idx]->tx_id = config.tx_id;
instance[idx]->rx_id=config.rx_id; instance[idx]->rx_id = config.rx_id;
instance[idx]->can_module_callback=config.can_module_callback; instance[idx]->can_module_callback = config.can_module_callback;
CANAddFilter(instance[idx]); CANAddFilter(instance[idx]);
return 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); HAL_CAN_AddTxMessage(_instance->can_handle, &_instance->txconf, _instance->tx_buff, &_instance->tx_mailbox);
} }
/* -----------------------belows are callback definitions--------------------------*/ /* -----------------------belows are callback definitions--------------------------*/
/** /**
* @brief this func will recv data from @param:fifox to a tmp can_rx_buff * @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 * 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 * @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]; uint8_t can_rx_buff[8];
CAN_RxHeaderTypeDef rxconf; 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++) 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]); instance[i]->can_module_callback(instance[i]);
break; break;
} }
} }
} }
/* ATTENTION: two CAN devices in STM32 share two FIFOs */ /* ATTENTION: two CAN devices in STM32 share two FIFOs */
/* functions below will call CANFIFOxCallback() to further process message from a specific CAN device */ /* 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 * @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 * @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); CANFIFOxCallback(hcan, CAN_RX_FIFO0);
} }
/** /**
* @brief rx fifo callback. Once FIFO_1 is full,this func would be called * @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 * @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); CANFIFOxCallback(hcan, CAN_RX_FIFO1);
} }

14
bsp/bsp_usart.c
View File

@ -2,29 +2,29 @@
#include "stdlib.h" #include "stdlib.h"
/* usart service instance,modules' info would be recoreded here using ModuleRegister() */ /* 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 * @brief usart service will start automatically, after each module registered
* *
* @param _instance instance owned by module * @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_UARTEx_ReceiveToIdle_DMA(_instance->usart_handle, _instance->recv_buff, _instance->recv_buff_size);
__HAL_DMA_DISABLE_IT(_instance->usart_handle->hdmarx, DMA_IT_HT); __HAL_DMA_DISABLE_IT(_instance->usart_handle->hdmarx, DMA_IT_HT);
} }
void USARTRegister(usart_instance* _instance) void USARTRegister(usart_instance *_instance)
{ {
static instance_idx; static instance_idx;
USARTServiceInit(_instance); 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);
} }
/** /**

5
modules/motor/LK9025.c
View File

@ -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; static uint8_t idx;
driven_motor_info[idx]=(driven_instance*)malloc(sizeof(driven_instance)); 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) void DrivenControl(int16_t motor1_current,int16_t motor2_current)

2
modules/motor/LK9025.h
View File

@ -30,7 +30,7 @@ typedef enum
unused = 0, unused = 0,
} driven_mode; } 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); void DrivenControl(int16_t motor1_current,int16_t motor2_current);

199
modules/motor/dji_motor.c
View File

@ -1,112 +1,173 @@
#include "dji_motor.h" #include "dji_motor.h"
static dji_motor_instance* dji_motor_info[DJI_MOTOR_CNT]={NULL}; static uint8_t idx = 0; // register idx
/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
// can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL};
// 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 * @brief 由于DJI电机发送以四个一组的形式进行,故对其进行特殊处理,用6个(2can*3group)can_instance专门负责发送
* * 该变量将在 DJIMotorControl() 中使用,分组在 MotorSenderGrouping()中进行
* @param idx *
* 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_rx_id;
uint8_t motor_send_num; uint8_t motor_send_num;
uint8_t motor_grouping; uint8_t motor_grouping;
switch (dji_motor_info[idx]->motor_type) switch (dji_motor_info[idx]->motor_type)
{ {
case M2006: case M2006:
case M3508: case M3508:
if(motor_id<5) 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;
} case GM6020:
else if (motor_id < 4)
{ {
dji_motor_info[idx]->message_num = motor_id;
} dji_motor_info[idx]->sender_group = config->can_handle == &hcan1 ? 0 : 3;
break; }
else
case GM6020: {
if(motor_id<5) 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;
else sender_enable_flag[dji_motor_info[idx]->sender_group] = 1;
{ break;
// other motors should not be registered here
} default:
break; 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++) 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.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.ecd = (uint16_t)(rxbuff[0] << 8 | rxbuff[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.speed_rpm = (uint16_t)(rxbuff[2] << 8 | rxbuff[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.given_current = (uint16_t)(rxbuff[4] << 8 | rxbuff[5]);
dji_motor_info[i]->motor_measure.temperate = _instance->rx_buff[6]; dji_motor_info[i]->motor_measure.temperate = rxbuff[6];
break; break;
} }
} }
} }
dji_motor_instance *DJIMotorInit(can_instance_config config,
dji_motor_instance* DJIMotorInit(can_instance_config config,
Motor_Controller_s controller_config,
Motor_Control_Setting_s motor_setting, Motor_Control_Setting_s motor_setting,
Motor_Controller_Init_s controller_init, Motor_Controller_Init_s controller_init,
Motor_Type_e type) 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 // motor setting
dji_motor_info[idx]->motor_type=type; dji_motor_info[idx]->motor_type = type;
dji_motor_info[idx]->motor_settings=motor_setting; dji_motor_info[idx]->motor_settings = motor_setting;
// motor controller init @todo : PID init // motor controller init
dji_motor_info[idx]->motor_settings.angle_feedback_source=motor_setting.angle_feedback_source; // @todo : PID init
dji_motor_info[idx]->motor_settings.speed_feedback_source=motor_setting.speed_feedback_source; dji_motor_info[idx]->motor_controller.other_angle_feedback_ptr = controller_init.other_angle_feedback_ptr;
// group motors, because 4 motors share the same CAN control message dji_motor_info[idx]->motor_controller.other_speed_feedback_ptr = controller_init.other_speed_feedback_ptr;
MotorSenderGrouping(idx,config); // group motors, because 4 motors share the same CAN control message
MotorSenderGrouping(&config);
// register motor to CAN bus // 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++]; return dji_motor_info[idx++];
} }
void DJIMotorSetRef(dji_motor_instance *motor, float ref)
void DJIMotorSetRef()
{ {
motor->motor_controller.pid_ref = ref;
} }
void DJIMotorControl() void DJIMotorControl()
{ {
static uint8_t group, num, set;
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
for (size_t i = 0; i < DJI_MOTOR_CNT; i++) 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]);
}
} }
} }

53
modules/motor/dji_motor.h
View File

@ -1,7 +1,7 @@
#ifndef DJI_MOTOR_H #ifndef DJI_MOTOR_H
#define DJI_MOTOR_H #define DJI_MOTOR_H
#define DJI_MOTOR_CNT 8 #define DJI_MOTOR_CNT 12
#include "bsp_can.h" #include "bsp_can.h"
#include "controller.h" #include "controller.h"
@ -9,12 +9,12 @@
/** /**
* @brief DJI intelligent motor typedef * @brief DJI intelligent motor typedef
* *
*/ */
typedef struct typedef struct
{ {
/* motor measurement recv from CAN feedback */ /* motor measurement recv from CAN feedback */
struct struct
{ {
uint16_t ecd; uint16_t ecd;
uint16_t last_ecd; uint16_t last_ecd;
@ -43,17 +43,54 @@ typedef struct
} dji_motor_instance; } 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_Control_Setting_s motor_setting,
Motor_Controller_Init_s controller_init, Motor_Controller_Init_s controller_init,
Motor_Type_e type); 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(); void DJIMotorControl();
#endif // !DJI_MOTOR_H #endif // !DJI_MOTOR_H

3
modules/motor/motor_def.h
View File

@ -40,6 +40,9 @@ typedef struct
PID_t* speed_PID; PID_t* speed_PID;
PID_t* angle_PID; PID_t* angle_PID;
float pid_ref;
float pid_output;
} Motor_Controller_s; } Motor_Controller_s;
typedef enum typedef enum