增加了大量注释

This commit is contained in:
NeoZng
2023-01-01 17:32:22 +08:00
parent c2f8b5c8c3
commit c05513587c
56 changed files with 773 additions and 598 deletions

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@@ -4,3 +4,5 @@
> TODO:
> 1. 预计添加不同错误标志,将错误类型和灯的闪烁频率或颜色等对应起来,方便调试
这是legacy support,后续将使用bsp_pwm进行重构

View File

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

View File

@@ -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 调用此函数初始化和视觉的串口通信

View File

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

View File

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

View File

@@ -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,检查是否要发送这一帧报文

View File

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

View File

@@ -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校验,无误再写入数据,不重复判断帧头
*/

View File

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

View File

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

View File

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