From bd2b992e9c3db420201b56a1af32587e8400ee5f Mon Sep 17 00:00:00 2001 From: NeoZng Date: Mon, 6 Mar 2023 18:52:24 +0800 Subject: [PATCH] =?UTF-8?q?=E5=88=A0=E9=99=A4=E4=BA=86message=20center?= =?UTF-8?q?=E7=9A=84=E6=8C=87=E9=92=88=E4=BC=A0=E9=80=92=E6=94=AF=E6=8C=81?= =?UTF-8?q?=EF=BC=8C=E5=A2=9E=E5=8A=A0=E4=BA=86=E5=AE=9A=E4=BD=8Dbug?= =?UTF-8?q?=E7=9A=84=E6=8A=80=E5=B7=A7=E6=96=87=E6=A1=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Bug_Report.md | 4 ++ application/balance_chassis/balance.c | 27 +++++--- application/balance_chassis/gain_table.h | 3 +- application/balance_chassis/vmc_project.h | 14 ++++ application/gimbal/gimbal.c | 6 +- modules/message_center/message_center.c | 83 +++-------------------- modules/message_center/message_center.h | 27 +------- 如何定位bug.md | 73 +++++++++++++++++++- 8 files changed, 126 insertions(+), 111 deletions(-) create mode 100644 application/balance_chassis/vmc_project.h diff --git a/Bug_Report.md b/Bug_Report.md index 4223b3f..a517636 100644 --- a/Bug_Report.md +++ b/Bug_Report.md @@ -1,5 +1,7 @@ # 异常报告 +已知可能出现的bug将会列在此处，并指明修复期限和任务执行者。 + 使用中遇到的bug和错误放在此处。参照下列格式： ## 标题用简短的一句话描述 @@ -32,6 +34,8 @@ ## 使用LK电机并挂载在hcan2上时会出现HardFault +> 已修复此问题。修复日志请查看当前目录下的“如何定位bug.md”。 + 使用MF9025v2电机,并将其配置在CAN2上。经过一次LKMotorControl，第二次进入时hcan->instance会在HAL_CAN_Add_Tx_Message()结束时被未知的语句修改成奇怪的值，造成HardFault ### 尝试解决的方案 diff --git a/application/balance_chassis/balance.c b/application/balance_chassis/balance.c index 741c35b..d587381 100644 --- a/application/balance_chassis/balance.c +++ b/application/balance_chassis/balance.c @@ -1,4 +1,10 @@ +// app #include "balance.h" +#include "vmc_project.h" +#include "gain_table.h" +#include "robot_def.h" +#include "general_def.h" +// module #include "HT04.h" #include "LK9025.h" #include "bmi088.h" @@ -6,9 +12,8 @@ #include "super_cap.h" #include "controller.h" #include "can_comm.h" +// standard #include "stdint.h" -#include "robot_def.h" -#include "general_def.h" #include "arm_math.h" // 需要用到较多三角函数 /* 底盘拥有的模块实例 */ @@ -47,7 +52,7 @@ static float T_leg_left, T_leg_right; // 左右驱动电机力矩 * */ static void CalcLQR() -{ +{ } /** @@ -55,15 +60,15 @@ static void CalcLQR() * */ static void VMCProject() -{ +{ // 拟将功能封装到vmc_project.h中 } /** * @brief 腿部角度控制:转向和抗劈叉 * */ -static PIDInstance swerving_pid; -static PIDInstance anti_crash_pid; +static PIDInstance swerving_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量 +static PIDInstance anti_crash_pid; // 抗劈叉,将输出以相反的方向叠加到左右腿的上 static void SynthesizeMotion() { @@ -73,15 +78,16 @@ static void SynthesizeMotion() * @brief 腿长控制:长度 + roll轴补偿(保持机体水平),用PD模拟弹簧的传递函数 * */ -static PIDInstance leg_length_pid; -static PIDInstance roll_compensate_pid; +static PIDInstance leg_length_pid; // 用PD模拟弹簧的传递函数,不需要积分项(弹簧是一个无积分的二阶系统),增益不可过大否则抗外界冲击响应时太"硬" +static PIDInstance roll_compensate_pid; // roll轴补偿,用于保持机体水平 static void LegControl() { } /** - * @brief 离地监测和?跳跃控制 + * @brief 离地监测和?跳跃控制? + * 通过模型解算地面的支持力完成离地检测 * */ static void FlyDetect() @@ -89,7 +95,7 @@ static void FlyDetect() } /** - * @brief 功率限制 + * @brief 功率限制,一般不需要 * */ static void WattLimit() @@ -242,6 +248,7 @@ void BalanceInit() PIDInit(&roll_compensate_pid, &roll_compensate_pid_conf); } +/* balanceTask可能需要以更高频率运行,以提高线性化的精确程度 */ void BalanceTask() { } diff --git a/application/balance_chassis/gain_table.h b/application/balance_chassis/gain_table.h index 1449e93..b2086bc 100644 --- a/application/balance_chassis/gain_table.h +++ b/application/balance_chassis/gain_table.h @@ -2,13 +2,14 @@ #pragma once #include "stdint.h" +#include "stm32f407xx.h" #include "arm_math.h" #include "math.h" #define GAIN_TABLE_SIZE 100 // 增益表大小 // K 2x6,6个状态变量2个输出(Tp关节电机和T驱动轮电机) -static float leglen2gain [GAIN_TABLE_SIZE][2][6] = {}; +static float leglen2gain [GAIN_TABLE_SIZE][2][6] = {0}; static interpolation_flag = 0; // 插值方式:1 线性插值 0 关闭插值 diff --git a/application/balance_chassis/vmc_project.h b/application/balance_chassis/vmc_project.h new file mode 100644 index 0000000..b74b239 --- /dev/null +++ b/application/balance_chassis/vmc_project.h @@ -0,0 +1,14 @@ +#ifndef VMC_PROJECT_H +#define VMC_PROJECT_H + +#include "stm32f407xx.h" +#include "arm_math.h" +#include "math.h" +#include "general_def.h" + +// 将五连杆和直杆的vmc映射放在此处,方便修改和调试,balance.c不会太长 +#endif // !VMC_PROJECT_H + + + + diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 5f23d3e..389ad64 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -4,7 +4,6 @@ #include "ins_task.h" #include "message_center.h" #include "general_def.h" - #include "bmi088.h" static attitude_t *gimba_IMU_data; // 云台IMU数据 @@ -136,6 +135,7 @@ void GimbalTask() // 后续增加未收到数据的处理 SubGetMessage(gimbal_sub, &gimbal_cmd_recv); + // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘 // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref switch (gimbal_cmd_recv.gimbal_mode) { @@ -170,6 +170,10 @@ void GimbalTask() break; } + // 在合适的地方添加pitch重力补偿前馈力矩 + // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩 + // ... + // 设置反馈数据,主要是imu和yaw的ecd gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data; gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->motor_measure.angle_single_round; diff --git a/modules/message_center/message_center.c b/modules/message_center/message_center.c index 387c9fb..cc4e826 100644 --- a/modules/message_center/message_center.c +++ b/modules/message_center/message_center.c @@ -2,78 +2,11 @@ #include "stdlib.h" #include "string.h" -/* 消息初始化用 */ -static char pname[MAX_EVENT_COUNT][MAX_EVENT_NAME_LEN + 1]; -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() -{ - // pub必须唯一,即消息名称不能重复,不得有多个pub发布相同消息名称 - // 对每一个subscriber,寻找相同消息名称的publisher,可能有多个sub从相同pub获取消息 - for (size_t i = 0; i < MAX_EVENT_COUNT; ++i) - { - if (s_pptr[i] != NULL) - { - for (size_t j = 0; j < MAX_EVENT_COUNT; ++j) // 遍历publisher - { - if (p_ptr[j] != NULL) // 不为空 - { - if (strcmp(sname[i], pname[j]) == 0) // 比较消息名是否一致 - { - *s_pptr[i] = p_ptr[j]; // 将sub的指针指向pub的数据 - break; - } - } - else // 到结尾,退出 - { - while (1) - ; // 如果你卡在这里,说明没有找到消息发布者!请确认消息名称是否键入错误 - } - } - } - else // 说明已经遍历完所有的subs - { - break; - } - } -} - -/* 传入数据地址 */ -void PublisherRegister(char *name, void *data) -{ - static uint8_t idx; - for (size_t i = 0; i < idx; ++i) - { - if (strcmp(pname[i], name) == 0) - while (1) - ; // 运行至此说明pub的消息发布名称冲突 - } - strcpy(pname[idx], name); - p_ptr[idx++] = data; -} - -/* 注意传入的是指针的地址,传参时使用&对数据指针取地址 */ -void SubscribeEvent(char *name, void **data_ptr) -{ - static uint8_t idx; - strcpy(sname[idx], name); - s_pptr[idx++] = data_ptr; -} - - - -/* ----------------------------------链表-队列版的实现----------------------------------- */ - /* 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) { @@ -112,18 +45,18 @@ Subscriber_t *SubRegister(char *name, uint8_t data_len) { // 给消息队列的每一个元素分配空间,queue里保存的实际上是数据执指针,这样可以兼容不同的数据长度 ret->queue[i] = malloc(sizeof(data_len)); } - //如果是第一个订阅者,特殊处理一下 - if(node->first_subs==NULL) + // 如果是第一个订阅者,特殊处理一下 + if (node->first_subs == NULL) { - node->first_subs=ret; + node->first_subs = ret; return ret; } // 遍历订阅者链表,直到到达尾部 Subscriber_t *sub = node->first_subs; // 作为iterator - while (sub->next_subs_queue) // 遍历订阅了该事件的订阅者链表 + while (sub->next_subs_queue) // 遍历订阅了该事件的订阅者链表 { sub = sub->next_subs_queue; // 移动到下一个订阅者,遇到空指针停下,说明到了链表尾部 - } + } sub->next_subs_queue = ret; // 把刚刚创建的订阅者接上 return ret; } @@ -157,6 +90,7 @@ Publisher_t *PubRegister(char *name, uint8_t data_len) if (strcmp(node->event_name, name) == 0) // 如果已经注册了相同的事件,直接返回结点指针 { CheckLen(data_len, node->data_len); + node->pub_registered_flag = 1; return node; } } // 遍历完发现尚未创建name对应的事件 @@ -165,6 +99,7 @@ Publisher_t *PubRegister(char *name, uint8_t data_len) memset(node->next_event_node, 0, sizeof(Publisher_t)); node->next_event_node->data_len = data_len; strcpy(node->next_event_node->event_name, name); + node->pub_registered_flag = 1; return node->next_event_node; } @@ -189,7 +124,7 @@ uint8_t PubPushMessage(Publisher_t *pub, void *data_ptr) while (iter) { if (iter->temp_size == QUEUE_SIZE) // 如果队列已满,则需要删除最老的数据(头部),再填入 - { + { // 队列头索引前移动,相当于抛弃前一个位置的数据,被抛弃的位置稍后会被写入新的数据 iter->front_idx = (iter->front_idx + 1) % QUEUE_SIZE; iter->temp_size--; // 相当于出队,size-1 diff --git a/modules/message_center/message_center.h b/modules/message_center/message_center.h index 44a3223..8335b4b 100644 --- a/modules/message_center/message_center.h +++ b/modules/message_center/message_center.h @@ -19,30 +19,6 @@ #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); - -/** - * @brief 订阅消息,成为消息订阅者 - * - * @param name 消息标识名 - * @param data 保存数据的指针的地址,注意这是一个二级指针,传入的时候对数据指针取地址(&) - */ -void SubscribeEvent(char *name, void **data); - -#endif // !PUBSUB_H - typedef struct mqt { /* 用数组模拟FIFO队列 */ @@ -69,6 +45,7 @@ typedef struct ent Subscriber_t *first_subs; /* 指向下一个Publisher的指针 */ struct ent *next_event_node; + uint8_t pub_registered_flag; // 用于标记该发布者是否已经注册 } Publisher_t; /** @@ -105,3 +82,5 @@ uint8_t SubGetMessage(Subscriber_t *sub, void *data_ptr); * @return uint8_t 新消息成功推送给几个订阅者 */ uint8_t PubPushMessage(Publisher_t *pub, void *data_ptr); + +#endif // !PUBSUB_H \ No newline at end of file diff --git a/如何定位bug.md b/如何定位bug.md index 045e711..a2e521f 100644 --- a/如何定位bug.md +++ b/如何定位bug.md @@ -85,6 +85,77 @@ long long的范围比float小。无符号和有符号数直接转换可能变成 **宏只在当前文件生效**，如果宏放在.c那么对其他的文件是不可见的，这也一般称作私有宏。 + + ## 典型debug案例 -这是一个结合了软件和硬件且多模块耦合的异常。 \ No newline at end of file +这是一个结合了软件和硬件且有多模块耦合的异常。该bug发生在调试平衡步兵的底盘过程当中。 + +### 引发bug的原因 + +1. 指针在强制类型转换中变成了错误的类型,使得指向的内存地址被错误地修改 +2. CAN总线负载过大导致电机反馈消息丢失 + +这里是发生bug的代码片段: + +```c +static void LKMotorDecode(CANInstance *_instance) +{ + static LKMotor_Measure_t *measure; + static uint8_t *rx_buff; + rx_buff = _instance->rx_buff; + measure = &((LKMotorInstance *)_instance)->measure; // 通过caninstance保存的id获取对应的motorinstance + // 上面一行应为: measure = &(((LKMotorInstance *)_instance->id)->measure); + measure->last_ecd = measure->ecd; + measure->ecd = ... + + // .... + +} +``` + +这是问题1的出处。can instance中保存了父指针，即拥有该instance的LKMotorInstance。这里想通过强制类型转换将`void*`类型的`id`转换成电机的instance指针类型并访问其measure成员变量以从CAN反馈的报文中更新量测值。然而却直接将can instance转换成motor instance。 + +随后，更新之后的数据被覆写到can instance内部，使得其成员变量改变，包括hcan、txbuf、rxbuf、tx/rxlen等。hcan是HAL定义的can句柄类型，里面保存了指向can状态和控制寄存器的指针以及其他HAL状态信息，然而其值被电机反馈回来的值覆写，之后HAL的接口访问hcan时将引起异常。 + +第二个问题则不是显式存在的： + +```c +void MotorControlTask() +{ + DJIMotorControl(); + + HTMotorControl(); + + LKMotorControl(); + + ServeoMotorControl(); + + StepMotorControl(); +} +``` + +这是motortask的内容，此任务将以500hz的频率运行。在发生bug时，我们将4个HT04电机和2个LK MF9025电机全部连接到CAN1上。注意，HT04不支持多电机指令，因此占用的带宽较大。在`LKMotorControl()`完成参考值计算和CAN发送之后立刻会调用`HTMotorControl()`，后者需要连续发送4条报文。而HT和LK电机都会在接收到控制指令之后发送反馈信息报文。由于HT电机的控制在LK电机控制之后立刻执行，导致总线被占据，LK电机发送的反馈数据仲裁失败无法获得总线占有权，使得主机收不到反馈数据。 + + + +### bug的发现和定位的尝试 + +程序的大体情况如下，当时进行轮足式倒立摆机器人的测试，启用了balance.c，在其中注册了4个HT04电机（can1）和2个LK9025电机（can2）。控制报文的发送频率均为500Hz。 + +测试时发现，9025电机可以接收到mcu发送的控制指令并响应，但是mcu始终无法获得反馈值，`LKMotorInstance->measure`的所有成员变量一直是零。由于CAN是总线架构，电机能接收到数据说明通信正常。HT04电机也可以正常控制并收到反馈信息。在`LKMotorDecode()`函数中添加断点发现能够成功进入1~2次，随后便引发HardFault。 + +此时内心有些动摇，开始检查硬件连线。我们尝试把LK电机也挂载到CAN1总线上。开始单步调试，发现LK电机可以正常接收一次反馈报文，之后就进入`Hardfault_handler()`。HT和DJI电机均无此问题。进一步进行每条指令的调试，发现在成功接收到一次报文之后（接收报文指的是can发生中断并在处理函数中调用LK电机的解码函数，我们并没有查看measure值是否刷新，实际上这时候反馈值仍然为零），进入该电机的控制报文发送时，通过在`Hardfault_handler()`中添加汇编语句`asm("bx lr")`，即跳转到最后一次执行的指令，发现访问`hcan->state`会引起硬件错误。遇到这种情况，说明发生了越界访问或使用了野指针。检查hcan的值，发现是一个非常大的地址。因此怀疑hcan指针被其他的内存访问语句修改。 + +有了方向之后，进一步对每一个函数都进行单步进入调试，同时时刻监测hcan1的值。然而，这时候出现即使一开始就单步调试也无法进入LK电机解码函数的问题。于是，怀疑是CAN过滤器的配置问题，使得LK电机反馈报文被过滤，检查LK的接收id无误后，认为可能由于LK电机的发送和接收ID都比较大（0x140和0x280），CAN标准ID放不下。但是查阅CAN specification后发现standar ID可以容纳11位的值，应该不会有问题。于是把过滤器配置为mask模式，让bxCAN控制器接收所有报文（即不进行过滤）。然而还是不奏效，仍然无法收到数据。 + +这时候想起HT电机是不支持多电机控制指令的，因此500Hz的控制频率似乎有些过高，相当于2ms内要完成2x4+1+2=11次CAN报文的发送。计算1M波特率下最大通信速率，果然超出了负载。于是降低`MotorTask()`的频率为200Hz，果然能重新接收到数据了。 + +继续单步调试，终于发现在`LKMotorDecode()`中，通过强制类型转换获取LKMotorInstance的时候，用错了变量，使得反馈值被写入电机的`CANInstance`内，导致hcan指向随机的地址，最终造成访问时引发hardfault。 + +修改之后，将LK电机挂载到CAN2上，控制频率回到500Hz，程序正常运行。 + +### 解决方案 + +均衡总线负载，调节任务运行时间。 +