完成了队列版本的pubsub机制,但尚未测试

2026-07-24 03:27:45 +08:00 · 2022-11-30 22:10:57 +08:00
parent 32ae8eaa7f
commit bcd7bf00e5
15 changed files with 369 additions and 193 deletions
--- a/modules/can_comm/can_comm.md
+++ b/modules/can_comm/can_comm.md
@@ -81,6 +81,49 @@ static void CANCommRxCallback(can_instance *_instance);

 `CANCommRxCallback()`是CAN comm初始化can实例时的回调函数，用于can接收中断，进行协议解析。

+## 使用范例
+
+例如，这里要发送的数据是一个float，接收的数据是如下的`struct`，**==注意要使用pack==**：
+
+```c
+#pragma pack(1)
+struct test
+{
+	float aa;
+	float bb;
+	float cc;
+	uint16_t dd;
+};
+#pragma pack()
+```
+
+初始化时设置如下：
+
+```c
+CANComm_Init_Config_s cconfig = {
+		.can_config = {
+            .can_handle=&hcan1,
+            .tx_id=0x02,
+            .rx_id=0x03},
+		.send_data_len = sizeof(float),
+		.recv_data_len = sizeof(struct test)
+};
+CANCommInstance* ins = CANCommInit(&cconfig);
+```
+
+通过`CANCommGet()`并使用强制类型转换获得接收到的数据指针：
+
+```c
+struct test* data_ptr=(struct test*)CANCommGet(ins)
+```
+
+发送通过`CANCommSend()`，建议使用强制类型转换：
+
+```c
+float tx=114.514;
+CANCommSend(ins, (uint8_t*)&tx);
+```
+
 ## 接收解析流程

 CAN comm的通信协议如下：
