mirror of
https://gitee.com/dlmu-cone/bf_original_balance_chassis
synced 2026-07-24 11:37:45 +08:00
增加了大量注释
This commit is contained in:
@@ -11,112 +11,26 @@
|
||||
#include "controller.h"
|
||||
#include <memory.h>
|
||||
|
||||
// PID优化环节函数声明
|
||||
static void f_Trapezoid_Intergral(PIDInstance *pid); // 梯形积分
|
||||
static void f_Integral_Limit(PIDInstance *pid); // 积分限幅
|
||||
static void f_Derivative_On_Measurement(PIDInstance *pid); // 微分先行(仅使用反馈值而不计参考输入的微分)
|
||||
static void f_Changing_Integration_Rate(PIDInstance *pid); // 变速积分(误差小时积分作用更强)
|
||||
static void f_Output_Filter(PIDInstance *pid); // 输出滤波(平滑输出)
|
||||
static void f_Derivative_Filter(PIDInstance *pid); // 微分滤波(采集时,滤除高频噪声)
|
||||
static void f_Output_Limit(PIDInstance *pid); // 输出限幅
|
||||
static void f_PID_ErrorHandle(PIDInstance *pid); // 堵转保护
|
||||
|
||||
/**
|
||||
* @brief 初始化PID,设置参数和启用的优化环节,将其他数据置零
|
||||
*
|
||||
* @param pid PID实例
|
||||
* @param config PID初始化设置
|
||||
*/
|
||||
void PID_Init(PIDInstance *pid, PID_Init_config_s *config)
|
||||
{
|
||||
memset(pid, 0, sizeof(PIDInstance));
|
||||
// utilize the quality of struct that its memeory is continuous
|
||||
memcpy(pid, config, sizeof(PID_Init_config_s));
|
||||
// set rest of memory to 0
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief PID计算
|
||||
* @param[in] PID结构体
|
||||
* @param[in] 测量值
|
||||
* @param[in] 期望值
|
||||
* @retval 返回空
|
||||
*/
|
||||
float PID_Calculate(PIDInstance *pid, float measure, float ref)
|
||||
{
|
||||
if (pid->Improve & ErrorHandle)
|
||||
f_PID_ErrorHandle(pid);
|
||||
|
||||
pid->dt = DWT_GetDeltaT((void *)&pid->DWT_CNT);
|
||||
|
||||
pid->Measure = measure;
|
||||
pid->Ref = ref;
|
||||
pid->Err = pid->Ref - pid->Measure;
|
||||
|
||||
if (abs(pid->Err) > pid->DeadBand)
|
||||
{
|
||||
pid->Pout = pid->Kp * pid->Err;
|
||||
pid->ITerm = pid->Ki * pid->Err * pid->dt;
|
||||
pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt;
|
||||
|
||||
// 梯形积分
|
||||
if (pid->Improve & Trapezoid_Intergral)
|
||||
f_Trapezoid_Intergral(pid);
|
||||
// 变速积分
|
||||
if (pid->Improve & ChangingIntegrationRate)
|
||||
f_Changing_Integration_Rate(pid);
|
||||
// 微分先行
|
||||
if (pid->Improve & Derivative_On_Measurement)
|
||||
f_Derivative_On_Measurement(pid);
|
||||
// 微分滤波器
|
||||
if (pid->Improve & DerivativeFilter)
|
||||
f_Derivative_Filter(pid);
|
||||
// 积分限幅
|
||||
if (pid->Improve & Integral_Limit)
|
||||
f_Integral_Limit(pid);
|
||||
|
||||
pid->Iout += pid->ITerm;
|
||||
pid->Output = pid->Pout + pid->Iout + pid->Dout;
|
||||
|
||||
// 输出滤波
|
||||
if (pid->Improve & OutputFilter)
|
||||
f_Output_Filter(pid);
|
||||
// 输出限幅
|
||||
f_Output_Limit(pid);
|
||||
}
|
||||
else // 进入死区,清空积分和输出
|
||||
{
|
||||
pid->Output=0;
|
||||
pid->ITerm=0;
|
||||
}
|
||||
|
||||
pid->Last_Measure = pid->Measure;
|
||||
pid->Last_Output = pid->Output;
|
||||
pid->Last_Dout = pid->Dout;
|
||||
pid->Last_Err = pid->Err;
|
||||
pid->Last_ITerm = pid->ITerm;
|
||||
|
||||
return pid->Output;
|
||||
}
|
||||
/* ----------------------------下面是pid优化环节的实现---------------------------- */
|
||||
|
||||
// 梯形积分
|
||||
static void f_Trapezoid_Intergral(PIDInstance *pid)
|
||||
{
|
||||
|
||||
// 计算梯形的面积,(上底+下底)*高/2
|
||||
pid->ITerm = pid->Ki * ((pid->Err + pid->Last_Err) / 2) * pid->dt;
|
||||
}
|
||||
|
||||
// 变速积分(误差小时积分作用更强)
|
||||
static void f_Changing_Integration_Rate(PIDInstance *pid)
|
||||
{
|
||||
if (pid->Err * pid->Iout > 0)
|
||||
{
|
||||
// 积分呈累积趋势
|
||||
// Integral still increasing
|
||||
if (abs(pid->Err) <= pid->CoefB)
|
||||
return; // Full integral
|
||||
if (abs(pid->Err) <= (pid->CoefA + pid->CoefB))
|
||||
pid->ITerm *= (pid->CoefA - abs(pid->Err) + pid->CoefB) / pid->CoefA;
|
||||
else
|
||||
else // 最大阈值,不使用积分
|
||||
pid->ITerm = 0;
|
||||
}
|
||||
}
|
||||
@@ -126,13 +40,11 @@ static void f_Integral_Limit(PIDInstance *pid)
|
||||
static float temp_Output, temp_Iout;
|
||||
temp_Iout = pid->Iout + pid->ITerm;
|
||||
temp_Output = pid->Pout + pid->Iout + pid->Dout;
|
||||
if (abs(temp_Output) > pid->MaxOut)
|
||||
if (abs(temp_Output) > pid->MaxOut)
|
||||
{
|
||||
if (pid->Err * pid->Iout > 0)
|
||||
if (pid->Err * pid->Iout > 0) // 积分却还在累积
|
||||
{
|
||||
// 积分呈累积趋势
|
||||
// Integral still increasing
|
||||
pid->ITerm = 0;
|
||||
pid->ITerm = 0; // 当前积分项置零
|
||||
}
|
||||
}
|
||||
|
||||
@@ -148,23 +60,27 @@ static void f_Integral_Limit(PIDInstance *pid)
|
||||
}
|
||||
}
|
||||
|
||||
// 微分先行(仅使用反馈值而不计参考输入的微分)
|
||||
static void f_Derivative_On_Measurement(PIDInstance *pid)
|
||||
{
|
||||
pid->Dout = pid->Kd * (pid->Last_Measure - pid->Measure) / pid->dt;
|
||||
}
|
||||
|
||||
// 微分滤波(采集微分时,滤除高频噪声)
|
||||
static void f_Derivative_Filter(PIDInstance *pid)
|
||||
{
|
||||
pid->Dout = pid->Dout * pid->dt / (pid->Derivative_LPF_RC + pid->dt) +
|
||||
pid->Last_Dout * pid->Derivative_LPF_RC / (pid->Derivative_LPF_RC + pid->dt);
|
||||
}
|
||||
|
||||
// 输出滤波
|
||||
static void f_Output_Filter(PIDInstance *pid)
|
||||
{
|
||||
pid->Output = pid->Output * pid->dt / (pid->Output_LPF_RC + pid->dt) +
|
||||
pid->Last_Output * pid->Output_LPF_RC / (pid->Output_LPF_RC + pid->dt);
|
||||
}
|
||||
|
||||
// 输出限幅
|
||||
static void f_Output_Limit(PIDInstance *pid)
|
||||
{
|
||||
if (pid->Output > pid->MaxOut)
|
||||
@@ -177,7 +93,7 @@ static void f_Output_Limit(PIDInstance *pid)
|
||||
}
|
||||
}
|
||||
|
||||
// PID ERRORHandle Function
|
||||
// 电机堵转检测
|
||||
static void f_PID_ErrorHandle(PIDInstance *pid)
|
||||
{
|
||||
/*Motor Blocked Handle*/
|
||||
@@ -199,4 +115,95 @@ static void f_PID_ErrorHandle(PIDInstance *pid)
|
||||
// Motor blocked over 1000times
|
||||
pid->ERRORHandler.ERRORType = Motor_Blocked;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* ---------------------------下面是PID的外部算法接口--------------------------- */
|
||||
|
||||
/**
|
||||
* @brief 初始化PID,设置参数和启用的优化环节,将其他数据置零
|
||||
*
|
||||
* @param pid PID实例
|
||||
* @param config PID初始化设置
|
||||
*/
|
||||
void PID_Init(PIDInstance *pid, PID_Init_config_s *config)
|
||||
{
|
||||
// config的数据和pid的部分数据是连续且相同的的,所以可以直接用memcpy
|
||||
// @todo: 不建议这样做,可扩展性差,不知道的开发者可能会误以为pid和config是同一个结构体
|
||||
// 后续修改为逐个赋值
|
||||
memset(pid, 0, sizeof(PIDInstance));
|
||||
// utilize the quality of struct that its memeory is continuous
|
||||
memcpy(pid, config, sizeof(PID_Init_config_s));
|
||||
// set rest of memory to 0
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief PID计算
|
||||
* @param[in] PID结构体
|
||||
* @param[in] 测量值
|
||||
* @param[in] 期望值
|
||||
* @retval 返回空
|
||||
*/
|
||||
float PID_Calculate(PIDInstance *pid, float measure, float ref)
|
||||
{
|
||||
// 堵转检测
|
||||
if (pid->Improve & ErrorHandle)
|
||||
f_PID_ErrorHandle(pid);
|
||||
|
||||
pid->dt = DWT_GetDeltaT((void *)&pid->DWT_CNT); //获取两次pid计算的时间间隔,用于积分和微分
|
||||
|
||||
// 保存上次的测量值和误差,计算当前error
|
||||
pid->Measure = measure;
|
||||
pid->Ref = ref;
|
||||
pid->Err = pid->Ref - pid->Measure;
|
||||
|
||||
// 如果在死区外,则计算PID
|
||||
if (abs(pid->Err) > pid->DeadBand)
|
||||
{
|
||||
// 基本的pid计算,使用位置式
|
||||
pid->Pout = pid->Kp * pid->Err;
|
||||
pid->ITerm = pid->Ki * pid->Err * pid->dt;
|
||||
pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt;
|
||||
|
||||
// 梯形积分
|
||||
if (pid->Improve & Trapezoid_Intergral)
|
||||
f_Trapezoid_Intergral(pid);
|
||||
// 变速积分
|
||||
if (pid->Improve & ChangingIntegrationRate)
|
||||
f_Changing_Integration_Rate(pid);
|
||||
// 微分先行
|
||||
if (pid->Improve & Derivative_On_Measurement)
|
||||
f_Derivative_On_Measurement(pid);
|
||||
// 微分滤波器
|
||||
if (pid->Improve & DerivativeFilter)
|
||||
f_Derivative_Filter(pid);
|
||||
// 积分限幅
|
||||
if (pid->Improve & Integral_Limit)
|
||||
f_Integral_Limit(pid);
|
||||
|
||||
pid->Iout += pid->ITerm; // 累加积分
|
||||
pid->Output = pid->Pout + pid->Iout + pid->Dout; // 计算输出
|
||||
|
||||
// 输出滤波
|
||||
if (pid->Improve & OutputFilter)
|
||||
f_Output_Filter(pid);
|
||||
|
||||
// 输出限幅
|
||||
f_Output_Limit(pid);
|
||||
}
|
||||
else // 进入死区, 则清空积分和输出
|
||||
{
|
||||
pid->Output=0;
|
||||
pid->ITerm=0;
|
||||
}
|
||||
|
||||
// 保存当前数据,用于下次计算
|
||||
pid->Last_Measure = pid->Measure;
|
||||
pid->Last_Output = pid->Output;
|
||||
pid->Last_Dout = pid->Dout;
|
||||
pid->Last_Err = pid->Err;
|
||||
pid->Last_ITerm = pid->ITerm;
|
||||
|
||||
return pid->Output;
|
||||
}
|
||||
@@ -89,15 +89,6 @@ float float_deadband(float Value, float minValue, float maxValue)
|
||||
return Value;
|
||||
}
|
||||
|
||||
// int26死区
|
||||
int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue)
|
||||
{
|
||||
if (Value < maxValue && Value > minValue)
|
||||
{
|
||||
Value = 0;
|
||||
}
|
||||
return Value;
|
||||
}
|
||||
|
||||
// 限幅函数
|
||||
float float_constrain(float Value, float minValue, float maxValue)
|
||||
|
||||
@@ -96,8 +96,6 @@ float abs_limit(float num, float Limit);
|
||||
float sign(float value);
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
float float_deadband(float Value, float minValue, float maxValue);
|
||||
// int26<32><36><EFBFBD><EFBFBD>
|
||||
int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue);
|
||||
//<2F><EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
|
||||
float float_constrain(float Value, float minValue, float maxValue);
|
||||
//<2F><EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
@@ -10,9 +10,10 @@
|
||||
*/
|
||||
static void CANCommResetRx(CANCommInstance *ins)
|
||||
{
|
||||
// 当前已经收到的buffer清零
|
||||
memset(ins->raw_recvbuf, 0, ins->cur_recv_len);
|
||||
ins->recv_state = 0;
|
||||
ins->cur_recv_len = 0;
|
||||
ins->recv_state = 0; // 接收状态重置
|
||||
ins->cur_recv_len = 0; // 当前已经收到的长度重置
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -23,33 +24,36 @@ static void CANCommResetRx(CANCommInstance *ins)
|
||||
static void CANCommRxCallback(CANInstance *_instance)
|
||||
{
|
||||
static CANCommInstance *comm;
|
||||
comm = (CANCommInstance *)_instance->id;
|
||||
comm = (CANCommInstance *)_instance->id; // 注意写法,将can instance的id强制转换为CANCommInstance*类型
|
||||
|
||||
/* 接收状态判断 */
|
||||
if (_instance->rx_buff[0] == CAN_COMM_HEADER && comm->recv_state == 0) // 尚未开始接收且新的一包里有帧头
|
||||
if (_instance->rx_buff[0] == CAN_COMM_HEADER && comm->recv_state == 0) // 之前尚未开始接收且此次包里第一个位置是帧头
|
||||
{
|
||||
if (_instance->rx_buff[1] == comm->recv_data_len) // 接收长度等于设定接收长度
|
||||
if (_instance->rx_buff[1] == comm->recv_data_len) // 如果这一包里的datalen也等于我们设定接收长度(这是因为暂时不支持动态包长)
|
||||
{
|
||||
comm->recv_state = 1;
|
||||
comm->recv_state = 1; // 设置接收状态为1,说明已经开始接收
|
||||
}
|
||||
else
|
||||
return; // 直接跳过即可
|
||||
}
|
||||
|
||||
if (comm->recv_state) // 已经收到过帧头
|
||||
{ // 如果已经接收到的长度加上当前一包的长度大于总buf len,说明接收错误
|
||||
{
|
||||
// 如果已经接收到的长度加上当前一包的长度大于总buf len,说明接收错误
|
||||
if (comm->cur_recv_len + _instance->rx_len > comm->recv_buf_len)
|
||||
{
|
||||
CANCommResetRx(comm);
|
||||
return; // 重置状态然后返回
|
||||
}
|
||||
|
||||
// 直接拷贝到当前的接收buffer后面
|
||||
// 直接把当前接收到的数据接到buffer后面
|
||||
memcpy(comm->raw_recvbuf + comm->cur_recv_len, _instance->rx_buff, _instance->rx_len);
|
||||
comm->cur_recv_len += _instance->rx_len;
|
||||
|
||||
// 当前已经收满
|
||||
// 收完这一包以后刚好等于总buf len,说明已经收完了
|
||||
if (comm->cur_recv_len == comm->recv_buf_len)
|
||||
{ // buff里本该是tail的位置不等于CAN_COMM_TAIL
|
||||
{
|
||||
// 如果buff里本该是tail的位置不等于CAN_COMM_TAIL
|
||||
if (comm->raw_recvbuf[comm->recv_buf_len - 1] != CAN_COMM_TAIL)
|
||||
{
|
||||
CANCommResetRx(comm);
|
||||
@@ -59,8 +63,8 @@ static void CANCommRxCallback(CANInstance *_instance)
|
||||
{
|
||||
if (comm->raw_recvbuf[comm->recv_buf_len - 2] ==
|
||||
crc_8(comm->raw_recvbuf + 2, comm->recv_data_len))
|
||||
{ // 通过校验,复制数据到unpack_data中
|
||||
memcpy(comm->unpacked_recv_data, comm->raw_recvbuf + 2, comm->recv_data_len);
|
||||
{ // 通过校验,复制数据到unpack_data中
|
||||
memcpy(comm->unpacked_recv_data, comm->raw_recvbuf + 2, comm->recv_data_len); // 数据量大的话考虑使用DMA
|
||||
comm->update_flag = 1; // 数据更新flag置为1
|
||||
}
|
||||
CANCommResetRx(comm);
|
||||
@@ -73,17 +77,18 @@ static void CANCommRxCallback(CANInstance *_instance)
|
||||
|
||||
CANCommInstance *CANCommInit(CANComm_Init_Config_s *comm_config)
|
||||
{
|
||||
CANCommInstance* ins = (CANCommInstance *)malloc(sizeof(CANCommInstance));
|
||||
CANCommInstance *ins = (CANCommInstance *)malloc(sizeof(CANCommInstance));
|
||||
memset(ins, 0, sizeof(CANCommInstance));
|
||||
|
||||
ins->recv_data_len = comm_config->recv_data_len;
|
||||
ins->recv_buf_len = comm_config->recv_data_len + CAN_COMM_OFFSET_BYTES;
|
||||
ins->recv_buf_len = comm_config->recv_data_len + CAN_COMM_OFFSET_BYTES; // head + datalen + crc8 + tail
|
||||
ins->send_data_len = comm_config->send_data_len;
|
||||
ins->send_buf_len = comm_config->send_data_len + CAN_COMM_OFFSET_BYTES;
|
||||
ins->raw_sendbuf[0] = CAN_COMM_HEADER;
|
||||
ins->raw_sendbuf[1] = comm_config->send_data_len;
|
||||
ins->raw_sendbuf[0] = CAN_COMM_HEADER; // head,直接设置避免每次发送都要重新赋值,下面的tail同理
|
||||
ins->raw_sendbuf[1] = comm_config->send_data_len; // datalen
|
||||
ins->raw_sendbuf[comm_config->send_data_len + CAN_COMM_OFFSET_BYTES - 1] = CAN_COMM_TAIL;
|
||||
|
||||
comm_config->can_config.id = ins;
|
||||
// can instance的设置
|
||||
comm_config->can_config.id = ins; // CANComm的实例指针作为CANInstance的id,回调函数中会用到
|
||||
comm_config->can_config.can_module_callback = CANCommRxCallback;
|
||||
ins->can_ins = CANRegister(&comm_config->can_config);
|
||||
return ins;
|
||||
@@ -93,10 +98,12 @@ void CANCommSend(CANCommInstance *instance, uint8_t *data)
|
||||
{
|
||||
static uint8_t crc8;
|
||||
static uint8_t send_len;
|
||||
// 将data copy到raw_sendbuf中,计算crc8
|
||||
memcpy(instance->raw_sendbuf + 2, data, instance->send_data_len);
|
||||
crc8 = crc_8(data, instance->send_data_len);
|
||||
instance->raw_sendbuf[2 + instance->send_data_len] = crc8;
|
||||
|
||||
// CAN单次发送最大为8字节,如果超过8字节,需要分包发送
|
||||
for (size_t i = 0; i < instance->send_buf_len; i += 8)
|
||||
{ // 如果是最后一包,send len将会小于8,要修改CAN的txconf中的DLC位,调用bsp_can提供的接口即可
|
||||
send_len = instance->send_buf_len - i >= 8 ? 8 : instance->send_buf_len - i;
|
||||
@@ -108,6 +115,6 @@ void CANCommSend(CANCommInstance *instance, uint8_t *data)
|
||||
|
||||
void *CANCommGet(CANCommInstance *instance)
|
||||
{
|
||||
instance->update_flag = 0;
|
||||
instance->update_flag = 0; // 读取后将更新flag置为0
|
||||
return instance->unpacked_recv_data;
|
||||
}
|
||||
@@ -16,9 +16,9 @@
|
||||
#define MX_CAN_COMM_COUNT 4 // 注意均衡负载,一条总线上不要挂载过多的外设
|
||||
|
||||
#define CAN_COMM_MAX_BUFFSIZE 60 // 最大发送/接收字节数,如果不够可以增加此数值
|
||||
#define CAN_COMM_HEADER 's'
|
||||
#define CAN_COMM_TAIL 'e'
|
||||
#define CAN_COMM_OFFSET_BYTES 4 // 's'+datalen+'e'+crc8
|
||||
#define CAN_COMM_HEADER 's' // 帧头
|
||||
#define CAN_COMM_TAIL 'e' // 帧尾
|
||||
#define CAN_COMM_OFFSET_BYTES 4 // 's'+ datalen + 'e' + crc8
|
||||
|
||||
#pragma pack(1)
|
||||
/* CAN comm 结构体, 拥有CAN comm的app应该包含一个CAN comm指针 */
|
||||
@@ -35,42 +35,44 @@ typedef struct
|
||||
uint8_t raw_recvbuf[CAN_COMM_MAX_BUFFSIZE + CAN_COMM_OFFSET_BYTES]; // 额外4个bytes保存帧头帧尾和校验和
|
||||
uint8_t unpacked_recv_data[CAN_COMM_MAX_BUFFSIZE]; // 解包后的数据,调用CANCommGet()后cast成对应的类型通过指针读取即可
|
||||
/* 接收和更新标志位*/
|
||||
uint8_t recv_state; // 接收状态,
|
||||
uint8_t cur_recv_len;
|
||||
uint8_t update_flag;
|
||||
uint8_t recv_state; // 接收状态,
|
||||
uint8_t cur_recv_len; // 当前已经接收到的数据长度(包括帧头帧尾datalen和校验和)
|
||||
uint8_t update_flag; // 数据更新标志位,当接收到新数据时,会将此标志位置1,调用CANCommGet()后会将此标志位置0
|
||||
} CANCommInstance;
|
||||
#pragma pack()
|
||||
|
||||
/* CAN comm 初始化结构体 */
|
||||
typedef struct
|
||||
{
|
||||
CAN_Init_Config_s can_config;
|
||||
uint8_t send_data_len;
|
||||
uint8_t recv_data_len;
|
||||
CAN_Init_Config_s can_config; // CAN初始化结构体
|
||||
uint8_t send_data_len; // 发送数据长度
|
||||
uint8_t recv_data_len; // 接收数据长度
|
||||
} CANComm_Init_Config_s;
|
||||
|
||||
/**
|
||||
* @brief
|
||||
* @brief 初始化CANComm
|
||||
*
|
||||
* @param config
|
||||
* @param config CANComm初始化结构体
|
||||
* @return CANCommInstance*
|
||||
*/
|
||||
CANCommInstance *CANCommInit(CANComm_Init_Config_s *comm_config);
|
||||
|
||||
/**
|
||||
* @brief 发送数据
|
||||
* @brief 通过CANComm发送数据
|
||||
*
|
||||
* @param instance can comm实例
|
||||
* @param instance cancomm实例
|
||||
* @param data 注意此地址的有效数据长度需要和初始化时传入的datalen相同
|
||||
*/
|
||||
void CANCommSend(CANCommInstance *instance, uint8_t *data);
|
||||
|
||||
/**
|
||||
* @brief 获取CAN COMM接收的数据,需要自己使用强制类型转换将返回的void指针转换成指定类型
|
||||
* @brief 获取CANComm接收的数据,需要自己使用强制类型转换将返回的void指针转换成指定类型
|
||||
*
|
||||
* @return void* 返回的数据指针
|
||||
* @attention 注意如果希望直接通过转换指针访问数据,如果数据是union或struct,要检查是否使用了pack(n)
|
||||
* CAN COMM接收到的数据可以看作是pack(1)之后的,是连续存放的
|
||||
* CANComm接收到的数据可以看作是pack(1)之后的,是连续存放的.
|
||||
* 如果使用了pack(n)可能会导致数据错乱,并且无法使用强制类型转换直接访问,而需要手动解包.
|
||||
* 强烈建议通过CANComm传输的数据使用pack(1)
|
||||
*/
|
||||
void *CANCommGet(CANCommInstance *instance);
|
||||
|
||||
|
||||
@@ -20,6 +20,7 @@ DaemonInstance *DaemonRegister(Daemon_Init_Config_s *config)
|
||||
return instance;
|
||||
}
|
||||
|
||||
/* "喂狗"函数 */
|
||||
void DaemonReload(DaemonInstance *instance)
|
||||
{
|
||||
instance->temp_count = instance->reload_count;
|
||||
@@ -36,11 +37,15 @@ void DaemonTask()
|
||||
for (size_t i = 0; i < idx; ++i)
|
||||
{
|
||||
dins = daemon_instances[i];
|
||||
if (dins->temp_count > 0)
|
||||
if (dins->temp_count > 0) // 如果计数器还有值,说明上一次喂狗后还没有超时,则计数器减一
|
||||
dins->temp_count--;
|
||||
else if (dins->callback) // 如果有callback
|
||||
else if (dins->callback) // 等于零说明超时了,调用回调函数(如果有的话)
|
||||
{
|
||||
dins->callback(dins->owner_id); // module内可以将owner_id强制类型转换成自身类型从而调用自身的offline callback
|
||||
dins->callback(dins->owner_id); // module内可以将owner_id强制类型转换成自身类型从而调用特定module的offline callback
|
||||
}
|
||||
}
|
||||
}
|
||||
// (需要id的原因是什么?) 下面是copilot的回答!
|
||||
// 需要id的原因是因为有些module可能有多个实例,而我们需要知道具体是哪个实例offline
|
||||
// 如果只有一个实例,则可以不用id,直接调用callback即可
|
||||
// 比如: 有一个module叫做"电机",它有两个实例,分别是"电机1"和"电机2",那么我们调用电机的离线处理函数时就需要知道是哪个电机offline
|
||||
|
||||
@@ -16,15 +16,14 @@ typedef struct daemon_ins
|
||||
|
||||
uint16_t temp_count; // 当前值,减为零说明模块离线或异常
|
||||
void *owner_id; // daemon实例的地址,初始化的时候填入
|
||||
|
||||
} DaemonInstance;
|
||||
|
||||
/* daemon初始化配置 */
|
||||
typedef struct
|
||||
{
|
||||
uint16_t reload_count; // 实际上这是app唯一需要设置的值?
|
||||
offline_callback callback;
|
||||
void *owner_id; // id取拥有daemon的实例的地址,如DJIMotorInstance*,cast成void*类型
|
||||
uint16_t reload_count; // 实际上这是app唯一需要设置的值?
|
||||
offline_callback callback; // 异常处理函数,当模块发生异常时会被调用
|
||||
void *owner_id; // id取拥有daemon的实例的地址,如DJIMotorInstance*,cast成void*类型
|
||||
} Daemon_Init_Config_s;
|
||||
|
||||
/**
|
||||
|
||||
@@ -4,3 +4,5 @@
|
||||
> TODO:
|
||||
> 1. 预计添加不同错误标志,将错误类型和灯的闪烁频率或颜色等对应起来,方便调试
|
||||
|
||||
|
||||
这是legacy support,后续将使用bsp_pwm进行重构
|
||||
@@ -13,7 +13,6 @@
|
||||
#include "usart.h"
|
||||
#include "seasky_protocol.h"
|
||||
|
||||
/* use usart1 as vision communication*/
|
||||
static Vision_Recv_s recv_data;
|
||||
// @todo:由于后续需要进行IMU-Cam的硬件触发采集控制,因此可能需要将发送设置为定时任务,或由IMU采集完成产生的中断唤醒的任务,
|
||||
// 使得时间戳对齐. 因此,在send_data中设定其他的标志位数据,让ins_task填充姿态值.
|
||||
@@ -58,7 +57,8 @@ void VisionSend(Vision_Send_s *send)
|
||||
static uint8_t send_buff[VISION_SEND_SIZE];
|
||||
static uint16_t tx_len;
|
||||
// TODO: code to set flag_register
|
||||
|
||||
|
||||
// 将数据转化为seasky协议的数据包
|
||||
get_protocol_send_data(0x02, flag_register, &send->yaw, 3, send_buff, &tx_len);
|
||||
USARTSend(vision_usart_instance, send_buff, tx_len);
|
||||
}
|
||||
@@ -4,9 +4,10 @@
|
||||
#include "bsp_usart.h"
|
||||
#include "seasky_protocol.h"
|
||||
|
||||
#define VISION_RECV_SIZE 36u
|
||||
#define VISION_RECV_SIZE 36u // 当前为固定值,36字节
|
||||
#define VISION_SEND_SIZE 36u
|
||||
|
||||
#pragma pack(1)
|
||||
typedef enum
|
||||
{
|
||||
NO_FIRE = 0,
|
||||
@@ -79,6 +80,7 @@ typedef struct
|
||||
|
||||
// uint32_t time_stamp; // @todo 用于和相机的时间戳对齐
|
||||
} Vision_Send_s;
|
||||
#pragma pack()
|
||||
|
||||
/**
|
||||
* @brief 调用此函数初始化和视觉的串口通信
|
||||
|
||||
@@ -8,7 +8,7 @@ static char sname[MAX_EVENT_COUNT][MAX_EVENT_NAME_LEN + 1];
|
||||
static void *p_ptr[MAX_EVENT_COUNT] = {NULL};
|
||||
static void **s_pptr[MAX_EVENT_COUNT] = {NULL}; // 因为要修改指针,所以需要二重指针
|
||||
|
||||
/* ----------------------------------第三方指针传递版的实现----------------------------------- */
|
||||
/* ----------------------------------第三方指针传递版的实现,deprecated----------------------------------- */
|
||||
|
||||
void MessageInit()
|
||||
{
|
||||
@@ -68,11 +68,12 @@ void SubscribeEvent(char *name, void **data_ptr)
|
||||
|
||||
/* ----------------------------------链表-队列版的实现----------------------------------- */
|
||||
|
||||
/* message_center是fake node,是方便链表编写的技巧,这样不需要处理链表头的特殊情况 */
|
||||
/* message_center是fake head node,是方便链表编写的技巧,这样就不需要处理链表头的特殊情况 */
|
||||
static Publisher_t message_center = {
|
||||
.event_name = "Message_Manager",
|
||||
.first_subs = NULL,
|
||||
.next_event_node = NULL};
|
||||
.next_event_node = NULL
|
||||
};
|
||||
|
||||
static void CheckName(char *name)
|
||||
{
|
||||
@@ -88,7 +89,7 @@ static void CheckLen(uint8_t len1, uint8_t len2)
|
||||
if (len1 != len2)
|
||||
{
|
||||
while (1)
|
||||
; // 相同事件的消息长度不同
|
||||
; // 进入这里说明相同事件的消息长度却不同
|
||||
}
|
||||
}
|
||||
|
||||
@@ -111,14 +112,14 @@ Subscriber_t *SubRegister(char *name, uint8_t data_len)
|
||||
{ // 给消息队列的每一个元素分配空间,queue里保存的实际上是数据执指针,这样可以兼容不同的数据长度
|
||||
ret->queue[i] = malloc(sizeof(data_len));
|
||||
}
|
||||
//如果之前没有订阅者,特殊处理一下
|
||||
//如果是第一个订阅者,特殊处理一下
|
||||
if(node->first_subs==NULL)
|
||||
{
|
||||
node->first_subs=ret;
|
||||
return ret;
|
||||
}
|
||||
// 遍历订阅者链表,直到到达尾部
|
||||
Subscriber_t *sub = node->first_subs; // iterator
|
||||
Subscriber_t *sub = node->first_subs; // 作为iterator
|
||||
while (sub->next_subs_queue) // 遍历订阅了该事件的订阅者链表
|
||||
{
|
||||
sub = sub->next_subs_queue; // 移动到下一个订阅者,遇到空指针停下,说明到了链表尾部
|
||||
@@ -182,11 +183,14 @@ uint8_t SubGetMessage(Subscriber_t *sub, void *data_ptr)
|
||||
|
||||
uint8_t PubPushMessage(Publisher_t *pub, void *data_ptr)
|
||||
{
|
||||
Subscriber_t *iter = pub->first_subs; // iter作为订阅者指针,遍历订阅该事件的所有订阅者;如果为空说明遍历结束
|
||||
while (iter) // 遍历订阅了当前事件的所有订阅者,依次填入最新消息
|
||||
static Subscriber_t *iter;
|
||||
iter = pub->first_subs; // iter作为订阅者指针,遍历订阅该事件的所有订阅者;如果为空说明遍历结束
|
||||
// 遍历订阅了当前事件的所有订阅者,依次填入最新消息
|
||||
while (iter)
|
||||
{
|
||||
if (iter->temp_size == QUEUE_SIZE) // 如果队列已满,则需要删除最老的数据(头部),再填入
|
||||
{ // 队列头索引前移动,相当于抛弃前一个位置,被抛弃的位置稍后会被写入新的数据
|
||||
{
|
||||
// 队列头索引前移动,相当于抛弃前一个位置的数据,被抛弃的位置稍后会被写入新的数据
|
||||
iter->front_idx = (iter->front_idx + 1) % QUEUE_SIZE;
|
||||
iter->temp_size--; // 相当于出队,size-1
|
||||
}
|
||||
|
||||
@@ -5,9 +5,9 @@
|
||||
* @todo 实现基于链表-队列的pubsub机制
|
||||
* @version 0.1
|
||||
* @date 2022-11-30
|
||||
*
|
||||
*
|
||||
* @copyright Copyright (c) 2022
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PUBSUB_H
|
||||
@@ -15,102 +15,93 @@
|
||||
|
||||
#include "stdint.h"
|
||||
|
||||
|
||||
#define MAX_EVENT_NAME_LEN 32 //最大的事件名长度,每个事件都有字符串来命名
|
||||
#define MAX_EVENT_COUNT 12 //最多支持的事件数量
|
||||
#define MAX_EVENT_NAME_LEN 32 // 最大的事件名长度,每个事件都有字符串来命名
|
||||
#define MAX_EVENT_COUNT 12 // 最多支持的事件数量
|
||||
#define QUEUE_SIZE 1
|
||||
|
||||
|
||||
/**
|
||||
* @brief 在所有应用初始化结束之后调用,当作app的"回调函数"
|
||||
*
|
||||
*
|
||||
*/
|
||||
void MessageInit();
|
||||
|
||||
/**
|
||||
* @brief 注册成为消息发布者
|
||||
*
|
||||
*
|
||||
* @param name 消息标识名,注意不要超过MAX_EVENT_NAME_LEN
|
||||
* @param data 发布者的数据地址
|
||||
*/
|
||||
void PublisherRegister(char* name,void* data);
|
||||
void PublisherRegister(char *name, void *data);
|
||||
|
||||
/**
|
||||
* @brief 订阅消息,成为消息订阅者
|
||||
*
|
||||
*
|
||||
* @param name 消息标识名
|
||||
* @param data 保存数据的指针的地址,注意这是一个二级指针,传入的时候对数据指针取地址(&)
|
||||
*/
|
||||
void SubscribeEvent(char* name,void** data);
|
||||
void SubscribeEvent(char *name, void **data);
|
||||
|
||||
#endif // !PUBSUB_H
|
||||
|
||||
|
||||
|
||||
/* 以下是队列版的pubsub,TODO */
|
||||
|
||||
typedef struct mqt
|
||||
{
|
||||
/* 用数组模拟FIFO队列 */
|
||||
void* queue[QUEUE_SIZE];
|
||||
void *queue[QUEUE_SIZE];
|
||||
uint8_t data_len;
|
||||
uint8_t front_idx;
|
||||
uint8_t back_idx;
|
||||
uint8_t temp_size; // 当前队列长度
|
||||
|
||||
/* 指向下一个订阅了相同的事件的订阅者的指针 */
|
||||
struct mqt* next_subs_queue;
|
||||
struct mqt *next_subs_queue; // 使得发布者可以通过链表访问所有订阅了相同事件的订阅者
|
||||
} Subscriber_t;
|
||||
|
||||
/**
|
||||
* @brief 发布者类型.每个发布者拥有发布者实例,这个实例
|
||||
*
|
||||
* @brief 发布者类型.每个发布者拥有发布者实例,并且可以通过链表访问所有订阅了自己发布的事件的订阅者
|
||||
*
|
||||
*/
|
||||
typedef struct ent
|
||||
{
|
||||
typedef struct ent
|
||||
{
|
||||
/* 事件名称 */
|
||||
char event_name[MAX_EVENT_NAME_LEN+2];
|
||||
uint8_t data_len;
|
||||
char event_name[MAX_EVENT_NAME_LEN + 1]; // 1个字节用于存放字符串结束符 '\0'
|
||||
uint8_t data_len; // 该事件的数据长度
|
||||
/* 指向第一个订阅了该事件的订阅者,通过链表访问所有订阅者 */
|
||||
Subscriber_t* first_subs;
|
||||
Subscriber_t *first_subs;
|
||||
/* 指向下一个Publisher的指针 */
|
||||
struct ent* next_event_node;
|
||||
|
||||
struct ent *next_event_node;
|
||||
} Publisher_t;
|
||||
|
||||
/**
|
||||
* @brief 订阅name的事件消息
|
||||
*
|
||||
*
|
||||
* @param name 事件名称
|
||||
* @param data_len 消息长度,通过sizeof()获取
|
||||
* @return Subscriber_t* 返回订阅者实例
|
||||
*/
|
||||
Subscriber_t* SubRegister(char* name,uint8_t data_len);
|
||||
Subscriber_t *SubRegister(char *name, uint8_t data_len);
|
||||
|
||||
/**
|
||||
* @brief 注册成为消息发布者
|
||||
*
|
||||
*
|
||||
* @param name 发布者发布的话题名称(事件)
|
||||
* @return Publisher_t* 返回发布者实例
|
||||
*/
|
||||
Publisher_t* PubRegister(char* name,uint8_t data_len);
|
||||
Publisher_t *PubRegister(char *name, uint8_t data_len);
|
||||
|
||||
/**
|
||||
* @brief 获取消息
|
||||
*
|
||||
*
|
||||
* @param sub 订阅者实例指针
|
||||
* @param data_ptr 数据指针,接收的消息将会放到此处
|
||||
* @return uint8_t 返回值为0说明没有新的消息(消息队列为空),为1说明获取到了新的消息
|
||||
*/
|
||||
uint8_t SubGetMessage(Subscriber_t* sub,void* data_ptr);
|
||||
uint8_t SubGetMessage(Subscriber_t *sub, void *data_ptr);
|
||||
|
||||
/**
|
||||
* @brief 发布者给所有订阅了事件的订阅者推送消息
|
||||
*
|
||||
*
|
||||
* @param pub 发布者实例指针
|
||||
* @param data_ptr 指向要发布的数据的指针
|
||||
* @return uint8_t 新消息成功推送给几个订阅者
|
||||
*/
|
||||
uint8_t PubPushMessage(Publisher_t* pub,void* data_ptr);
|
||||
|
||||
|
||||
uint8_t PubPushMessage(Publisher_t *pub, void *data_ptr);
|
||||
|
||||
@@ -10,23 +10,23 @@ static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL};
|
||||
* @brief 由于DJI电机发送以四个一组的形式进行,故对其进行特殊处理,用6个(2can*3group)can_instance专门负责发送
|
||||
* 该变量将在 DJIMotorControl() 中使用,分组在 MotorSenderGrouping()中进行
|
||||
*
|
||||
* C610(m2006)/C620(m3508):0x1ff,0x200; GM6020:0x1ff,0x2ff
|
||||
* 反馈: GM6020: 0x204+id ; C610/C620: 0x200+id
|
||||
* C610(m2006)/C620(m3508):0x1ff,0x200;
|
||||
* GM6020:0x1ff,0x2ff
|
||||
* 反馈(rx_id): GM6020: 0x204+id ; C610/C620: 0x200+id
|
||||
* can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF
|
||||
* can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF
|
||||
*/
|
||||
static CANInstance 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 CANInstance 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() 使用
|
||||
* @brief 6个用于确认是否有电机注册到sender_assignment中的标志位,防止发送空帧,此变量将在DJIMotorControl()使用
|
||||
* flag的初始化在 MotorSenderGrouping()中进行
|
||||
*
|
||||
*/
|
||||
@@ -38,9 +38,7 @@ static uint8_t sender_enable_flag[6] = {0};
|
||||
*/
|
||||
static void IDcrash_Handler(uint8_t conflict_motor_idx, uint8_t temp_motor_idx)
|
||||
{
|
||||
while (1)
|
||||
{
|
||||
};
|
||||
while (1);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -108,7 +106,7 @@ static void MotorSenderGrouping(CAN_Init_Config_s *config)
|
||||
break;
|
||||
|
||||
default: // other motors should not be registered here
|
||||
break;
|
||||
while(1); // 其他电机不应该在这里注册
|
||||
}
|
||||
}
|
||||
|
||||
@@ -124,9 +122,10 @@ static void DecodeDJIMotor(CANInstance *_instance)
|
||||
static uint8_t *rxbuff;
|
||||
static DJI_Motor_Measure_s *measure;
|
||||
rxbuff = _instance->rx_buff;
|
||||
// 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址
|
||||
measure = &((DJIMotorInstance *)_instance->id)->motor_measure; // 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->angle_single_round = ECD_ANGLE_COEF_DJI * (float)measure->ecd;
|
||||
@@ -136,7 +135,7 @@ static void DecodeDJIMotor(CANInstance *_instance)
|
||||
CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5]));
|
||||
measure->temperate = rxbuff[6];
|
||||
|
||||
// multi rounds calc,计算的前提是两次采样间电机转过的角度小于180°
|
||||
// 多圈角度计算,计算的前提是两次采样间电机转过的角度小于180°
|
||||
if (measure->ecd - measure->last_ecd > 4096)
|
||||
measure->total_round--;
|
||||
else if (measure->ecd - measure->last_ecd < -4096)
|
||||
@@ -150,23 +149,23 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
|
||||
DJIMotorInstance *instance = (DJIMotorInstance *)malloc(sizeof(DJIMotorInstance));
|
||||
memset(instance, 0, sizeof(DJIMotorInstance));
|
||||
|
||||
// motor basic setting
|
||||
// motor basic setting 电机基本设置
|
||||
instance->motor_type = config->motor_type;
|
||||
instance->motor_settings = config->controller_setting_init_config;
|
||||
|
||||
// motor controller init
|
||||
// motor controller init 电机控制器初始化
|
||||
PID_Init(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
|
||||
PID_Init(&instance->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID);
|
||||
PID_Init(&instance->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
|
||||
instance->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
|
||||
instance->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
|
||||
|
||||
// group motors, because 4 motors share the same CAN control message
|
||||
// 电机分组,因为至多4个电机可以共用一帧CAN控制报文
|
||||
MotorSenderGrouping(&config->can_init_config);
|
||||
|
||||
// register motor to CAN bus
|
||||
// 注册电机到CAN总线
|
||||
config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback
|
||||
config->can_init_config.id = instance; // set id,eq to address(it is identity)
|
||||
config->can_init_config.id = instance;// set id,eq to address(it is identity)
|
||||
instance->motor_can_instance = CANRegister(&config->can_init_config);
|
||||
|
||||
DJIMotorEnable(instance);
|
||||
@@ -228,7 +227,7 @@ void DJIMotorControl()
|
||||
static float pid_measure, pid_ref; // 电机PID测量值和设定值
|
||||
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
|
||||
for (size_t i = 0; i < idx; ++i)
|
||||
{ // 减小访存开销,先保存指针引用
|
||||
{ // 减小访存开销,先保存指针引用
|
||||
motor = dji_motor_instance[i];
|
||||
motor_setting = &motor->motor_settings;
|
||||
motor_controller = &motor->motor_controller;
|
||||
@@ -280,7 +279,6 @@ void DJIMotorControl()
|
||||
{ // 若该电机处于停止状态,直接将buff置零
|
||||
memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// 遍历flag,检查是否要发送这一帧报文
|
||||
|
||||
@@ -22,22 +22,22 @@
|
||||
#define DJI_MOTOR_CNT 12
|
||||
|
||||
/* 滤波系数设置为1的时候即关闭滤波 */
|
||||
#define SPEED_SMOOTH_COEF 0.85f // better to be greater than 0.85
|
||||
#define CURRENT_SMOOTH_COEF 0.9f // this coef *must* be greater than 0.9
|
||||
#define ECD_ANGLE_COEF_DJI (360.0f/8192.0f) // ,将编码器值转化为角度制
|
||||
#define SPEED_SMOOTH_COEF 0.85f // 最好大于0.85
|
||||
#define CURRENT_SMOOTH_COEF 0.9f // 必须大于0.9
|
||||
#define ECD_ANGLE_COEF_DJI 0.043945f // (360/8192),将编码器值转化为角度制
|
||||
|
||||
/* DJI电机CAN反馈信息*/
|
||||
typedef struct
|
||||
{
|
||||
uint16_t last_ecd; // 上一次读取的编码器值
|
||||
uint16_t ecd; // 0-8191,刻度总共有8192格
|
||||
uint16_t last_ecd; // 上一次读取的编码器值
|
||||
uint16_t ecd; // 0-8191,刻度总共有8192格
|
||||
float angle_single_round; // 单圈角度
|
||||
float speed_aps; // 角速度,单位为:度/秒 rounds per minute
|
||||
int16_t real_current; // 实际电流
|
||||
uint8_t temperate; // 温度 Celsius
|
||||
int16_t real_current; // 实际电流
|
||||
uint8_t temperate; // 温度 Celsius
|
||||
|
||||
float total_angle; // 总角度,注意方向
|
||||
int32_t total_round; // 总圈数,注意方向
|
||||
float total_angle; // 总角度,注意方向
|
||||
int32_t total_round; // 总圈数,注意方向
|
||||
} DJI_Motor_Measure_s;
|
||||
|
||||
/**
|
||||
@@ -46,25 +46,18 @@ typedef struct
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
/* motor measurement recv from CAN feedback */
|
||||
DJI_Motor_Measure_s motor_measure;
|
||||
|
||||
/* basic config of a motor*/
|
||||
Motor_Control_Setting_s motor_settings;
|
||||
DJI_Motor_Measure_s motor_measure; // 电机测量值
|
||||
Motor_Control_Setting_s motor_settings; // 电机设置
|
||||
Motor_Controller_s motor_controller; // 电机控制器
|
||||
|
||||
/* controller used in the motor (3 loops)*/
|
||||
Motor_Controller_s motor_controller;
|
||||
|
||||
/* the CAN instance own by motor instance*/
|
||||
CANInstance *motor_can_instance;
|
||||
|
||||
/* sender assigment*/
|
||||
CANInstance *motor_can_instance; // 电机CAN实例
|
||||
// 分组发送设置
|
||||
uint8_t sender_group;
|
||||
uint8_t message_num;
|
||||
|
||||
Motor_Type_e motor_type; // 电机类型
|
||||
Motor_Working_Type_e stop_flag; // 启停标志
|
||||
|
||||
} DJIMotorInstance;
|
||||
|
||||
/**
|
||||
|
||||
@@ -7,7 +7,6 @@
|
||||
// 参考深圳大学 Infantry_X-master
|
||||
#define RE_RX_BUFFER_SIZE 200
|
||||
|
||||
// static USARTInstance referee_usart_instance;
|
||||
static USARTInstance *referee_usart_instance;
|
||||
|
||||
/**************裁判系统数据******************/
|
||||
@@ -17,7 +16,7 @@ static uint16_t Judge_SelfClient_ID; // 发送者机器人对应的客户端ID
|
||||
|
||||
/**
|
||||
* @brief 读取裁判数据,中断中读取保证速度
|
||||
* @param 缓存数据
|
||||
* @param ReadFromUsart: 读取到的裁判系统原始数据
|
||||
* @retval 是否对正误判断做处理
|
||||
* @attention 在此判断帧头和CRC校验,无误再写入数据,不重复判断帧头
|
||||
*/
|
||||
|
||||
@@ -2,11 +2,12 @@
|
||||
#include "string.h"
|
||||
#include "bsp_usart.h"
|
||||
#include "memory.h"
|
||||
#include "stdlib.h"
|
||||
|
||||
#define REMOTE_CONTROL_FRAME_SIZE 18u // 遥控器接收的buffer大小
|
||||
// 遥控器数据
|
||||
static RC_ctrl_t rc_ctrl[2]; //[0]:当前数据,[1]:上一次的数据.用于按键判断
|
||||
// 遥控器拥有的串口实例
|
||||
static RC_ctrl_t rc_ctrl[2]; //[0]:当前数据TEMP,[1]:上一次的数据LAST.用于按键持续按下和切换的判断
|
||||
// 遥控器拥有的串口实例,因为遥控器是单例,所以这里只有一个,就不封装了
|
||||
static USARTInstance *rc_usart_instance;
|
||||
|
||||
/**
|
||||
@@ -17,8 +18,8 @@ static void RectifyRCjoystick()
|
||||
{
|
||||
for (uint8_t i = 0; i < 5; ++i)
|
||||
{
|
||||
if (*(&rc_ctrl[TEMP].rc.rocker_l_+i) > 660 || *(&rc_ctrl[TEMP].rc.rocker_l_+i) < -660)
|
||||
*(&rc_ctrl[TEMP].rc.rocker_l_+i) = 0;
|
||||
if (abs(*(&rc_ctrl[TEMP].rc.rocker_l_ + i)) > 660)
|
||||
*(&rc_ctrl[TEMP].rc.rocker_l_ + i) = 0;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -28,7 +29,7 @@ static void RectifyRCjoystick()
|
||||
* @param[out] rc_ctrl: remote control data struct point
|
||||
* @retval none
|
||||
*/
|
||||
static void sbus_to_rc(volatile const uint8_t *sbus_buf)
|
||||
static void sbus_to_rc(const uint8_t *sbus_buf)
|
||||
{
|
||||
memcpy(&rc_ctrl[1], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据
|
||||
// 摇杆,直接解算时减去偏置
|
||||
@@ -68,7 +69,7 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf)
|
||||
rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL][j] = rc_ctrl[TEMP].key_temp & i;
|
||||
}
|
||||
|
||||
// 位域的按键值解算,直接memcpy即可,注意小端低字节在前,即lsb在第一位
|
||||
// 位域的按键值解算,直接memcpy即可,注意小端低字节在前,即lsb在第一位,msb在最后. 尚未测试
|
||||
// *(uint16_t *)&rc_ctrl[TEMP].key_test[KEY_PRESS] = (uint16_t)(sbus_buf[14] | (sbus_buf[15] << 8));
|
||||
// *(uint16_t *)&rc_ctrl[TEMP].key_test[KEY_STATE] = *(uint16_t *)&rc_ctrl[TEMP].key_test[KEY_PRESS] & ~(*(uint16_t *)&(rc_ctrl[1].key_test[KEY_PRESS]));
|
||||
// if (rc_ctrl[TEMP].key_test[KEY_PRESS].ctrl)
|
||||
|
||||
@@ -17,7 +17,7 @@
|
||||
#include "main.h"
|
||||
#include "usart.h"
|
||||
|
||||
//
|
||||
// 用于遥控器数据读取,遥控器数据是一个大小为2的数组
|
||||
#define LAST 1
|
||||
#define TEMP 0
|
||||
|
||||
@@ -33,17 +33,18 @@
|
||||
#define RC_CH_VALUE_MAX ((uint16_t)1684)
|
||||
|
||||
/* ----------------------- RC Switch Definition----------------------------- */
|
||||
#define RC_SW_UP ((uint16_t)1)
|
||||
#define RC_SW_MID ((uint16_t)3)
|
||||
#define RC_SW_DOWN ((uint16_t)2)
|
||||
#define RC_SW_UP ((uint16_t)1) // 开关向上时的值
|
||||
#define RC_SW_MID ((uint16_t)3) // 开关中间时的值
|
||||
#define RC_SW_DOWN ((uint16_t)2) // 开关向下时的值
|
||||
// 三个判断开关状态的宏
|
||||
#define switch_is_down(s) (s == RC_SW_DOWN)
|
||||
#define switch_is_mid(s) (s == RC_SW_MID)
|
||||
#define switch_is_up(s) (s == RC_SW_UP)
|
||||
#define LEFT_SW 1
|
||||
#define RIGHT_SW 0
|
||||
#define LEFT_SW 1 // 左侧开关
|
||||
#define RIGHT_SW 0 // 右侧开关
|
||||
|
||||
/* ----------------------- PC Key Definition-------------------------------- */
|
||||
// 对应key[x][0~16],获取对应的键;例如通过key[KEY_PRESS][Key_W]获取W键是否按下
|
||||
// 对应key[x][0~16],获取对应的键;例如通过key[KEY_PRESS][Key_W]获取W键是否按下,后续改为位域后删除
|
||||
#define Key_W 0
|
||||
#define Key_S 1
|
||||
#define Key_D 2
|
||||
|
||||
@@ -10,28 +10,42 @@
|
||||
|
||||
#include "bsp_can.h"
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint16_t vol;
|
||||
uint16_t current;
|
||||
uint16_t power;
|
||||
} SuperCap_Msg_s;
|
||||
|
||||
#pragma pack(1)
|
||||
typedef struct
|
||||
{
|
||||
CANInstance *can_ins;
|
||||
SuperCap_Msg_s cap_msg;
|
||||
} SuperCapInstance;
|
||||
uint16_t vol; // 电压
|
||||
uint16_t current; // 电流
|
||||
uint16_t power; // 功率
|
||||
} SuperCap_Msg_s;
|
||||
#pragma pack()
|
||||
|
||||
/* 超级电容实例 */
|
||||
typedef struct
|
||||
{
|
||||
CANInstance *can_ins; // CAN实例
|
||||
SuperCap_Msg_s cap_msg; // 超级电容信息
|
||||
} SuperCapInstance;
|
||||
|
||||
/* 超级电容初始化配置 */
|
||||
typedef struct
|
||||
{
|
||||
CAN_Init_Config_s can_config;
|
||||
} SuperCap_Init_Config_s;
|
||||
|
||||
/**
|
||||
* @brief 初始化超级电容
|
||||
*
|
||||
* @param supercap_config 超级电容初始化配置
|
||||
* @return SuperCapInstance* 超级电容实例指针
|
||||
*/
|
||||
SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config);
|
||||
|
||||
/**
|
||||
* @brief 发送超级电容控制信息
|
||||
*
|
||||
* @param instance 超级电容实例
|
||||
* @param data 超级电容控制信息
|
||||
*/
|
||||
void SuperCapSend(SuperCapInstance *instance, uint8_t *data);
|
||||
|
||||
#endif // !SUPER_CAP_Hd
|
||||
|
||||
Reference in New Issue
Block a user