From bcd7bf00e5e2d978633d64c3c2d45a429bbd5b7c Mon Sep 17 00:00:00 2001 From: NeoZeng Date: Wed, 30 Nov 2022 22:10:57 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E6=88=90=E4=BA=86=E9=98=9F=E5=88=97?= =?UTF-8?q?=E7=89=88=E6=9C=AC=E7=9A=84pubsub=E6=9C=BA=E5=88=B6,=E4=BD=86?= =?UTF-8?q?=E5=B0=9A=E6=9C=AA=E6=B5=8B=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 4 +- .vscode/tasks.json | 4 +- HAL_N_Middlewares/Src/freertos.c | 2 + HAL_N_Middlewares/Src/main.c | 72 ++------------ Makefile | 3 +- README.md | 2 +- VSCode+Ozone使用方法.md | 2 + application/robot.c | 1 + modules/can_comm/can_comm.md | 43 +++++++++ modules/imu/ins_task.c | 108 +++++++++++---------- modules/led_light/led_task.c | 52 ++++------ modules/led_light/led_task.h | 29 +----- modules/message_center/message_center.c | 122 +++++++++++++++++++++++- modules/message_center/message_center.h | 68 ++++++++++++- modules/motor/dji_motor.md | 50 +++++++++- 15 files changed, 369 insertions(+), 193 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index ff85c6b..a55b601 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -21,7 +21,9 @@ "master_process.h": "c", "seasky_protocol.h": "c", "bsp_usart.h": "c", - "message_center.h": "c" + "message_center.h": "c", + "stddef.h": "c", + "stm32_hal_legacy.h": "c" }, "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools", } \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 2f03117..7ef9fdf 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -15,7 +15,7 @@ { "label": "download dap", "type": "shell", // 如果希望在下载前编译,可以把command换成下面的命令 - "command":"make download_dap", // "mingw32-make -j24 && make download_dap", + "command":"mingw32-make download_dap", // "mingw32-make -j24 && mingw32-make download_dap", "group": { // 如果没有修改代码,编译任务不会消耗时间,因此推荐使用上面的替换. "kind": "build", "isDefault": false, @@ -24,7 +24,7 @@ { "label": "download jlink", "type": "shell", - "command":"make download_jlink", + "command":"mingw32-make download_jlink", // "mingw32-make -j24 && mingw32-make download_dap" "group": { "kind": "build", "isDefault": false, diff --git a/HAL_N_Middlewares/Src/freertos.c b/HAL_N_Middlewares/Src/freertos.c index bf3a360..71659f9 100644 --- a/HAL_N_Middlewares/Src/freertos.c +++ b/HAL_N_Middlewares/Src/freertos.c @@ -27,6 +27,7 @@ /* USER CODE BEGIN Includes */ #include "ins_task.h" #include "motor_task.h" +#include "led_task.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -137,6 +138,7 @@ void StartDefaultTask(void const * argument) /* Infinite loop */ for(;;) { + led_RGB_flow_task(); osDelay(1); } /* USER CODE END StartDefaultTask */ diff --git a/HAL_N_Middlewares/Src/main.c b/HAL_N_Middlewares/Src/main.c index 88a5292..b89d655 100644 --- a/HAL_N_Middlewares/Src/main.c +++ b/HAL_N_Middlewares/Src/main.c @@ -32,19 +32,7 @@ /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ -#include "remote_control.h" -#include "bsp_usart.h" -#include "bsp_can.h" -#include "can.h" -#include "dji_motor.h" -#include "motor_task.h" -#include "referee.h" -#include "ins_task.h" -#include "can_comm.h" -#include "master_process.h" -#include "led_task.h" -#include "bsp_led.h" -#include "message_center.h" +#include "robot.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -76,13 +64,7 @@ void MX_FREERTOS_Init(void); /* Private user code ---------------------------------------------------------*/ /* USER CODE BEGIN 0 */ -typedef struct -{ - float a; - float b; - float c; -}sdf; -volatile sdf* rx; + /* USER CODE END 0 */ /** @@ -128,64 +110,24 @@ int main(void) MX_USART1_UART_Init(); MX_USART6_UART_Init(); /* USER CODE BEGIN 2 */ - RC_init(&huart3); - DWT_Init(168); - - 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); - CANComm_Init_Config_s cconfig = { - .can_config = {.can_handle=&hcan1,.tx_id=0x02,.rx_id=0x03}, - .send_data_len = 4, - .recv_data_len = 12}; - CANCommInstance* in = CANCommInit(&cconfig); - volatile float tx = 32; - - RefereeInit(&huart6); - Vision_Recv_s* recv=VisionInit(&huart1); - Vision_Send_s send={.pitch=1,.roll=2,.yaw=3}; - LED_init(); + RobotInit(); - - float *subsub; - float* sub2; - float test=1; - PublisherRegister("test",&test); - SubscribeEvent("test",&subsub); - SubscribeEvent("test",&sub2); - PublisherRegister("test",sub2); - MessageInit(); /* USER CODE END 2 */ /* Call init function for freertos objects (in freertos.c) */ - // MX_FREERTOS_Init(); + MX_FREERTOS_Init(); - // /* Start scheduler */ - // osKernelStart(); + /* Start scheduler */ + osKernelStart(); /* We should never get here as control is now taken by the scheduler */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ - // INS_Init(); - while (1) { /* USER CODE END WHILE */ - DJIMotorSetRef(djimotor, 10); - MotorControlTask(); - HAL_Delay(1); - CANCommSend(in, (uint8_t*)&tx); - tx+=1; - rx=(sdf*)CANCommGet(in); - VisionSend(&send); - led_RGB_flow_task(); - // INS_Task(); + /* USER CODE BEGIN 3 */ } /* USER CODE END 3 */ diff --git a/Makefile b/Makefile index 9c21a48..1a54f43 100644 --- a/Makefile +++ b/Makefile @@ -294,7 +294,8 @@ $(BUILD_DIR): # clean up ####################################### clean: - rmdir /q $(BUILD_DIR) + rd /s/q $(BUILD_DIR) +# linux: rm -rf $(BUILD_DIR) ####################################### # download without debugging diff --git a/README.md b/README.md index 162218b..8ce7226 100644 --- a/README.md +++ b/README.md @@ -14,7 +14,7 @@ VSCode可通过Cortex-Debug利用OpenOCD进行调试，jlink/stlink/dap-link都支持，具体的使用方法和环境配置教程在[VSCode+Ozone使用方法](./VSCode+Ozone使用方法.md)中。 - 推荐使用 SEGGER ozone进行调试。 + 推荐使用 **SEGGER ozone** 进行调试。 - 分层： diff --git a/VSCode+Ozone使用方法.md b/VSCode+Ozone使用方法.md index 55783e4..a1cd6a2 100644 --- a/VSCode+Ozone使用方法.md +++ b/VSCode+Ozone使用方法.md @@ -196,6 +196,8 @@ ITM是instrument trace macrocell指令追踪宏单元的缩写，它用于提供 图片看不清请打开原图。验证安装： 打开命令行（win+R，cmd，回车），输入`gcc -v`，如果没有报错，并输出了一堆路径和参数说明安装成功。 + + 安装完之后，建议将ming的bin文件夹下的ming32-make.exe复制一份，并将copy更名为make.exe - 配置gcc-arm-none-eabi环境变量，**把压缩包解压以后放在某个地方**，然后同上，将工具链的bin添加到PATH：（will be deprecated soon，请注意这种方法将会在主分支发布正式版的时候删除） diff --git a/application/robot.c b/application/robot.c index 5d342f8..59615d3 100644 --- a/application/robot.c +++ b/application/robot.c @@ -11,6 +11,7 @@ void RobotInit() { + #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) #endif diff --git a/modules/can_comm/can_comm.md b/modules/can_comm/can_comm.md index 607ac98..7db0f77 100644 --- 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的通信协议如下： diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index 7aed32f..33f6b45 100644 --- 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); } /** diff --git a/modules/led_light/led_task.c b/modules/led_light/led_task.c index a7d1388..37ef7f8 100644 --- 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 #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); + } diff --git a/modules/led_light/led_task.h b/modules/led_light/led_task.h index 512f65c..6769ec8 100644 --- 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��Ч�� - * @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 - -/** - * @brief led rgb task - * @param[in] pvParameters: NULL - * @retval none - */ -/** - * @brief led RGB���� - * @param[in] pvParameters: NULL - * @retval none - */ -extern void led_RGB_flow_task(); +void led_RGB_flow_task(); #endif diff --git a/modules/message_center/message_center.c b/modules/message_center/message_center.c index 848a221..46b637f 100644 --- 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 + } } \ No newline at end of file diff --git a/modules/message_center/message_center.h b/modules/message_center/message_center.h index 957646b..2dcd25d 100644 --- 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); + + diff --git a/modules/motor/dji_motor.md b/modules/motor/dji_motor.md index 97c5a46..ba532c7 100644 --- 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`中的回调函数会调用解包函数进行反馈数据解析。 - 该函数还会对电流和速度反馈值进行滤波，消除高频噪声；同时计算多圈角度和单圈绝对角度。 \ No newline at end of file + 该函数还会对电流和速度反馈值进行滤波，消除高频噪声；同时计算多圈角度和单圈绝对角度。 + +## 使用范例 + +```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()`。 \ No newline at end of file