TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 11:37:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

finish all bsp utilities

Browse Source
This commit is contained in:
NeoZng
2022-11-01 22:32:15 +08:00
parent c113ca81e0
commit a2b7558047
13 changed files with 514 additions and 436 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
modules/motor/dji_motor.c
View File

@@ -1,5 +1,8 @@
#include "dji_motor.h"
#define PI2 3.141592f
#define ECD_ANGLE_COEF 3.835e-4 // ecd/8192*pi
static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
@@ -33,7 +36,7 @@ static uint8_t sender_enable_flag[6] = {0};
* @brief 当注册的电机id冲突时,会进入这个函数并提示冲突的ID
* @todo 通过segger jlink 发送日志
*/
static void IDcrash_Handler()
static void IDcrash_Handler(uint8_t conflict_motor_idx, uint8_t temp_motor_idx)
{
while (1)
{
@@ -67,7 +70,7 @@ static void MotorSenderGrouping(can_instance_config *config)
}
// 计算接收id并设置分组发送id
config->rx_id = 0x200 + motor_id+1;
config->rx_id = 0x200 + motor_id + 1;
sender_enable_flag[motor_grouping] = 1;
dji_motor_info[idx]->message_num = motor_send_num;
dji_motor_info[idx]->sender_group = motor_grouping;
@@ -76,7 +79,7 @@ static void MotorSenderGrouping(can_instance_config *config)
for (size_t i = 0; i < idx; i++)
{
if (dji_motor_info[i]->motor_can_instance.can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance.rx_id == config->rx_id)
IDcrash_Handler();
IDcrash_Handler(i, idx);
}
break;
@@ -100,7 +103,7 @@ static void MotorSenderGrouping(can_instance_config *config)
for (size_t i = 0; i < idx; i++)
{
if (dji_motor_info[i]->motor_can_instance.can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance.rx_id == config->rx_id)
IDcrash_Handler();
IDcrash_Handler(i, idx);
}
break;
@@ -110,22 +113,33 @@ static void MotorSenderGrouping(can_instance_config *config)
}
/**
* @todo 是否可以简化多圈角度的计算？
* @brief 根据返回的can_instance对反馈报文进行解析
*
* @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例
*/
static void DecodeDJIMotor(can_instance *_instance)
{
uint8_t *rxbuff = _instance->rx_buff;
static uint8_t *rxbuff;
static dji_motor_measure *measure;
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)(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];
rxbuff = _instance->rx_buff;
measure = &dji_motor_info[i]->motor_measure;
// resolve data and apply filter to current and speed
measure->last_ecd = measure->ecd;
measure->ecd = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]);
measure->speed_rpm = (1 - SPEED_SMOOTH_COEF) * measure->speed_rpm + SPEED_SMOOTH_COEF * (int16_t)(rxbuff[2] << 8 | rxbuff[3]);
measure->given_current = (1 - CURRENT_SMOOTH_COEF) * measure->given_current + CURRENT_SMOOTH_COEF * (uint16_t)(rxbuff[4] << 8 | rxbuff[5]);
measure->temperate = rxbuff[6];
// multi round calc
if (measure->ecd - measure->last_ecd > 4096)
measure->total_round--;
else if (measure->ecd - measure->last_ecd < -4096)
measure->total_round++;
measure->total_angle = measure->total_round * PI2 + measure->ecd * ECD_ANGLE_COEF; // @todo simplify the calculation
break;
}
}
@@ -138,6 +152,7 @@ dji_motor_instance *DJIMotorInit(can_instance_config can_config,
Motor_Type_e type)
{
dji_motor_info[idx] = (dji_motor_instance *)malloc(sizeof(dji_motor_instance));
memset(dji_motor_info[idx], 0, sizeof(dji_motor_instance));
// motor basic setting
dji_motor_info[idx]->motor_type = type;
@@ -159,15 +174,15 @@ dji_motor_instance *DJIMotorInit(can_instance_config can_config,
}
// 改变反馈来源
void DJIMotorChangeFeed(dji_motor_instance *motor,Closeloop_Type_e loop,Feedback_Source_e type)
void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedback_Source_e type)
{
if(loop==ANGLE_LOOP)
if (loop == ANGLE_LOOP)
{
motor->motor_settings.angle_feedback_source=type;
motor->motor_settings.angle_feedback_source = type;
}
if(loop==SPEED_LOOP)
if (loop == SPEED_LOOP)
{
motor->motor_settings.speed_feedback_source=type;
motor->motor_settings.speed_feedback_source = type;
}
}
@@ -176,14 +191,13 @@ void DJIMotorSetRef(dji_motor_instance *motor, float ref)
{
motor->motor_controller.pid_ref = ref;
}
// 计算三环PID,发送控制报文
void DJIMotorControl()
{
{
// 预先通过静态变量定义避免反复释放分配栈空间,直接保存一次指针引用从而减小访存的开销
static uint8_t group, num;
static int16_t set;
static dji_motor_instance* motor;
static dji_motor_instance *motor;
static Motor_Control_Setting_s *motor_setting;
static Motor_Controller_s *motor_controller;
static dji_motor_measure *motor_measure;
@@ -193,33 +207,33 @@ void DJIMotorControl()
{
if (dji_motor_info[i])
{
motor=dji_motor_info[i];
motor_setting=&motor->motor_settings;
motor_controller=&motor->motor_controller;
motor_measure=&motor->motor_measure;
motor = dji_motor_info[i];
motor_setting = &motor->motor_settings;
motor_controller = &motor->motor_controller;
motor_measure = &motor->motor_measure;
if (motor_setting->close_loop_type & ANGLE_LOOP) // 计算位置环
{
if (motor_setting->angle_feedback_source == OTHER_FEED)
pid_measure=*motor_controller->other_angle_feedback_ptr;
pid_measure = *motor_controller->other_angle_feedback_ptr;
else // MOTOR_FEED
pid_measure=motor_measure->total_angle;
pid_measure = motor_measure->total_angle;
// 更新pid_ref进入下一个环
motor_controller->pid_ref=PID_Calculate(&motor_controller->angle_PID,pid_measure,motor_controller->pid_ref);
motor_controller->pid_ref = PID_Calculate(&motor_controller->angle_PID, pid_measure, motor_controller->pid_ref);
}
if (motor_setting->close_loop_type & SPEED_LOOP) // 计算速度环
{
if (motor_setting->speed_feedback_source == OTHER_FEED)
pid_measure=*motor_controller->other_speed_feedback_ptr;
pid_measure = *motor_controller->other_speed_feedback_ptr;
else
pid_measure=motor_measure->speed_rpm;
motor_controller->pid_ref=PID_Calculate(&motor_controller->speed_PID,pid_measure,motor_controller->pid_ref);
pid_measure = motor_measure->speed_rpm;
motor_controller->pid_ref = PID_Calculate(&motor_controller->speed_PID, pid_measure, motor_controller->pid_ref);
}
if (motor_setting->close_loop_type & CURRENT_LOOP) //计算电流环
{
motor_controller->pid_ref=PID_Calculate(&motor_controller->current_PID,motor_measure->given_current,motor_controller->pid_ref);
motor_controller->pid_ref = PID_Calculate(&motor_controller->current_PID, motor_measure->given_current, motor_controller->pid_ref);
}
set = (int16_t)motor_controller->pid_ref;
@@ -230,7 +244,7 @@ void DJIMotorControl()
group = motor->sender_group;
num = motor->message_num;
sender_assignment[group].tx_buff[num] = 0xff & set >> 8;
sender_assignment[group].tx_buff[num+1] = 0xff & set;
sender_assignment[group].tx_buff[num + 1] = 0xff & set;
}
else // 遇到空指针说明所有遍历结束,退出循环
break;

38
modules/motor/dji_motor.h
View File

@@ -1,7 +1,20 @@
/**
* @file dji_motor.h
* @author neozng
* @brief DJI智能电机头文件
* @version 0.1
* @date 2022-11-01
*
* @copyright Copyright (c) 2022 HNU YueLu EC all rights reserved
*
*/
#ifndef DJI_MOTOR_H
#define DJI_MOTOR_H
#define DJI_MOTOR_CNT 12
#define SPEED_SMOOTH_COEF 0.85f // better to be greater than 0.8
#define CURRENT_SMOOTH_COEF 0.98f // this coef must be greater than 0.95
#include "bsp_can.h"
#include "controller.h"
@@ -45,7 +58,6 @@ typedef struct
} dji_motor_instance;
/**
* @brief 调用此函数注册一个DJI智能电机,需要传递较多的初始化参数,请在application初始化的时候调用此函数
* 推荐传参时像标准库一样构造initStructure然后传入此函数.
@@ -55,20 +67,20 @@ typedef struct
* ....};
* 请注意不要在一条总线上挂载过多的电机(超过6个),若一定要这么做,请降低每个电机的反馈频率(设为500Hz),
* 并减小DJIMotorControl()任务的运行频率.
*
*
* @attention M3508和M2006的反馈报文都是0x200+id,而GM6020的反馈是0x204+id,请注意前两者和后者的id不要冲突.
* 如果产生冲突,在初始化电机的时候会进入IDcrash_Handler(),可以通过debug来判断是否出现冲突.
*
*
* @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,
@@ -76,32 +88,28 @@ dji_motor_instance *DJIMotorInit(can_instance_config config,
Motor_Controller_Init_s controller_init,
Motor_Type_e type);
/**
* @brief 被application层的应用调用,给电机设定参考值.
* 对于应用,可以将电机视为传递函数为1的设备,不需要关心底层的闭环
*
*
* @param motor 要设置的电机
* @param ref 设定参考值
*/
void DJIMotorSetRef(dji_motor_instance *motor, float ref);
/**
* @brief 切换反馈的目标来源,如将角速度和角度的来源换为IMU(小陀螺模式常用)
*
*
* @param motor 要切换反馈数据来源的电机
* @param loop 要切换反馈数据来源的控制闭环
* @param type 目标反馈模式
*/
void DJIMotorChangeFeed(dji_motor_instance *motor,Closeloop_Type_e loop,Feedback_Source_e type);
void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedback_Source_e type);
/**
* @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率
* @todo 增加前馈功能
* @todo 增加前馈功能
*/
void DJIMotorControl();
#endif // !DJI_MOTOR_H

28
modules/motor/motor_def.h
View File

@@ -1,6 +1,21 @@
/**
* @file motor_def.h
* @author neozng
* @brief 电机通用的数据结构定义
* @version beta
* @date 2022-11-01
*
* @copyright Copyright (c) 2022 HNU YueLu EC all rights reserved
*
*/
#ifndef MOTOR_DEF_H
#define MOTOR_DEF_H
/**
* @brief 闭环类型,如果需要多个闭环,则使用或运算
* 例如需要速度环和电流环: CURRENT_LOOP|SPEED_LOOP
*/
typedef enum
{
CURRENT_LOOP = 0b0001,
@@ -13,18 +28,21 @@ typedef enum
___ = 0b0111
} Closeloop_Type_e;
/* 反馈来源设定,若设为OTHER_FEED则需要指定数据来源指针,详见Motor_Controller_s*/
typedef enum
{
MOTOR_FEED = 0,
OTHER_FEED = 1
} Feedback_Source_e;
/* 电机正反转标志 */
typedef enum
{
MOTOR_DIRECTION_NORMAL = 0,
MOTOR_DIRECTION_REVERSE = 1
} Reverse_Flag_e;
/* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
typedef struct
{
Closeloop_Type_e close_loop_type;
@@ -34,6 +52,7 @@ typedef struct
} Motor_Control_Setting_s;
/* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/
typedef struct
{
float *other_angle_feedback_ptr;
@@ -43,10 +62,10 @@ typedef struct
PID_t speed_PID;
PID_t angle_PID;
// 将会作为每个环的输入和输出
float pid_ref;
float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环
} Motor_Controller_s;
/* 电机类型枚举 */
typedef enum
{
GM6020 = 0,
@@ -56,6 +75,11 @@ typedef enum
HT04 = 4
} Motor_Type_e;
/**
* @brief 电机控制器初始化结构体,包括三环PID的配置以及两个反馈数据来源指针
* 如果不需要某个控制环,可以不设置对应的pid config
* 需要其他数据来源进行反馈闭环,不仅要设置这里的指针还需要在Motor_Control_Setting_s启用其他数据来源标志
*/
typedef struct
{
float *other_angle_feedback_ptr;

10
modules/motor/motor_task.h
View File

@@ -1,3 +1,13 @@
/**
* @file motor_task.h
* @author neozng
* @brief 对所有电机,舵机等控制任务的进一步封装,MotorControlTask()将在操作系统中按一定频率调用
* @version beta
* @date 2022-11-01
*
* @copyright Copyright (c) 2022
*
*/
#ifndef MOTOR_TASK_H
#define MOTOR_TASK_H