--- a/modules/imu/ins_task.c
+++ b/modules/imu/ins_task.c
@@ -80,65 +80,69 @@ void INS_Task(void)
 {
    static uint32_t count = 0;
    const float gravity[3] = {0, 0, 9.81f};
-    dt = DWT_GetDeltaT(&INS_DWT_Count);
-    t += dt;
-
-    // ins update
-    if ((count % 1) == 0)
+    while (1)
    {
-        BMI088_Read(&BMI088);
+        dt = DWT_GetDeltaT(&INS_DWT_Count);
+        t += dt;

-        INS.Accel[X] = BMI088.Accel[X];
-        INS.Accel[Y] = BMI088.Accel[Y];
-        INS.Accel[Z] = BMI088.Accel[Z];
-        INS.Gyro[X] = BMI088.Gyro[X];
-        INS.Gyro[Y] = BMI088.Gyro[Y];
-        INS.Gyro[Z] = BMI088.Gyro[Z];
-
-        // demo function,用于修正安装误差,可以不管,本demo暂时没用
-        IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
-
-        // 计算重力加速度矢量和b系的XY两轴的夹角,可用作功能扩展,本demo暂时没用
-        INS.atanxz = -atan2f(INS.Accel[X], INS.Accel[Z]) * 180 / PI;
-        INS.atanyz = atan2f(INS.Accel[Y], INS.Accel[Z]) * 180 / PI;
-
-        // 核心函数,EKF更新四元数
-        IMU_QuaternionEKF_Update(INS.Gyro[X], INS.Gyro[Y], INS.Gyro[Z], INS.Accel[X], INS.Accel[Y], INS.Accel[Z], dt);
-
-        memcpy(INS.q, QEKF_INS.q, sizeof(QEKF_INS.q));
-
-        // 机体系基向量转换到导航坐标系，本例选取惯性系为导航系
-        BodyFrameToEarthFrame(xb, INS.xn, INS.q);
-        BodyFrameToEarthFrame(yb, INS.yn, INS.q);
-        BodyFrameToEarthFrame(zb, INS.zn, INS.q);
-
-        // 将重力从导航坐标系n转换到机体系b,随后根据加速度计数据计算运动加速度
-        float gravity_b[3];
-        EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
-        for (uint8_t i = 0; i < 3; i++) // 同样过一个低通滤波
+        // ins update
+        if ((count % 1) == 0)
        {
-            INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
+            BMI088_Read(&BMI088);
+
+            INS.Accel[X] = BMI088.Accel[X];
+            INS.Accel[Y] = BMI088.Accel[Y];
+            INS.Accel[Z] = BMI088.Accel[Z];
+            INS.Gyro[X] = BMI088.Gyro[X];
+            INS.Gyro[Y] = BMI088.Gyro[Y];
+            INS.Gyro[Z] = BMI088.Gyro[Z];
+
+            // demo function,用于修正安装误差,可以不管,本demo暂时没用
+            IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
+
+            // 计算重力加速度矢量和b系的XY两轴的夹角,可用作功能扩展,本demo暂时没用
+            INS.atanxz = -atan2f(INS.Accel[X], INS.Accel[Z]) * 180 / PI;
+            INS.atanyz = atan2f(INS.Accel[Y], INS.Accel[Z]) * 180 / PI;
+
+            // 核心函数,EKF更新四元数
+            IMU_QuaternionEKF_Update(INS.Gyro[X], INS.Gyro[Y], INS.Gyro[Z], INS.Accel[X], INS.Accel[Y], INS.Accel[Z], dt);
+
+            memcpy(INS.q, QEKF_INS.q, sizeof(QEKF_INS.q));
+
+            // 机体系基向量转换到导航坐标系，本例选取惯性系为导航系
+            BodyFrameToEarthFrame(xb, INS.xn, INS.q);
+            BodyFrameToEarthFrame(yb, INS.yn, INS.q);
+            BodyFrameToEarthFrame(zb, INS.zn, INS.q);
+
+            // 将重力从导航坐标系n转换到机体系b,随后根据加速度计数据计算运动加速度
+            float gravity_b[3];
+            EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
+            for (uint8_t i = 0; i < 3; i++) // 同样过一个低通滤波
+            {
+                INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
+            }
+            BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
+
+            // 获取最终数据
+            INS.Yaw = QEKF_INS.Yaw;
+            INS.Pitch = QEKF_INS.Pitch;
+            INS.Roll = QEKF_INS.Roll;
+            INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
        }
-        BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n

-        // 获取最终数据
-        INS.Yaw = QEKF_INS.Yaw;
-        INS.Pitch = QEKF_INS.Pitch;
-        INS.Roll = QEKF_INS.Roll;
-        INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
-    }
+        // temperature control
+        if ((count % 2) == 0)
+        {
+            // 500hz
+            IMU_Temperature_Ctrl();
+        }

-    // temperature control
-    if ((count % 2) == 0)
-    {
-        // 500hz
-        IMU_Temperature_Ctrl();
-    }
-
-    if ((count++ % 1000) == 0)
-    {
-        // 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
+        if ((count++ % 1000) == 0)
+        {
+            // 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
+        }
    }
+    osDelay(1);
 }

 /**
--- a/modules/led_light/led_task.c
+++ b/modules/led_light/led_task.c
@@ -1,48 +1,34 @@
 /**
-  ****************************(C) COPYRIGHT 2019 DJI****************************
-  * @file       led_trigger_task.c/h
-  * @brief      led RGB show.led RGB灯效。
-  * @note
-  * @history
-  *  Version    Date            Author          Modification
-  *  V1.0.0     Nov-11-2019     RM              1. rgb led
-  *
-  @verbatim
-  ==============================================================================
+ * @file led_task.c
+ * @author DJI RM
+ * @author modified by NeoZeng neozng1@hnu.edu.cn
+ * @brief 流水灯效
+ * @version 0.1
+ * @date 2022-11-30
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */

-  ==============================================================================
-  @endverbatim
-  ****************************(C) COPYRIGHT 2019 DJI****************************
-  */
 #include "led_task.h"
 #include "bsp_led.h"
-#include "cmsis_os.h"
 #include "main.h"
+#include <stdint-gcc.h>

 #define RGB_FLOW_COLOR_CHANGE_TIME 1000
 #define RGB_FLOW_COLOR_LENGHT 6
-// blue-> green(dark)-> red -> blue(dark) -> green(dark) -> red(dark) -> blue
+
 // 蓝 -> 绿(灭) -> 红 -> 蓝(灭) -> 绿 -> 红(灭) -> 蓝
 uint32_t RGB_flow_color[RGB_FLOW_COLOR_LENGHT + 1] = {0xFF0000FF, 0x0000FF00, 0xFFFF0000, 0x000000FF, 0xFF00FF00, 0x00FF0000, 0xFF0000FF};

