finish all bsp utilities
This commit is contained in:
NeoZng
parent
c113ca81e0
commit
a2b7558047
|
|
@ -1,17 +1,30 @@
|
|||
/**
|
||||
* @file bsp_usart.c
|
||||
* @author neozng
|
||||
* @brief 串口bsp层的实现
|
||||
* @version beta
|
||||
* @date 2022-11-01
|
||||
*
|
||||
* @copyright Copyright (c) 2022
|
||||
*
|
||||
*/
|
||||
#include "bsp_usart.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 USARTRegister() */
|
||||
/* usart服务实例,所有注册了usart的模块信息会被保存在这里 */
|
||||
static usart_instance *instance[DEVICE_USART_CNT];
|
||||
|
||||
/**
|
||||
* @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)
|
||||
{
|
||||
HAL_UARTEx_ReceiveToIdle_DMA(_instance->usart_handle, _instance->recv_buff, _instance->recv_buff_size);
|
||||
// 关闭dma half transfer中断防止两次进入HAL_UARTEx_RxEventCallback()
|
||||
__HAL_DMA_DISABLE_IT(_instance->usart_handle->hdmarx, DMA_IT_HT);
|
||||
}
|
||||
|
||||
|
|
@ -22,21 +35,21 @@ void USARTRegister(usart_instance *_instance)
|
|||
instance[instance_idx++] = _instance;
|
||||
}
|
||||
|
||||
/* @todo 当前仅进行了形式上的封装,后续要进一步考虑是否将module的行为与bsp完全分离 */
|
||||
void USARTSend(usart_instance *_instance, uint8_t *send_buf, uint16_t send_size)
|
||||
{
|
||||
HAL_UART_Transmit_DMA(_instance->usart_handle, send_buf, send_size);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief everytiem when dma/idle happens,this function will be called
|
||||
* here, for each uart, specific callback is refered for further process
|
||||
* etc:visual protocol resolve/remote control resolve/referee protocol resolve
|
||||
* @brief 每次dma/idle中断发生时，都会调用此函数.对于每个uart实例会调用对应的回调进行进一步的处理
|
||||
* 例如:视觉协议解析/遥控器解析/裁判系统解析
|
||||
*
|
||||
* @note because DMA half transfer iterrupt(DMA_IT_HT) would call this function too, so we just
|
||||
* @note because DMA half transfer iterrupt(DMA_IT_HT) would call this callback function too, so we just
|
||||
* disable it when transfer complete using macro: __HAL_DMA_DISABLE_IT(huart->hdmarx,DMA_IT_HT)
|
||||
*
|
||||
* @param huart uart handle indicate which uart is being handled
|
||||
* @param Size not used temporarily
|
||||
* @param huart uart handle indicate which uart is being handled 发生中断的串口
|
||||
* @param Size not used temporarily,此次接收到的总数居量,暂时没用
|
||||
*/
|
||||
void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
|
||||
{
|
||||
|
|
@ -58,7 +71,7 @@ void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
|
|||
*
|
||||
* @note most frequent error ex: parity/overrrun/frame error
|
||||
*
|
||||
* @param huart uart handle type,indicate where error comes from
|
||||
* @param huart uart handle type, indicate where error comes from
|
||||
*/
|
||||
void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -4,41 +4,38 @@
|
|||
#include "struct_typedef.h"
|
||||
#include "main.h"
|
||||
|
||||
#define DEVICE_USART_CNT 3
|
||||
#define USART_RX_LIMIT 128 // if your protocol needs bigger buff, modify here
|
||||
#define DEVICE_USART_CNT 3 // C板至多分配3个串口
|
||||
#define USART_RXBUFF_LIMIT 128 // if your protocol needs bigger buff, modify here
|
||||
|
||||
/* application callback,which resolves specific protocol */
|
||||
/* application callback,which resolves specific protocol,解析协议的回调函数 */
|
||||
typedef void (*usart_module_callback)();
|
||||
|
||||
/* usart_instance struct,each app would have one instance */
|
||||
typedef struct
|
||||
{
|
||||
uint8_t recv_buff[USART_RX_LIMIT];
|
||||
uint8_t recv_buff_size;
|
||||
UART_HandleTypeDef *usart_handle;
|
||||
usart_module_callback module_callback;
|
||||
uint8_t recv_buff[USART_RXBUFF_LIMIT]; // 预先定义的最大buff大小,如果太小请修改USART_RXBUFF_LIMIT
|
||||
uint8_t recv_buff_size; // 模块接收一包数据的大小
|
||||
UART_HandleTypeDef *usart_handle; // 实例对应的usart_handle
|
||||
usart_module_callback module_callback; // 解析收到的数据的回调函数
|
||||
} usart_instance;
|
||||
|
||||
/**
|
||||
* @brief calling this func would regiter a module to usart_service
|
||||
* this function provides completely wrap and abstract for those module who use usart as commu method
|
||||
*
|
||||
* @param id module id.attention,each id is mapped to a specific usart port(ex:remote control->usart3)
|
||||
* @param prbuff pointer to where the raw recv data stored
|
||||
* @param rbuffsize size of recv data buff
|
||||
* @param psbuff pointer to where the data to be sent stored
|
||||
* @param callback func pointer,this func would be called in USART_service.c/HAL_UARTEx_RxEventCallback()
|
||||
* to resolve raw data using protocol provides by specific app calling Moduleregister()
|
||||
* @brief 注册一个串口实例.
|
||||
*
|
||||
* @param _instance 传入一个由module拥有的串口实例,注意在传入前进行基本信息的配置,详见usart_instance的struct定义
|
||||
*/
|
||||
void USARTRegister(usart_instance* _instance);
|
||||
void USARTRegister(usart_instance *_instance);
|
||||
|
||||
/**
|
||||
* @todo 是否需要进一步封装发送buff和size,并创建一个串口任务以一定频率自动发送?
|
||||
* 若采用此方法,则串口实例的拥有者仅需要在自己的任务中设置发送值,不需要关心发送buffer大小以及何时发送.
|
||||
*
|
||||
* @brief api for sending data through a specific serial port,indicated by the first parameter:id
|
||||
* 通过调用该函数可以发送一帧数据,需要传入一个usart实例,发送buff以及这一帧的长度
|
||||
*
|
||||
* @param id specify which usart would be used
|
||||
* @param send_size how many bytes to send
|
||||
*/
|
||||
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);
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,37 +1,38 @@
|
|||
|
||||
/**
|
||||
* @file controller.c
|
||||
* @author wanghongxi
|
||||
* @author modified by neozng
|
||||
* @brief PID控制器定义
|
||||
* @version beta
|
||||
* @date 2022-11-01
|
||||
*
|
||||
* @copyrightCopyright (c) 2022 HNU YueLu EC all rights reserved
|
||||
*/
|
||||
#include "controller.h"
|
||||
#include <memory.h>
|
||||
|
||||
/******************************* PID CONTROL *********************************/
|
||||
// PID优化环节函数声明
|
||||
static void f_Trapezoid_Intergral(PID_t *pid);
|
||||
static void f_Integral_Limit(PID_t *pid);
|
||||
static void f_Derivative_On_Measurement(PID_t *pid);
|
||||
static void f_Changing_Integration_Rate(PID_t *pid);
|
||||
static void f_Output_Filter(PID_t *pid);
|
||||
static void f_Derivative_Filter(PID_t *pid);
|
||||
static void f_Output_Limit(PID_t *pid);
|
||||
static void f_Proportion_Limit(PID_t *pid);
|
||||
static void f_PID_ErrorHandle(PID_t *pid);
|
||||
static void f_Trapezoid_Intergral(PID_t *pid); // 梯形积分
|
||||
static void f_Integral_Limit(PID_t *pid); // 积分限幅
|
||||
static void f_Derivative_On_Measurement(PID_t *pid); // 微分先行(仅使用反馈微分)
|
||||
static void f_Changing_Integration_Rate(PID_t *pid); // 变速积分
|
||||
static void f_Output_Filter(PID_t *pid); // 输出滤波
|
||||
static void f_Derivative_Filter(PID_t *pid); // 微分滤波(采集时)
|
||||
static void f_Output_Limit(PID_t *pid); // 输出限幅
|
||||
static void f_PID_ErrorHandle(PID_t *pid); // 堵转保护
|
||||
|
||||
/**
|
||||
* @brief
|
||||
* @brief 初始化PID,设置参数和启用的优化环节,将其他数据置零
|
||||
*
|
||||
* @param pid
|
||||
* @param config
|
||||
* @param pid PID实例
|
||||
* @param config PID初始化设置
|
||||
*/
|
||||
void PID_Init(PID_t* pid,PID_Init_config_s *config)
|
||||
void PID_Init(PID_t *pid, PID_Init_config_s *config)
|
||||
{
|
||||
// utilize the quality of struct that its memeory is continuous
|
||||
memcpy(pid, config, sizeof(PID_Init_config_s));
|
||||
memset(&pid->Measure,0,sizeof(PID_t)-sizeof(PID_Init_config_s));
|
||||
|
||||
// // DWT定时器计数变量清零
|
||||
// // reset DWT Timer count counter
|
||||
// pid->DWT_CNT = 0;
|
||||
|
||||
// // 设置PID异常处理 目前仅包含电机堵转保护
|
||||
// pid->ERRORHandler.ERRORCount = 0;
|
||||
// pid->ERRORHandler.ERRORType = PID_ERROR_NONE;
|
||||
// set rest of memory to 0
|
||||
memset(&pid->Measure, 0, sizeof(PID_t) - sizeof(PID_Init_config_s));
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -13,7 +13,6 @@
|
|||
#ifndef _CONTROLLER_H
|
||||
#define _CONTROLLER_H
|
||||
|
||||
|
||||
#include "main.h"
|
||||
#include "stdint.h"
|
||||
#include "string.h"
|
||||
|
|
@ -26,21 +25,21 @@
|
|||
#define abs(x) ((x > 0) ? x : -x)
|
||||
#endif
|
||||
|
||||
|
||||
/******************************* PID CONTROL *********************************/
|
||||
// PID 优化环节使能标志位
|
||||
typedef enum pid_Improvement_e
|
||||
{
|
||||
NONE = 0X00, //0000 0000
|
||||
Integral_Limit = 0x01, //0000 0001
|
||||
Derivative_On_Measurement = 0x02, //0000 0010
|
||||
Trapezoid_Intergral = 0x04, //0000 0100
|
||||
Proportional_On_Measurement = 0x08, //0000 1000
|
||||
OutputFilter = 0x10, //0001 0000
|
||||
ChangingIntegrationRate = 0x20, //0010 0000
|
||||
DerivativeFilter = 0x40, //0100 0000
|
||||
ErrorHandle = 0x80, //1000 0000
|
||||
NONE = 0b00000000, // 0000 0000
|
||||
Integral_Limit = 0b00000001, // 0000 0001
|
||||
Derivative_On_Measurement = 0b00000010, // 0000 0010
|
||||
Trapezoid_Intergral = 0b00000100, // 0000 0100
|
||||
Proportional_On_Measurement = 0b00001000, // 0000 1000
|
||||
OutputFilter = 0b00010000, // 0001 0000
|
||||
ChangingIntegrationRate = 0b00100000, // 0010 0000
|
||||
DerivativeFilter = 0b01000000, // 0100 0000
|
||||
ErrorHandle = 0b10000000, // 1000 0000
|
||||
} PID_Improvement_e;
|
||||
|
||||
/* PID 报错类型枚举*/
|
||||
typedef enum errorType_e
|
||||
{
|
||||
PID_ERROR_NONE = 0x00U,
|
||||
|
|
@ -53,9 +52,10 @@ typedef struct
|
|||
ErrorType_e ERRORType;
|
||||
} PID_ErrorHandler_t;
|
||||
|
||||
/* PID结构体 */
|
||||
typedef struct
|
||||
{
|
||||
//---------------------------------- init config block
|
||||
//---------------------------------- init config block
|
||||
// config parameter
|
||||
float Kp;
|
||||
float Ki;
|
||||
|
|
@ -64,13 +64,13 @@ typedef struct
|
|||
float MaxOut;
|
||||
float IntegralLimit;
|
||||
float DeadBand;
|
||||
float CoefA; //For Changing Integral
|
||||
float CoefB; //ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
|
||||
float CoefA; // For Changing Integral
|
||||
float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
|
||||
float Output_LPF_RC; // RC = 1/omegac
|
||||
float Derivative_LPF_RC;
|
||||
|
||||
uint8_t Improve;
|
||||
//-----------------------------------
|
||||
//-----------------------------------
|
||||
// for calculating
|
||||
float Measure;
|
||||
float Last_Measure;
|
||||
|
|
@ -106,8 +106,8 @@ typedef struct
|
|||
float MaxOut;
|
||||
float IntegralLimit;
|
||||
float DeadBand;
|
||||
float CoefA; //For Changing Integral
|
||||
float CoefB; //ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
|
||||
float CoefA; // For Changing Integral
|
||||
float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
|
||||
float Output_LPF_RC; // RC = 1/omegac
|
||||
float Derivative_LPF_RC;
|
||||
|
||||
|
|
@ -115,32 +115,22 @@ typedef struct
|
|||
|
||||
} PID_Init_config_s;
|
||||
|
||||
/**
|
||||
* @brief 初始化PID实例
|
||||
*
|
||||
* @param pid PID实例指针
|
||||
* @param config PID初始化配置
|
||||
*/
|
||||
void PID_Init(PID_t *pid, PID_Init_config_s *config);
|
||||
|
||||
|
||||
void PID_Init(PID_t* pid,PID_Init_config_s* config);
|
||||
/**
|
||||
* @brief 计算PID输出
|
||||
*
|
||||
* @param pid PID实例指针
|
||||
* @param measure 反馈值
|
||||
* @param ref 设定值
|
||||
* @return float PID计算输出
|
||||
*/
|
||||
float PID_Calculate(PID_t *pid, float measure, float ref);
|
||||
|
||||
|
||||
/*************************** Tracking Differentiator ***************************/
|
||||
typedef struct
|
||||
{
|
||||
float Input;
|
||||
|
||||
float h0;
|
||||
float r;
|
||||
|
||||
float x;
|
||||
float dx;
|
||||
float ddx;
|
||||
|
||||
float last_dx;
|
||||
float last_ddx;
|
||||
|
||||
uint32_t DWT_CNT;
|
||||
float dt;
|
||||
} TD_t;
|
||||
|
||||
void TD_Init(TD_t *td, float r, float h0);
|
||||
float TD_Calculate(TD_t *td, float input);
|
||||
|
||||
#endif
|
||||
|
|
@ -16,7 +16,7 @@
|
|||
#include "bsp_temperature.h"
|
||||
#include "spi.h"
|
||||
|
||||
INS_t INS;
|
||||
static INS_t INS;
|
||||
IMU_Param_t IMU_Param;
|
||||
PID_t TempCtrl = {0};
|
||||
|
||||
|
|
@ -31,6 +31,11 @@ float RefTemp = 40;
|
|||
|
||||
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);
|
||||
|
||||
attitude_t *GetINSptr()
|
||||
{
|
||||
return (attitude_t *)&INS.Roll;
|
||||
}
|
||||
|
||||
void INS_Init(void)
|
||||
{
|
||||
// while (BMI088_init(&hspi1, 1) != BMI088_NO_ERROR);
|
||||
|
|
@ -51,7 +56,7 @@ void INS_Init(void)
|
|||
.Ki = 20,
|
||||
.Kd = 0,
|
||||
.Improve = 0x01}; // enable integratiaon limit
|
||||
PID_Init(&TempCtrl,&config);
|
||||
PID_Init(&TempCtrl, &config);
|
||||
|
||||
// noise of accel is relatively big and of high freq,thus lpf is used
|
||||
INS.AccelLPF = 0.0085;
|
||||
|
|
|
|||
|
|
@ -23,6 +23,14 @@
|
|||
|
||||
#define INS_TASK_PERIOD 1
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float Roll;
|
||||
float Pitch;
|
||||
float Yaw;
|
||||
float YawTotalAngle;
|
||||
} attitude_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float q[4]; // 四元数估计值
|
||||
|
|
@ -49,9 +57,8 @@ typedef struct
|
|||
float YawTotalAngle;
|
||||
} INS_t;
|
||||
|
||||
|
||||
/**
|
||||
* @brief 用于修正安装误差的参数,demo中可无视
|
||||
* @brief 用于修正安装误差的参数
|
||||
*
|
||||
*/
|
||||
typedef struct
|
||||
|
|
@ -65,16 +72,79 @@ typedef struct
|
|||
float Roll;
|
||||
} IMU_Param_t;
|
||||
|
||||
extern INS_t INS;
|
||||
/**
|
||||
* @brief 反馈位姿结构体指针,apps通过初始化时保存该指针以实现数据访问
|
||||
*
|
||||
* @return attitude_t*
|
||||
*/
|
||||
attitude_t *GetINSptr();
|
||||
|
||||
/**
|
||||
* @brief 初始化惯导解算系统
|
||||
*
|
||||
*/
|
||||
void INS_Init(void);
|
||||
|
||||
/**
|
||||
* @brief 此函数放入实时系统中,以1kHz频率运行
|
||||
* p.s. osDelay(1);
|
||||
*
|
||||
*/
|
||||
void INS_Task(void);
|
||||
|
||||
/**
|
||||
* @brief 温控函数
|
||||
*
|
||||
*/
|
||||
void IMU_Temperature_Ctrl(void);
|
||||
|
||||
/**
|
||||
* @brief 四元数更新函数,即实现dq/dt=0.5Ωq
|
||||
*
|
||||
* @param q 四元数
|
||||
* @param gx
|
||||
* @param gy
|
||||
* @param gz
|
||||
* @param dt 距离上次调用的时间间隔
|
||||
*/
|
||||
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt);
|
||||
|
||||
/**
|
||||
* @brief 四元数转换成欧拉角 ZYX
|
||||
*
|
||||
* @param q
|
||||
* @param Yaw
|
||||
* @param Pitch
|
||||
* @param Roll
|
||||
*/
|
||||
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll);
|
||||
|
||||
/**
|
||||
* @brief ZYX欧拉角转换为四元数
|
||||
*
|
||||
* @param Yaw
|
||||
* @param Pitch
|
||||
* @param Roll
|
||||
* @param q
|
||||
*/
|
||||
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q);
|
||||
|
||||
/**
|
||||
* @brief 机体系到惯性系的变换函数
|
||||
*
|
||||
* @param vecBF body frame
|
||||
* @param vecEF earth frame
|
||||
* @param q
|
||||
*/
|
||||
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
|
||||
|
||||
/**
|
||||
* @brief 惯性系转换到机体系
|
||||
*
|
||||
* @param vecEF
|
||||
* @param vecBF
|
||||
* @param q
|
||||
*/
|
||||
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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然后传入此函数.
|
||||
|
|
@ -76,7 +88,6 @@ dji_motor_instance *DJIMotorInit(can_instance_config config,
|
|||
Motor_Controller_Init_s controller_init,
|
||||
Motor_Type_e type);
|
||||
|
||||
|
||||
/**
|
||||
* @brief 被application层的应用调用,给电机设定参考值.
|
||||
* 对于应用,可以将电机视为传递函数为1的设备,不需要关心底层的闭环
|
||||
|
|
@ -86,7 +97,6 @@ dji_motor_instance *DJIMotorInit(can_instance_config config,
|
|||
*/
|
||||
void DJIMotorSetRef(dji_motor_instance *motor, float ref);
|
||||
|
||||
|
||||
/**
|
||||
* @brief 切换反馈的目标来源,如将角速度和角度的来源换为IMU(小陀螺模式常用)
|
||||
*
|
||||
|
|
@ -94,8 +104,7 @@ void DJIMotorSetRef(dji_motor_instance *motor, float ref);
|
|||
* @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()确定控制频率
|
||||
|
|
@ -103,5 +112,4 @@ void DJIMotorChangeFeed(dji_motor_instance *motor,Closeloop_Type_e loop,Feedback
|
|||
*/
|
||||
void DJIMotorControl();
|
||||
|
||||
|
||||
#endif // !DJI_MOTOR_H
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -2,13 +2,11 @@
|
|||
#include "string.h"
|
||||
#include "bsp_usart.h"
|
||||
|
||||
#define RC_CHANNAL_ERROR_VALUE 700
|
||||
#define REMOTE_CONTROL_FRAME_SIZE 18u
|
||||
#define RC_abs(x) (x)>0?(x):(-x)
|
||||
#define RC_USART_SERVICE_ID 1
|
||||
#define REMOTE_CONTROL_FRAME_SIZE 18u // 遥控器接收的buffer大小
|
||||
|
||||
//remote control data
|
||||
// 遥控器数据
|
||||
static RC_ctrl_t rc_ctrl;
|
||||
// 遥控器拥有的串口实例
|
||||
static usart_instance rc_usart_instance;
|
||||
|
||||
/**
|
||||
|
|
@ -17,75 +15,6 @@ static usart_instance rc_usart_instance;
|
|||
* @param[out] rc_ctrl: remote control data struct point
|
||||
* @retval none
|
||||
*/
|
||||
static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl);
|
||||
|
||||
/**
|
||||
* @brief protocol resolve callback
|
||||
* this func would be called when usart3 idle interrupt happens
|
||||
*
|
||||
*/
|
||||
static void ReceiveCallback()
|
||||
{
|
||||
sbus_to_rc(rc_usart_instance.recv_buff,&rc_ctrl);
|
||||
}
|
||||
|
||||
void RC_init(UART_HandleTypeDef* rc_usart_handle)
|
||||
{
|
||||
rc_usart_instance.module_callback=ReceiveCallback;
|
||||
rc_usart_instance.usart_handle=rc_usart_handle;
|
||||
rc_usart_instance.recv_buff_size=REMOTE_CONTROL_FRAME_SIZE;
|
||||
USARTRegister(&rc_usart_instance);
|
||||
}
|
||||
|
||||
const RC_ctrl_t *get_remote_control_point(void)
|
||||
{
|
||||
return &rc_ctrl;
|
||||
}
|
||||
|
||||
uint8_t RC_data_is_error(void)
|
||||
{
|
||||
if (RC_abs(rc_ctrl.rc.ch[0]) > RC_CHANNAL_ERROR_VALUE)
|
||||
{
|
||||
goto error;
|
||||
}
|
||||
if (RC_abs(rc_ctrl.rc.ch[1]) > RC_CHANNAL_ERROR_VALUE)
|
||||
{
|
||||
goto error;
|
||||
}
|
||||
if (RC_abs(rc_ctrl.rc.ch[2]) > RC_CHANNAL_ERROR_VALUE)
|
||||
{
|
||||
goto error;
|
||||
}
|
||||
if (RC_abs(rc_ctrl.rc.ch[3]) > RC_CHANNAL_ERROR_VALUE)
|
||||
{
|
||||
goto error;
|
||||
}
|
||||
if (rc_ctrl.rc.s[0] == 0)
|
||||
{
|
||||
goto error;
|
||||
}
|
||||
if (rc_ctrl.rc.s[1] == 0)
|
||||
{
|
||||
goto error;
|
||||
}
|
||||
return 0;
|
||||
error:
|
||||
rc_ctrl.rc.ch[0] = 0;
|
||||
rc_ctrl.rc.ch[1] = 0;
|
||||
rc_ctrl.rc.ch[2] = 0;
|
||||
rc_ctrl.rc.ch[3] = 0;
|
||||
rc_ctrl.rc.ch[4] = 0;
|
||||
rc_ctrl.rc.s[0] = RC_SW_DOWN;
|
||||
rc_ctrl.rc.s[1] = RC_SW_DOWN;
|
||||
rc_ctrl.mouse.x = 0;
|
||||
rc_ctrl.mouse.y = 0;
|
||||
rc_ctrl.mouse.z = 0;
|
||||
rc_ctrl.mouse.press_l = 0;
|
||||
rc_ctrl.mouse.press_r = 0;
|
||||
rc_ctrl.key.v = 0;
|
||||
return 1;
|
||||
}
|
||||
|
||||
static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
|
||||
{
|
||||
if (sbus_buf == NULL || rc_ctrl == NULL)
|
||||
|
|
@ -95,7 +24,8 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
|
|||
rc_ctrl->rc.ch[0] = (sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff; //!< Channel 0
|
||||
rc_ctrl->rc.ch[1] = ((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff; //!< Channel 1
|
||||
rc_ctrl->rc.ch[2] = ((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | //!< Channel 2
|
||||
(sbus_buf[4] << 10)) &0x07ff;
|
||||
(sbus_buf[4] << 10)) &
|
||||
0x07ff;
|
||||
rc_ctrl->rc.ch[3] = ((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff; //!< Channel 3
|
||||
rc_ctrl->rc.s[0] = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left
|
||||
rc_ctrl->rc.s[1] = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right
|
||||
|
|
@ -105,7 +35,7 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
|
|||
rc_ctrl->mouse.press_l = sbus_buf[12]; //!< Mouse Left Is Press ?
|
||||
rc_ctrl->mouse.press_r = sbus_buf[13]; //!< Mouse Right Is Press ?
|
||||
rc_ctrl->key.v = sbus_buf[14] | (sbus_buf[15] << 8); //!< KeyBoard value
|
||||
rc_ctrl->rc.ch[4] = sbus_buf[16] | (sbus_buf[17] << 8); //NULL
|
||||
rc_ctrl->rc.ch[4] = sbus_buf[16] | (sbus_buf[17] << 8); // NULL
|
||||
|
||||
rc_ctrl->rc.ch[0] -= RC_CH_VALUE_OFFSET;
|
||||
rc_ctrl->rc.ch[1] -= RC_CH_VALUE_OFFSET;
|
||||
|
|
@ -114,3 +44,26 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
|
|||
rc_ctrl->rc.ch[4] -= RC_CH_VALUE_OFFSET;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief protocol resolve callback
|
||||
* this func would be called when usart3 idle interrupt happens
|
||||
* 对sbus_to_rc的简单封装
|
||||
*
|
||||
*/
|
||||
static void ReceiveCallback()
|
||||
{
|
||||
sbus_to_rc(rc_usart_instance.recv_buff, &rc_ctrl);
|
||||
}
|
||||
|
||||
void RC_init(UART_HandleTypeDef *rc_usart_handle)
|
||||
{
|
||||
rc_usart_instance.module_callback = ReceiveCallback;
|
||||
rc_usart_instance.usart_handle = rc_usart_handle;
|
||||
rc_usart_instance.recv_buff_size = REMOTE_CONTROL_FRAME_SIZE;
|
||||
USARTRegister(&rc_usart_instance);
|
||||
}
|
||||
|
||||
const RC_ctrl_t *get_remote_control_point(void)
|
||||
{
|
||||
return &rc_ctrl;
|
||||
}
|
||||
|
|
@ -1,21 +1,14 @@
|
|||
/**
|
||||
****************************(C) COPYRIGHT 2016 DJI****************************
|
||||
* @file remote_control.c/h
|
||||
* @brief 遥控器处理，遥控器是通过类似SBUS的协议传输，利用DMA传输方式节约CPU
|
||||
* 资源，利用串口空闲中断来拉起处理函数，同时提供一些掉线重启DMA，串口
|
||||
* 的方式保证热插拔的稳定性。
|
||||
* @note
|
||||
* @history
|
||||
* Version Date Author Modification
|
||||
* V1.0.0 Dec-26-2018 RM 1. done
|
||||
* V1.0.0 Nov-11-2019 RM 1. support development board tpye c
|
||||
* @file remote_control.h
|
||||
* @author DJI 2016
|
||||
* @author modified by neozng
|
||||
* @brief 遥控器模块定义头文件
|
||||
* @version beta
|
||||
* @date 2022-11-01
|
||||
*
|
||||
* @copyright Copyright (c) 2016 DJI corp
|
||||
* @copyright Copyright (c) 2022 HNU YueLu EC all rights reserved
|
||||
*
|
||||
@verbatim
|
||||
==============================================================================
|
||||
|
||||
==============================================================================
|
||||
@endverbatim
|
||||
****************************(C) COPYRIGHT 2016 DJI****************************
|
||||
*/
|
||||
#ifndef REMOTE_CONTROL_H
|
||||
#define REMOTE_CONTROL_H
|
||||
|
|
@ -70,27 +63,27 @@ typedef struct
|
|||
{
|
||||
uint16_t v;
|
||||
} key;
|
||||
|
||||
} RC_ctrl_t;
|
||||
/* ----------------------- Internal Data ----------------------------------- */
|
||||
|
||||
/* ------------------------- Internal Data ----------------------------------- */
|
||||
|
||||
/**
|
||||
* @brief register remote control into usart_serive
|
||||
* @brief 初始化遥控器,该函数会将遥控器注册到串口
|
||||
*
|
||||
* @attention remember to assign the correct handler
|
||||
* @attention 注意分配正确的串口硬件,遥控器在C板上使用USART3
|
||||
*
|
||||
*/
|
||||
void RC_init(UART_HandleTypeDef* rc_usart_handle);
|
||||
void RC_init(UART_HandleTypeDef *rc_usart_handle);
|
||||
|
||||
/**
|
||||
* @brief Get the remote control point object,then you can access data
|
||||
* @brief 获取遥控器的数据指针
|
||||
*
|
||||
* @return const RC_ctrl_t*
|
||||
*/
|
||||
extern const RC_ctrl_t *get_remote_control_point(void);
|
||||
|
||||
/**
|
||||
* @brief check whether there is an error in rc data
|
||||
* @brief 检查遥控器数据是否出错
|
||||
*
|
||||
* @return uint8_t
|
||||
*/
|
||||
|
|
|
|||
Loading…
Reference in New Issue