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",
"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",
}

4
.vscode/tasks.json vendored
View File

@ -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,

2
HAL_N_Middlewares/Src/freertos.c
View File

@ -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 */

72
HAL_N_Middlewares/Src/main.c
View File

@ -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);
RobotInit();
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();
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 */

3
Makefile
View File

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

2
README.md
View File

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

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

@ -197,6 +197,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，请注意这种方法将会在主分支发布正式版的时候删除）
![image-20221112172858593](assets\image-20221112172858593.png)

1
application/robot.c
View File

@ -11,6 +11,7 @@
void RobotInit()
{
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
#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接收中断，进行协议解析。
## 使用范例
例如，这里要发送的数据是一个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的通信协议如下：

4
modules/imu/ins_task.c
View File

@ -80,6 +80,8 @@ void INS_Task(void)
{
static uint32_t count = 0;
const float gravity[3] = {0, 0, 9.81f};
while (1)
{
dt = DWT_GetDeltaT(&INS_DWT_Count);
t += dt;
@ -140,6 +142,8 @@ void INS_Task(void)
// 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
}
}
osDelay(1);
}
/**
* @brief Transform 3dvector from BodyFrame to EarthFrame

50
modules/led_light/led_task.c
View File

@ -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
* @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
*
@verbatim
==============================================================================
==============================================================================
@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);
}

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
#define LED_TRIGGER_TASK_H
#include <stdint-gcc.h>
/**
* @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();
void led_RGB_flow_task();
#endif

122
modules/message_center/message_center.c
View File

@ -1,4 +1,5 @@
#include "message_center.h"
#include "stdlib.h"
#include "string.h"
/* 消息初始化用 */
@ -14,24 +15,29 @@ 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;
}
}
}
/* 传入数据地址 */
void PublisherRegister(char* name,void* data)
@ -53,3 +59,109 @@ void SubscribeEvent(char* name,void** data_ptr)
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
}
}

68
modules/message_center/message_center.h
View File

@ -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);

48
modules/motor/dji_motor.md
View File

@ -7,8 +7,6 @@
> 1. 给不同的电机设置不同的低通滤波器惯性系数而不是统一使用宏
> 2. 为M2006和M3508增加开环的零位校准函数
## 总览和封装说明
> 如果你不需要理解该模块的工作原理，你只需要查看这一小节。
@ -393,3 +391,49 @@ 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()`。