-/**
- * @brief          led rgb task
- * @param[in]      pvParameters: NULL
- * @retval         none
- */
-/**
- * @brief          led RGB任务
- * @param[in]      pvParameters: NULL
- * @retval         none
- */
+
 void led_RGB_flow_task()
 {
-	uint16_t i, j;
-	float delta_alpha, delta_red, delta_green, delta_blue;
-	float alpha, red, green, blue;
-	uint32_t aRGB;
+	static float delta_alpha, delta_red, delta_green, delta_blue;
+	static float alpha, red, green, blue;
+	static uint32_t aRGB;

-	for (i = 0; i < RGB_FLOW_COLOR_LENGHT; i++)
+	for (size_t i = 0; i < RGB_FLOW_COLOR_LENGHT; i++)
 	{
 		alpha = (RGB_flow_color[i] & 0xFF000000) >> 24;
 		red = ((RGB_flow_color[i] & 0x00FF0000) >> 16);
@@ -58,7 +44,7 @@ void led_RGB_flow_task()
 		delta_red /= RGB_FLOW_COLOR_CHANGE_TIME;
 		delta_green /= RGB_FLOW_COLOR_CHANGE_TIME;
 		delta_blue /= RGB_FLOW_COLOR_CHANGE_TIME;
-		for (j = 0; j < RGB_FLOW_COLOR_CHANGE_TIME; j++)
+		for (size_t j = 0; j < RGB_FLOW_COLOR_CHANGE_TIME; j++)
 		{
 			alpha += delta_alpha;
 			red += delta_red;
@@ -69,5 +55,5 @@ void led_RGB_flow_task()
 			aRGB_led_show(aRGB);
 		}
 	}
-	HAL_Delay(1);
+
 }
--- a/modules/led_light/led_task.h
+++ b/modules/led_light/led_task.h
@@ -1,34 +1,7 @@
-/**
-  ****************************(C) COPYRIGHT 2019 DJI****************************
-  * @file       led_trigger_task.c/h
-  * @brief      led RGB show.led RGB<47><42>Ч<EFBFBD><D0A7>
-  * @note
-  * @history
-  *  Version    Date            Author          Modification
-  *  V1.0.0     Nov-11-2019     RM              1. rgb led
-  *
-  @verbatim
-  ==============================================================================

-  ==============================================================================
-  @endverbatim
-  ****************************(C) COPYRIGHT 2019 DJI****************************
-  */
 #ifndef LED_TRIGGER_TASK_H
 #define LED_TRIGGER_TASK_H

-#include <stdint-gcc.h>
-
-/**
- * @brief          led rgb task
- * @param[in]      pvParameters: NULL
- * @retval         none
- */
-/**
- * @brief          led RGB<47><42><EFBFBD><EFBFBD>
- * @param[in]      pvParameters: NULL
- * @retval         none
- */
-extern void led_RGB_flow_task();
+void led_RGB_flow_task();

 #endif
--- a/modules/message_center/message_center.c
+++ b/modules/message_center/message_center.c
@@ -1,4 +1,5 @@
 #include "message_center.h"
+#include "stdlib.h"
 #include "string.h"

 /* 消息初始化用 */
@@ -14,22 +15,27 @@ void MessageInit()
    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) //比较消息名是否一致
-                    {volatile size_t ss=strlen(sname[i]);
-                    volatile size_t sss=strlen(pname[j]);
+                {
+                    if(strcmp(sname[i],pname[j])==0) //比较消息名是否一致
+                    {
                        *s_pptr[i]=p_ptr[j]; // 将sub的指针指向pub的数据
                        break;
                    }
-                    
-                    
+                }
                else //到结尾,退出
+                {
                    while(1); //如果你卡在这里,说明没有找到消息发布者!请确认消息名称是否键入错误
+                }
            }
+        }
        else //说明已经遍历完所有的subs
+        {
            break;
+        }
    }
 }

@@ -52,4 +58,110 @@ void SubscribeEvent(char* name,void** data_ptr)
    static uint8_t idx;
    strcpy(sname[idx],name);
    s_pptr[idx++]=data_ptr;
