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"
/* 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);
}

14
bsp/bsp_usart.c
View File

@ -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);
}
/**

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;
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)

2
modules/motor/LK9025.h
View File

@ -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);

199
modules/motor/dji_motor.c
View File

@ -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]);
}
}
}

53
modules/motor/dji_motor.h
View File

@ -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

3
modules/motor/motor_def.h
View File

@ -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