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

14
bsp/bsp_can.c
View File

@ -6,6 +6,8 @@
/* can instance ptrs storage, used for recv callback */
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()
*
@ -53,9 +55,7 @@ static void CANServiceInit()
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);
}
/* -----------------------two callable function -----------------------*/
/* -----------------------two extern callable function -----------------------*/
can_instance *CANRegister(can_instance_config config)
{
@ -81,17 +81,15 @@ can_instance* CANRegister(can_instance_config config)
return instance[idx++];
}
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
@ -115,7 +113,6 @@ static void CANFIFOxCallback(CAN_HandleTypeDef* _hcan,uint32_t fifox)
}
}
/* ATTENTION: two CAN devices in STM32 share two FIFOs */
/* functions below will call CANFIFOxCallback() to further process message from a specific CAN device */
/**
@ -128,7 +125,6 @@ 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
*

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

121
modules/motor/dji_motor.c
View File

@ -1,9 +1,16 @@
#include "dji_motor.h"
static uint8_t idx = 0; // register idx
/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
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
/**
* @brief 由于DJI电机发送以四个一组的形式进行,故对其进行特殊处理,用6个(2can*3group)can_instance专门负责发送
* 该变量将在 DJIMotorControl() 中使用,分组在 MotorSenderGrouping()中进行
*
* 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}},
@ -14,99 +21,153 @@ static can_instance sender_assignment[6]=
[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
* @brief 根据电调/拨码开关上的ID,计算发送ID和接收ID,并对电机进行分组以便处理多电机控制命令
*
* @param idx
* @param config
*/
static void MotorSenderGrouping(uint8_t idx,can_instance_config 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)
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:
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 ? 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;
}
}
/**
* @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)
{
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.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,
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));
// 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;
// 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(idx,config);
MotorSenderGrouping(&config);
// register motor to CAN bus
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]);
}
}
}

47
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"
@ -43,17 +43,54 @@ typedef struct
} dji_motor_instance;
/**
* @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_Controller_s controller_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