+}
+
+
+
+
+
+
+/* 以下是队列版的pubsub,TODO */
+
+/* message_center是fake node,方便链表编写的技巧,这样不需要处理链表头的特殊情况 */
+static Publisher_t message_center={
+    .event_name="Message_Manager",
+    .first_subs=NULL,
+    .next_event_node=NULL
+};
+
+Subscriber_t* SubRegister(char* name,uint8_t data_len)
+{
+    Publisher_t* node=&message_center;
+    while(node->next_event_node) // 遍历链表
+    {
+        node=node->next_event_node;
+        if(strcmp(name,node->event_name)==0) //如果事件名相同就订阅这个事件
+        {
+            Subscriber_t* sub=node->first_subs; //指向第一个事件
+            while (sub->next_subs_queue) //遍历订阅了该事件的链表
+            {
+                sub=sub->next_subs_queue;
+            } //遇到空指针停下,创建新的订阅节点
+            Subscriber_t* ret=(Subscriber_t*)malloc(sizeof(Subscriber_t));
+            memset(ret,0,sizeof(Subscriber_t));
+            ret->data_len=data_len;
+            for (size_t i = 0; i < QUEUE_SIZE; i++)
+            {   //给消息队列分配空间,queue里保存的实际上是数据执指针,这样可以兼容不同的数据长度
+                ret->queue[i]=malloc(sizeof(data_len)); 
+            }
+            sub->next_subs_queue=ret; //接到尾部,延长该订阅该事件的subs链表
+            return ret;
+        }
+        // 时间名不同,在下一轮循环访问下一个结点
+    } 
+    //遍历完,发现尚未注册事件,那么创建一个事件.此时node是publisher链表的最后一个结点
+    node->next_event_node=(Publisher_t*)malloc(sizeof(Publisher_t)); 
+    strcpy(node->next_event_node->event_name,name);
+    node->next_event_node->next_event_node=NULL; //新publisher的下一个节点置为NULL
+    //创建subscriber作为新事件的第一个订阅者
+    Subscriber_t* ret=(Subscriber_t*)malloc(sizeof(Subscriber_t));
+    memset(ret,0,sizeof(Subscriber_t));
+    ret->data_len=data_len;
+    for (size_t i = 0; i < QUEUE_SIZE; i++)
+    {   //给消息队列分配空间
+        ret->queue[i]=malloc(sizeof(data_len)); 
+    }
+    node->next_event_node->first_subs=ret;
+    return ret;
+    
+}
+
+Publisher_t* PubRegister(char* name,uint8_t data_len)
+{
+    Publisher_t* node=&message_center;
+    while(node->next_event_node) //message_center会直接跳过,不需要特殊处理
+    {
+        node=node->next_event_node;
+        if(strcmp(node->event_name,name)==0)
+        {
+            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);
+    return node->next_event_node;
+}
+
+/* 如果队列为空,会返回0;成功获取数据,返回1 */
+uint8_t SubGetMessage(Subscriber_t* sub,void* data_ptr)
+{
+    if(sub->temp_size==0)
+    {
+        return 0;
+    }
+    memcpy(sub->queue[sub->front_idx],data_ptr,sub->data_len);
+    sub->front_idx=(sub->front_idx++)%QUEUE_SIZE;
+    sub->temp_size--;
+    return 1;
+}
+
+void PubPushMessage(Publisher_t* pub,void* data_ptr)
+{
+    static Subscriber_t* node;
+    node->next_subs_queue=pub->first_subs;
+    while (node->next_subs_queue) //遍历订阅了当前事件的所有订阅者,依次填入最新消息
+    {
+        node=node->next_subs_queue;
+        if(node->temp_size==QUEUE_SIZE) //如果队列已满,则需要删除最老的数据(头部),再填入
+        {   //头索引增加,相当于移动到新一个位置上
+            node->front_idx=(node->front_idx+1)%QUEUE_SIZE;
+            node->temp_size--; //相当于出队,size-1
+        }
+        // 将Pub的数据复制到队列的尾部(最新)
+        memcpy(node->queue[node->back_idx],data_ptr,pub->data_len);
+        node->back_idx=(node->back_idx+1)%QUEUE_SIZE; //队列尾部前移
+        node->temp_size++; //入队,size+1
+    }
 }
--- a/modules/message_center/message_center.h
+++ b/modules/message_center/message_center.h
@@ -13,12 +13,12 @@
 #ifndef PUBSUB_H
 #define PUBSUB_H

-#include "stdlib.h"
 #include "stdint-gcc.h"


 #define MAX_EVENT_NAME_LEN  32  //最大的事件名长度,每个事件都有字符串来命名
 #define MAX_EVENT_COUNT 12      //最多支持的事件数量
+#define QUEUE_SIZE 1


 /**
@@ -47,5 +47,69 @@ void SubscribeEvent(char* name,void** data);



-
 /* 以下是队列版的pubsub,TODO */
+
+typedef struct mqt
+{
+    /* 用数组模拟FIFO队列 */
+    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;
+} Subscriber_t;
+
+/**
+ * @brief 发布者类型.每个发布者拥有发布者实例,这个实例
+ * 
+ */
+typedef struct ent  
+{   
+    /* 事件名称 */
+    char event_name[MAX_EVENT_NAME_LEN];
+    uint8_t data_len;
+    /* 指向第一个订阅了该事件的订阅者,通过链表访问所有订阅者 */
+    Subscriber_t* first_subs;
+    /* 指向下一个Publisher的指针 */
+    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);
+
+/**
+ * @brief 注册成为消息发布者
+ * 
+ * @param name 发布者发布的话题名称(事件)
+ * @return Publisher_t* 返回发布者实例
+ */
+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);
+
+/**
+ * @brief 发布者给所有订阅了事件的订阅者推送消息
+ * 
+ * @param pub 发布者实例指针
+ * @param data_ptr 指向要发布的数据的指针
+ */
+void PubPushMessage(Publisher_t* pub,void* data_ptr);
+
+
--- a/modules/motor/dji_motor.md
+++ b/modules/motor/dji_motor.md
@@ -7,8 +7,6 @@
 > 1. 给不同的电机设置不同的低通滤波器惯性系数而不是统一使用宏
 > 2. 为M2006和M3508增加开环的零位校准函数

