Merge branch 'master' of gitee.com:hnuyuelurm/basic_framework

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
 
 ### 尝试解决的方案

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 swerving_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
-static PIDInstance anti_crash_pid;
+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 leg_length_pid; // 用PD模拟弹簧的传递函数,不需要积分项(弹簧是一个无积分的二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
-static PIDInstance roll_compensate_pid;
+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()
 {
 }

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 关闭插值
 

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

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;

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

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

如何定位bug.md

@@ -85,6 +85,77 @@ long long的范围比float小。无符号和有符号数直接转换可能变成
 
 **宏只在当前文件生效**，如果宏放在.c那么对其他的文件是不可见的，这也一般称作私有宏。
 
+
+
 ## 典型debug案例
 
-这是一个结合了软件和硬件且多模块耦合的异常。
+这是一个结合了软件和硬件且有多模块耦合的异常。该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，程序正常运行。
+
+### 解决方案
+
+均衡总线负载，调节任务运行时间。
+