HFUT_Firmament_EC
/
sentry_chassis_hzz
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

This commit is contained in:
NeoZeng 2022-11-30 22:10:57 +08:00
parent 32ae8eaa7f
commit bcd7bf00e5
15 changed files with 369 additions and 193 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.vscode/settings.json vendored
View File

@ -21,7 +21,9 @@
"master_process.h": "c", "master_process.h": "c",
"seasky_protocol.h": "c", "seasky_protocol.h": "c",
"bsp_usart.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", "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools",
} }

4
.vscode/tasks.json vendored
View File

@ -15,7 +15,7 @@
{ {
"label": "download dap", "label": "download dap",
"type": "shell", // 如果希望在下载前编译,可以把command换成下面的命令 "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": { // 如果没有修改代码,编译任务不会消耗时间,因此推荐使用上面的替换. "group": { // 如果没有修改代码,编译任务不会消耗时间,因此推荐使用上面的替换.
"kind": "build", "kind": "build",
"isDefault": false, "isDefault": false,
@ -24,7 +24,7 @@
{ {
"label": "download jlink", "label": "download jlink",
"type": "shell", "type": "shell",
"command":"make download_jlink", "command":"mingw32-make download_jlink", // "mingw32-make -j24 && mingw32-make download_dap"
"group": { "group": {
"kind": "build", "kind": "build",
"isDefault": false, "isDefault": false,

2
HAL_N_Middlewares/Src/freertos.c
View File

@ -27,6 +27,7 @@
/* USER CODE BEGIN Includes */ /* USER CODE BEGIN Includes */
#include "ins_task.h" #include "ins_task.h"
#include "motor_task.h" #include "motor_task.h"
#include "led_task.h"
/* USER CODE END Includes */ /* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/
@ -137,6 +138,7 @@ void StartDefaultTask(void const * argument)
/* Infinite loop */ /* Infinite loop */
for(;;) for(;;)
{ {
led_RGB_flow_task();
osDelay(1); osDelay(1);
} }
/* USER CODE END StartDefaultTask */ /* USER CODE END StartDefaultTask */

72
HAL_N_Middlewares/Src/main.c
View File

@ -32,19 +32,7 @@
/* Private includes ----------------------------------------------------------*/ /* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */ /* USER CODE BEGIN Includes */
#include "remote_control.h" #include "robot.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"
/* USER CODE END Includes */ /* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/
@ -76,13 +64,7 @@ void MX_FREERTOS_Init(void);
/* Private user code ---------------------------------------------------------*/ /* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */ /* USER CODE BEGIN 0 */
typedef struct
{
float a;
float b;
float c;
}sdf;
volatile sdf* rx;
/* USER CODE END 0 */ /* USER CODE END 0 */
/** /**
@ -128,64 +110,24 @@ int main(void)
MX_USART1_UART_Init(); MX_USART1_UART_Init();
MX_USART6_UART_Init(); MX_USART6_UART_Init();
/* USER CODE BEGIN 2 */ /* 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 = { RobotInit();
.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();
float *subsub;
float* sub2;
float test=1;
PublisherRegister("test",&test);
SubscribeEvent("test",&subsub);
SubscribeEvent("test",&sub2);
PublisherRegister("test",sub2);
MessageInit();
/* USER CODE END 2 */ /* USER CODE END 2 */
/* Call init function for freertos objects (in freertos.c) */ /* Call init function for freertos objects (in freertos.c) */
// MX_FREERTOS_Init(); MX_FREERTOS_Init();
// /* Start scheduler */ /* Start scheduler */
// osKernelStart(); osKernelStart();
/* We should never get here as control is now taken by the scheduler */ /* We should never get here as control is now taken by the scheduler */
/* Infinite loop */ /* Infinite loop */
/* USER CODE BEGIN WHILE */ /* USER CODE BEGIN WHILE */
// INS_Init();
while (1) while (1)
{ {
/* USER CODE END WHILE */ /* 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 BEGIN 3 */
} }
/* USER CODE END 3 */ /* USER CODE END 3 */

3
Makefile
View File

@ -294,7 +294,8 @@ $(BUILD_DIR):
# clean up # clean up
####################################### #######################################
clean: clean:
rmdir /q $(BUILD_DIR) rd /s/q $(BUILD_DIR)
# linux: rm -rf $(BUILD_DIR)
####################################### #######################################
# download without debugging # download without debugging

2
README.md
View File

@ -14,7 +14,7 @@
VSCode可通过Cortex-Debug利用OpenOCD进行调试，jlink/stlink/dap-link都支持，具体的使用方法和环境配置教程在[VSCode+Ozone使用方法](./VSCode+Ozone使用方法.md)中。 VSCode可通过Cortex-Debug利用OpenOCD进行调试，jlink/stlink/dap-link都支持，具体的使用方法和环境配置教程在[VSCode+Ozone使用方法](./VSCode+Ozone使用方法.md)中。
推荐使用 SEGGER ozone进行调试。 推荐使用 **SEGGER ozone** 进行调试。
- 分层： - 分层：

2
VSCode+Ozone使用方法.md
View File

@ -196,6 +196,8 @@ ITM是instrument trace macrocell指令追踪宏单元的缩写，它用于提供
图片看不清请打开原图。验证安装： 图片看不清请打开原图。验证安装：
打开命令行（win+R，cmd，回车），输入`gcc -v`，如果没有报错，并输出了一堆路径和参数说明安装成功。 打开命令行（win+R，cmd，回车），输入`gcc -v`，如果没有报错，并输出了一堆路径和参数说明安装成功。
安装完之后，建议将ming的bin文件夹下的ming32-make.exe复制一份，并将copy更名为make.exe
- 配置gcc-arm-none-eabi环境变量，**把压缩包解压以后放在某个地方**，然后同上，将工具链的bin添加到PATH：（will be deprecated soon，请注意这种方法将会在主分支发布正式版的时候删除） - 配置gcc-arm-none-eabi环境变量，**把压缩包解压以后放在某个地方**，然后同上，将工具链的bin添加到PATH：（will be deprecated soon，请注意这种方法将会在主分支发布正式版的时候删除）

1
application/robot.c
View File

@ -11,6 +11,7 @@
void RobotInit() void RobotInit()
{ {
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD) #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
#endif #endif

43
modules/can_comm/can_comm.md
View File

@ -81,6 +81,49 @@ static void CANCommRxCallback(can_instance *_instance);
`CANCommRxCallback()`是CAN comm初始化can实例时的回调函数，用于can接收中断，进行协议解析。 `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的通信协议如下： CAN comm的通信协议如下：

108
modules/imu/ins_task.c
View File

@ -80,65 +80,69 @@ void INS_Task(void)
{ {
static uint32_t count = 0; static uint32_t count = 0;
const float gravity[3] = {0, 0, 9.81f}; const float gravity[3] = {0, 0, 9.81f};
dt = DWT_GetDeltaT(&INS_DWT_Count); while (1)
t += dt;
// ins update
if ((count % 1) == 0)
{ {
BMI088_Read(&BMI088); dt = DWT_GetDeltaT(&INS_DWT_Count);
t += dt;
INS.Accel[X] = BMI088.Accel[X]; // ins update
INS.Accel[Y] = BMI088.Accel[Y]; if ((count % 1) == 0)
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); 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
// 获取最终数据 // temperature control
INS.Yaw = QEKF_INS.Yaw; if ((count % 2) == 0)
INS.Pitch = QEKF_INS.Pitch; {
INS.Roll = QEKF_INS.Roll; // 500hz
INS.YawTotalAngle = QEKF_INS.YawTotalAngle; IMU_Temperature_Ctrl();
} }
// temperature control if ((count++ % 1000) == 0)
if ((count % 2) == 0) {
{ // 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
// 500hz }
IMU_Temperature_Ctrl();
}
if ((count++ % 1000) == 0)
{
// 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
} }
osDelay(1);
} }
/** /**

52
modules/led_light/led_task.c
View File

@ -1,48 +1,34 @@
/** /**
****************************(C) COPYRIGHT 2019 DJI**************************** * @file led_task.c
* @file led_trigger_task.c/h * @author DJI RM
* @brief led RGB show.led RGB灯效。 * @author modified by NeoZeng neozng1@hnu.edu.cn
* @note * @brief 流水灯效
* @history * @version 0.1
* Version Date Author Modification * @date 2022-11-30
* V1.0.0 Nov-11-2019 RM 1. rgb led *
* * @copyright Copyright (c) 2022
@verbatim *
============================================================================== */
==============================================================================
@endverbatim
****************************(C) COPYRIGHT 2019 DJI****************************
*/
#include "led_task.h" #include "led_task.h"
#include "bsp_led.h" #include "bsp_led.h"
#include "cmsis_os.h"
#include "main.h" #include "main.h"
#include <stdint-gcc.h>
#define RGB_FLOW_COLOR_CHANGE_TIME 1000 #define RGB_FLOW_COLOR_CHANGE_TIME 1000
#define RGB_FLOW_COLOR_LENGHT 6 #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}; 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() void led_RGB_flow_task()
{ {
uint16_t i, j; static float delta_alpha, delta_red, delta_green, delta_blue;
float delta_alpha, delta_red, delta_green, delta_blue; static float alpha, red, green, blue;
float alpha, red, green, blue; static uint32_t aRGB;
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; alpha = (RGB_flow_color[i] & 0xFF000000) >> 24;
red = ((RGB_flow_color[i] & 0x00FF0000) >> 16); red = ((RGB_flow_color[i] & 0x00FF0000) >> 16);
@ -58,7 +44,7 @@ void led_RGB_flow_task()
delta_red /= RGB_FLOW_COLOR_CHANGE_TIME; delta_red /= RGB_FLOW_COLOR_CHANGE_TIME;
delta_green /= RGB_FLOW_COLOR_CHANGE_TIME; delta_green /= RGB_FLOW_COLOR_CHANGE_TIME;
delta_blue /= 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; alpha += delta_alpha;
red += delta_red; red += delta_red;
@ -69,5 +55,5 @@ void led_RGB_flow_task()
aRGB_led_show(aRGB); aRGB_led_show(aRGB);
} }
} }
HAL_Delay(1);
} }

29
modules/led_light/led_task.h
View File

@ -1,34 +1,7 @@
/**
****************************(C) COPYRIGHT 2019 DJI****************************
* @file led_trigger_task.c/h
* @brief led RGB show.led RGB<EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD>
* @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 #ifndef LED_TRIGGER_TASK_H
#define LED_TRIGGER_TASK_H #define LED_TRIGGER_TASK_H
#include <stdint-gcc.h> void led_RGB_flow_task();
/**
* @brief led rgb task
* @param[in] pvParameters: NULL
* @retval none
*/
/**
* @brief led RGB<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param[in] pvParameters: NULL
* @retval none
*/
extern void led_RGB_flow_task();
#endif #endif

122
modules/message_center/message_center.c
View File

@ -1,4 +1,5 @@
#include "message_center.h" #include "message_center.h"
#include "stdlib.h"
#include "string.h" #include "string.h"
/* 消息初始化用 */ /* 消息初始化用 */
@ -14,22 +15,27 @@ void MessageInit()
for (size_t i = 0; i < MAX_EVENT_COUNT; i++) for (size_t i = 0; i < MAX_EVENT_COUNT; i++)
{ {
if(s_pptr[i]!=NULL) if(s_pptr[i]!=NULL)
{
for (size_t j = 0; j < MAX_EVENT_COUNT; j++) //遍历publisher for (size_t j = 0; j < MAX_EVENT_COUNT; j++) //遍历publisher
{ {
if(p_ptr[j]!=NULL) //不为空 if(p_ptr[j]!=NULL) //不为空
if(strcmp(&sname[i],pname[j])==0) //比较消息名是否一致 {
{volatile size_t ss=strlen(sname[i]); if(strcmp(sname[i],pname[j])==0) //比较消息名是否一致
volatile size_t sss=strlen(pname[j]); {
*s_pptr[i]=p_ptr[j]; // 将sub的指针指向pub的数据 *s_pptr[i]=p_ptr[j]; // 将sub的指针指向pub的数据
break; break;
} }
}
else //到结尾,退出 else //到结尾,退出
{
while(1); //如果你卡在这里,说明没有找到消息发布者!请确认消息名称是否键入错误 while(1); //如果你卡在这里,说明没有找到消息发布者!请确认消息名称是否键入错误
}
} }
}
else //说明已经遍历完所有的subs else //说明已经遍历完所有的subs
{
break; break;
}
} }
} }
@ -52,4 +58,110 @@ void SubscribeEvent(char* name,void** data_ptr)
static uint8_t idx; static uint8_t idx;
strcpy(sname[idx],name); strcpy(sname[idx],name);
s_pptr[idx++]=data_ptr; 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
}
} }

68
modules/message_center/message_center.h
View File

@ -13,12 +13,12 @@
#ifndef PUBSUB_H #ifndef PUBSUB_H
#define PUBSUB_H #define PUBSUB_H
#include "stdlib.h"
#include "stdint-gcc.h" #include "stdint-gcc.h"
#define MAX_EVENT_NAME_LEN 32 //最大的事件名长度,每个事件都有字符串来命名 #define MAX_EVENT_NAME_LEN 32 //最大的事件名长度,每个事件都有字符串来命名
#define MAX_EVENT_COUNT 12 //最多支持的事件数量 #define MAX_EVENT_COUNT 12 //最多支持的事件数量
#define QUEUE_SIZE 1
/** /**
@ -47,5 +47,69 @@ void SubscribeEvent(char* name,void** data);
/* 以下是队列版的pubsub,TODO */ /* 以下是队列版的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);

50
modules/motor/dji_motor.md
View File

@ -7,8 +7,6 @@
> 1. 给不同的电机设置不同的低通滤波器惯性系数而不是统一使用宏 > 1. 给不同的电机设置不同的低通滤波器惯性系数而不是统一使用宏
> 2. 为M2006和M3508增加开环的零位校准函数 > 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`中的回调函数会调用解包函数进行反馈数据解析。 - `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()`。