-
-
 ## 总览和封装说明

 > 如果你不需要理解该模块的工作原理，你只需要查看这一小节。
@@ -392,4 +390,50 @@ static void DecodeDJIMotor(can_instance *_instance)

 - `DecodeDJIMotor()`是解析电机反馈报文的函数，在`DJIMotorInit()`中会将其注册到该电机实例对应的`can_instance`中（即`can_instance`的`can_module_callback()`）。这样，当该电机的反馈报文到达时，`bsp_can.c`中的回调函数会调用解包函数进行反馈数据解析。

-  该函数还会对电流和速度反馈值进行滤波，消除高频噪声；同时计算多圈角度和单圈绝对角度。
+  该函数还会对电流和速度反馈值进行滤波，消除高频噪声；同时计算多圈角度和单圈绝对角度。
+
+## 使用范例
+
+```c
+//初始化设置
+Motor_Init_Config_s config = {
+		.motor_type = GM6020,
+		.can_init_config = {
+			.can_handle = &hcan1,
+			.tx_id = 6
+        },
+		.controller_setting_init_config = {
+            .angle_feedback_source = MOTOR_FEED, 
+            .close_loop_type = SPEED_LOOP | ANGLE_LOOP, 
+            .speed_feedback_source = MOTOR_FEED, 
+            .reverse_flag = MOTOR_DIRECTION_NORMAL
+        },
+		.controller_param_init_config = {
+            .angle_PID = {
+                .Improve = 0, 
+                .Kp = 1, 
+                .Ki = 0, 
+                .Kd = 0, 
+                .DeadBand = 0, 
+                .MaxOut = 4000}, 
+            .speed_PID = {
+                .Improve = 0, 
+                .Kp = 1, 
+                .Ki = 0, 
+                .Kd = 0, 
+                .DeadBand = 0, 
+                .MaxOut = 4000
+            }
+        }
+};
+//注册电机并保存实例指针
+dji_motor_instance *djimotor = DJIMotorInit(&config);
+```
+
+然后在任务中修改电机设定值即可实现控制：
+
+```
+DJIMotorSetRef(djimotor, 10);
+```
+
+前提是已经将`MotorTask()`放入实时系统任务当中。你也可以单独执行`MotorControl()`。