TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 03:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

修复PID堵转检测错误，修复LK电机位置环计算错误，更新文档，修改了消息中心的命名（不影响app层）

Browse Source
This commit is contained in:
NeoZng
2023-05-14 14:56:00 +08:00
parent 299073321b
commit ecb56ef935
12 changed files with 63 additions and 641 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
modules/algorithm/controller.c
View File

@@ -97,7 +97,7 @@ static void f_Output_Limit(PIDInstance *pid)
static void f_PID_ErrorHandle(PIDInstance *pid)
{
/*Motor Blocked Handle*/
if (pid->Output < pid->MaxOut * 0.001f || fabsf(pid->Ref) < 0.0001f)
if (fabsf(pid->Output) < pid->MaxOut * 0.001f || fabsf(pid->Ref) < 0.0001f)
return;
if ((fabsf(pid->Ref - pid->Measure) / fabsf(pid->Ref)) > 0.95f)

38
modules/message_center/message_center.c
View File

@@ -5,17 +5,17 @@
/* message_center是fake head node,是方便链表编写的技巧,这样就不需要处理链表头的特殊情况 */
static Publisher_t message_center = {
.event_name = "Message_Manager",
.topic_name = "Message_Manager",
.first_subs = NULL,
.next_event_node = NULL};
.next_topic_node = NULL};
static void CheckName(char *name)
{
if (strnlen(name, MAX_EVENT_NAME_LEN + 1) >= MAX_EVENT_NAME_LEN)
if (strnlen(name, MAX_TOPIC_NAME_LEN + 1) >= MAX_TOPIC_NAME_LEN)
{
LOGERROR("EVENT NAME TOO LONG:%s", name);
while (1)
; // 进入这里说明事件名超出长度限制
; // 进入这里说明话题名超出长度限制
}
}
@@ -25,7 +25,7 @@ static void CheckLen(uint8_t len1, uint8_t len2)
{
LOGERROR("EVENT LEN NOT SAME:%d,%d", len1, len2);
while (1)
; // 进入这里说明相同事件的消息长度却不同
; // 进入这里说明相同话题的消息长度却不同
}
}
@@ -33,28 +33,28 @@ Publisher_t *PubRegister(char *name, uint8_t data_len)
{
CheckName(name);
Publisher_t *node = &message_center;
while (node->next_event_node) // message_center会直接跳过,不需要特殊处理,这被称作dumb_head(编程技巧)
while (node->next_topic_node) // message_center会直接跳过,不需要特殊处理,这被称作dumb_head(编程技巧)
{
node = node->next_event_node; // 切换到下一个发布者(事件)结点
if (strcmp(node->event_name, name) == 0) // 如果已经注册了相同的事件,直接返回结点指针
node = node->next_topic_node; // 切换到下一个发布者(话题)结点
if (strcmp(node->topic_name, name) == 0) // 如果已经注册了相同的话题,直接返回结点指针
{
CheckLen(data_len, node->data_len);
node->pub_registered_flag = 1;
return node;
}
} // 遍历完发现尚未创建name对应的事件
// 在链表尾部创建新的事件并初始化
node->next_event_node = (Publisher_t *)malloc(sizeof(Publisher_t));
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);
} // 遍历完发现尚未创建name对应的话题
// 在链表尾部创建新的话题并初始化
node->next_topic_node = (Publisher_t *)malloc(sizeof(Publisher_t));
memset(node->next_topic_node, 0, sizeof(Publisher_t));
node->next_topic_node->data_len = data_len;
strcpy(node->next_topic_node->topic_name, name);
node->pub_registered_flag = 1;
return node->next_event_node;
return node->next_topic_node;
}
Subscriber_t *SubRegister(char *name, uint8_t data_len)
{
Publisher_t* pub = PubRegister(name, data_len); // 查找或创建该事件的发布者
Publisher_t *pub = PubRegister(name, data_len); // 查找或创建该话题的发布者
// 创建新的订阅者结点,申请内存,注意要memset因为新空间不一定是空的,可能有之前留存的垃圾值
Subscriber_t *ret = (Subscriber_t *)malloc(sizeof(Subscriber_t));
memset(ret, 0, sizeof(Subscriber_t));
@@ -72,7 +72,7 @@ Subscriber_t *SubRegister(char *name, uint8_t data_len)
}
// 若该话题已经有订阅者, 遍历订阅者链表,直到到达尾部
Subscriber_t *sub = pub->first_subs; // 作为iterator
while (sub->next_subs_queue) // 遍历订阅了该事件的订阅者链表
while (sub->next_subs_queue) // 遍历订阅了该话题的订阅者链表
{
sub = sub->next_subs_queue; // 移动到下一个订阅者,遇到空指针停下,说明到了链表尾部
}
@@ -96,8 +96,8 @@ uint8_t SubGetMessage(Subscriber_t *sub, void *data_ptr)
uint8_t PubPushMessage(Publisher_t *pub, void *data_ptr)
{
static Subscriber_t *iter;
iter = pub->first_subs; // iter作为订阅者指针,遍历订阅该事件的所有订阅者;如果为空说明遍历结束
// 遍历订阅了当前事件的所有订阅者,依次填入最新消息
iter = pub->first_subs; // iter作为订阅者指针,遍历订阅该话题的所有订阅者;如果为空说明遍历结束
// 遍历订阅了当前话题的所有订阅者,依次填入最新消息
while (iter)
{
if (iter->temp_size == QUEUE_SIZE) // 如果队列已满,则需要删除最老的数据(头部),再填入

28
modules/message_center/message_center.h
View File

@@ -15,8 +15,8 @@
#include "stdint.h"
#define MAX_EVENT_NAME_LEN 32 // 最大的事件名长度,每个事件都有字符串来命名
#define MAX_EVENT_COUNT 12 // 最多支持的事件数量
#define MAX_TOPIC_NAME_LEN 32 // 最大的话题名长度,每个话题都有字符串来命名
#define MAX_TOPIC_COUNT 12 // 最多支持的话题数量
#define QUEUE_SIZE 1
typedef struct mqt
@@ -28,30 +28,30 @@ typedef struct mqt
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
{
/* 事件名称 */
char event_name[MAX_EVENT_NAME_LEN + 1]; // 1个字节用于存放字符串结束符 '\0'
uint8_t data_len; // 该事件的数据长度
/* 指向第一个订阅了该事件的订阅者,通过链表访问所有订阅者 */
/* 话题名称 */
char topic_name[MAX_TOPIC_NAME_LEN + 1]; // 1个字节用于存放字符串结束符 '\0'
uint8_t data_len; // 该话题的数据长度
/* 指向第一个订阅了该话题的订阅者,通过链表访问所有订阅者 */
Subscriber_t *first_subs;
/* 指向下一个Publisher的指针 */
struct ent *next_event_node;
struct ent *next_topic_node;
uint8_t pub_registered_flag; // 用于标记该发布者是否已经注册
} Publisher_t;
/**
* @brief 订阅name的事件消息
* @brief 订阅name的话题消息
*
* @param name 事件名称
* @param name 话题名称
* @param data_len 消息长度,通过sizeof()获取
* @return Subscriber_t* 返回订阅者实例
*/
@@ -60,7 +60,7 @@ Subscriber_t *SubRegister(char *name, uint8_t data_len);
/**
* @brief 注册成为消息发布者
*
* @param name 发布者发布的话题名称(事件)
* @param name 发布者发布的话题名称(话题)
* @return Publisher_t* 返回发布者实例
*/
Publisher_t *PubRegister(char *name, uint8_t data_len);
@@ -75,7 +75,7 @@ Publisher_t *PubRegister(char *name, uint8_t data_len);
uint8_t SubGetMessage(Subscriber_t *sub, void *data_ptr);
/**
* @brief 发布者给所有订阅了事件的订阅者推送消息
* @brief 发布者给所有订阅了话题的订阅者推送消息
*
* @param pub 发布者实例指针
* @param data_ptr 指向要发布的数据的指针

50
modules/message_center/message_center.md
View File

@@ -4,15 +4,7 @@
> TODO:
>
> 1. 增加互斥锁或在读写数据的时候进入临界区，防止数据竞争
>
> 2. 降低代码冗杂度，将`PubRegister`和`SubRegister`的共同部分抽象出来编写新的函数
>
> 3. 支持自定义队列长度，使得订阅者可以自行确定需要的队列长度，适应不同的需求
>
> 4. ==**！！！重要！！！**==
>
> ==**Legacy support：将在正式版发布的时候剔除第三方指针传递者的实现，请迁移到新的链表实现。**==
> 支持自定义队列长度，使得订阅者可以自行确定需要的队列长度，适应不同的需求
@@ -20,13 +12,13 @@
**重要定义：**
- 发布者：发布消息的对象。发布者会将自己的消息推送给所有订阅了某个特定**事件**的订阅者。
- 订阅者：获取消息的对象。订阅者在订阅了某个事件之后，可以通过接口获得该事件的消息。
- 事件（event / topic）：用于区分消息来源的对象。可以将一个事件看作一刊杂志，不同的发布者会将文章汇集到杂志上，而订阅者选择订阅一种杂志，然后就可以获取所有写在杂志上的文章。
- 发布者：发布消息的对象。发布者会将自己的消息推送给所有订阅了某个特定**话题**的订阅者。
- 订阅者：获取消息的对象。订阅者在订阅了某个话题之后，可以通过接口获得该话题的消息。
- 话题（topic）：用于区分消息来源的对象。可以将一个话题看作一刊杂志，不同的发布者会将文章汇集到杂志上，而订阅者选择订阅一种杂志，然后就可以获取所有写在杂志上的文章。
Message Center不同应用间进行消息传递的中介，它的存在可以在相当大的程度上解耦不同的app，使得不同的应用之间**不存在包含关系**，让代码的自由度更大，将不同模块之间的关系降为**松耦合**。在以往，如果一个.c文件中的数据需要被其他任务/源文件共享，那么其他模块应该要包含前者的头文件，且头文件中应当存在获取该模块数据的接口（即函数，一般是返回数据指针或直接返回数据，**强烈建议不要使用全局变量**）；但现在，不同的应用之间完全隔离，他们不需要了解彼此的存在，而是只能看见一个**消息中心**以及一些事件（有些地方也称**话题**）。
Message Center不同应用间进行消息传递的中介，它的存在可以在相当大的程度上解耦不同的app，使得不同的应用之间**不存在包含关系**，让代码的自由度更大，将不同模块之间的关系降为**松耦合**。在以往，如果一个.c文件中的数据需要被其他任务/源文件共享，那么其他模块应该要包含前者的头文件，且头文件中应当存在获取该模块数据的接口（即函数，一般是返回数据指针或直接返回数据，**强烈建议不要使用全局变量**）；但现在，不同的应用之间完全隔离，他们不需要了解彼此的存在，而是只能看见一个**消息中心**以及一些**话题**。
需要被共享的消息，将会被**发布者**（publisher）发送到消息中心；要获取消息，则由**订阅者**（subscriber）从消息中心根据订阅的消息名（事件）获取。在这之前，发布者要在消息中心完成**注册**，将自己要发布的消息类型和消息名称提交到消息中心；订阅者同样要先在消息中心完成订阅，将自己要接收的消息类型和消息名称提交到订阅中心。消息中心会根据**消息名称**，把订阅者绑定到发布相同名称的发布者上。
需要被共享的消息，将会被**发布者**（publisher）发送到消息中心；要获取消息，则由**订阅者**（subscriber）从消息中心根据订阅的话题获取。在这之前，发布者要在消息中心完成**注册**，将自己要发布的消息类型和话题名称提交到消息中心；订阅者同样要先在消息中心完成订阅，将自己要接收的消息类型和话题名称提交到订阅中心。消息中心会根据**话题名称**，把订阅者绑定到发布相同名称的发布者上。
> 为了节省空间，数据结构上采用了链表+循环数组模拟队列的方式。C没有哈希表，因此让发布者保存所有订阅者的地址（实际上只保存首地址，然后通过链表访问所有订阅者）。
@@ -42,7 +34,7 @@ Message Center对外提供了四个接口，所有原本要进行信息交互的
## 外部接口
**在代码实现上，事件实际上就是通过一个字符串体现的。**
**在代码实现上，话题名实际上就是通过一个字符串体现的。**
```c
Subscriber_t* SubRegister(char* name,uint8_t data_len);
@@ -56,7 +48,7 @@ void PubPushMessage(Publisher_t* pub,void* data_ptr);
### 订阅者
订阅者应该保存一个订阅者类型的指针`Subscriber_t*`，在初始化的时候调用`SubRegister()`并传入要订阅的事件名和该事件对应消息的长度，可以直接输入字符串，示例如下，将从event_name订阅float数据：
订阅者应该保存一个订阅者类型的指针`Subscriber_t*`，在初始化的时候调用`SubRegister()`并传入要订阅的话题名和该话题对应消息的长度，可以直接输入字符串，示例如下，将从event_name订阅float数据：
```c
Subscriber_t* my_sub;
@@ -69,18 +61,18 @@ my_sub=SubRegister("event_name",sizeof(float));
### 发布者
发布者应该保存一个发布者类型的指针，在初始化的时候传入要发布的事件名和该事件对应的消息长度。
发布者应该保存一个发布者类型的指针，在初始化的时候传入要发布的话题名和该话题对应的消息长度。
完成注册后，通过`PubPushMessage()`发布新的消息。所有订阅了该事件的订阅者都会收到新的消息推送。
完成注册后，通过`PubPushMessage()`发布新的消息。所有订阅了该话题的订阅者都会收到新的消息推送。
### 可修改的宏
```c
#define MAX_EVENT_NAME_LEN 32 //最大的事件名长度,每个事件都由字符串来命名
#define MAX_EVENT_NAME_LEN 32 //最大的话题名长度,每个话题都由字符串来命名
#define QUEUE_SIZE 1 //消息队列的长度
```
修改第一个可以扩大事件名长度，第二个确定消息队列的长度，数量越大可以保存的消息越多。
修改第一个可以扩大话题名长度，第二个确定消息队列的长度，数量越大可以保存的消息越多。
@@ -96,14 +88,14 @@ static void CheckName(char* name)
{
if(strnlen(name,MAX_EVENT_NAME_LEN+1)>=MAX_EVENT_NAME_LEN)
{
while (1); // 进入这里说明事件名超出长度限制
while (1); // 进入这里说明话题名超出长度限制
}
}
```
`message_center`内部保存了指向第一个发布者的指针，可以看作整个消息中心的抽象。通过这个变量，可以访问所有发布者和订阅者。它将会在各个函数中作为dumb_head（哑结点）以简化逻辑，这样不需要对链表头进行特殊处理。
`CheckName()`在发布者/订阅者注册的时候被调用，用于检查事件名是否超过长度限制。超长后会进入死循环，方便开发者检查。
`CheckName()`在发布者/订阅者注册的时候被调用，用于检查话题名是否超过长度限制。超长后会进入死循环，方便开发者检查。
> 四个外部接口的实现都有详细的注释，有兴趣的同学可以自行阅读。下方也提供了流程图。
@@ -117,23 +109,23 @@ static void CheckName(char* name)
<center>建议打开原图查看</center>
**多个publisher可以绑定同一个事件，往该事件推送消息。但一个subscriber只能订阅一个事件，如果应用需要订阅多个事件，则要创建对应数量的订阅者。**
**多个publisher可以绑定同一个话题，往该话题推送消息。但一个subscriber只能订阅一个话题，如果应用需要订阅多个话题，则要创建对应数量的订阅者。**
> 对于电控程序目前的情况，不存在多个publisher向同一个事件推送消息的情况。
> 对于电控程序目前的情况，不存在多个publisher向同一个话题推送消息的情况。
**对于一个事件，其消息长度必须相同**。发布者和订阅者在注册时都会传入消息长度，用`sizof(your_data)`获取。应当保证不同的模块在进行交互式，使用相同的数据长度。
**对于相同话题，其消息长度必须相同**。发布者和订阅者在注册时都会传入消息长度，用`sizof(your_data)`获取。应当保证不同的模块在进行交互式，使用相同的数据长度。
### 发布者和订阅者注册的流程
- **发布者：**
遍历发布者的事件结点，如果发现相同的事件，直接返回指针即可；遍历完成后发现尚未创建则创建新的事件。
遍历发布者的话题结点，如果发现相同的话题，直接返回指针即可；遍历完成后发现尚未创建则创建新的话题。
<img src="../../assets/image-20221201152530558.png" alt="image-20221201152530558" style="zoom: 80%;" />
- **订阅者：**
需要注意，由于不同应用/模块的初始化顺序不同，可能出现订阅者先于发布者订阅某一消息的情况，所以要进行发布者链表的遍历，判断是否已经存在相同事件名的发布者，不存在则要先创建发布者结点再将新建订阅者结点并挂载到前者上。
需要注意，由于不同应用/模块的初始化顺序不同，可能出现订阅者先于发布者订阅某一消息的情况，所以要进行发布者链表的遍历，判断是否已经存在相同话题名的发布者，不存在则要先创建发布者结点再将新建订阅者结点并挂载到前者上。
<img src="../../assets/image-20221201152904044.png" alt="image-20221201152904044" style="zoom:80%;" />
@@ -160,9 +152,9 @@ front=(front+1)%SIZE_OF_ARRAY;
- **发布者推送消息到指定事件**
- **发布者推送消息到指定话题**
通过发布者指针，将订阅了该事件的所有订阅者遍历，将新数据入队。
通过发布者指针，将订阅了该话题的所有订阅者遍历，将新数据入队。
- **订阅者获取消息**

2
modules/motor/LKmotor/LK9025.c
View File

@@ -91,7 +91,7 @@ void LKMotorControl()
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_angle_feedback_ptr;
else
pid_measure = measure->real_current;
pid_measure = measure->total_angle;
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
pid_ref += *motor->speed_feedforward_ptr;