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

将os任务的创建统一转移到app层方便修改,使得架构更加清晰.修复了INS未初始化导致的读取错误

This commit is contained in:
NeoZng 2023-06-05 22:43:25 +08:00
parent 253f391cd5
commit 72884ef96b
9 changed files with 136 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
README.md
View File

@ -3,10 +3,12 @@
> **代码参考了哈工深南宫小樱战队的框架设计和RMUA官方开源RoboRTS-firmware框架，在此鸣谢。**
> 每个bsp/module/application都有对应文档，建议阅读之后再看代码&进行开发。框架的搭建思路和讲解视频戳这里：[basic_framework讲解](https://www.bilibili.com/video/BV1Bd4y1E7CN)。
> 开发之前必看的文档：README.md & VSCode+Ozone使用方法.md + 修改HAL配置时文件目录的更改.md。开发app层请看application目录下的文档，若要开发module以及bsp务必把上层文档也浏览一遍以熟悉接口定义的方式。
> 开发之前必看的文档：**README.md & VSCode+Ozone使用方法.md** 。开发app层请看application目录下的文档，若要开发module以及bsp务必把上层文档也浏览一遍以熟悉接口定义的方式。
> **程序的运行流程和框架所有app/module/bsp的数据流图直接拉到本文档底部。**
此框架为机器人通用设计，当前的app层是为步兵设计的。不同的机器人只需要重新编写应用层。在我们的战队仓库中有英雄、工程、哨兵等兵种，可作参考。
此框架为机器人通用设计，当前的app层是为步兵设计的。不同的机器人只需要重新编写应用层。在我们的战队仓库中有英雄、工程、哨兵、平衡步兵等兵种，可作参考。
此框架在RoboMaster A型开发板的移植也已在组织仓库中提供。
[TOC]

98
Src/freertos.c
View File

@ -25,14 +25,7 @@
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "ins_task.h"
#include "motor_task.h"
#include "led_task.h"
#include "referee_task.h"
#include "master_process.h"
#include "daemon.h"
#include "robot.h"
#include "HT04.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
@ -52,26 +45,13 @@
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
osThreadId insTaskHandle;
osThreadId ledTaskHandle;
osThreadId robotTaskHandle;
osThreadId motorTaskHandle;
osThreadId daemonTaskHandle;
osThreadId uiTaskHandle;
/* USER CODE END Variables */
osThreadId defaultTaskHandle;
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
void StartINSTASK(void const *argument);
void StartMOTORTASK(void const *argument);
void StartDAEMONTASK(void const *argument);
void StartROBOTTASK(void const *argument);
void StartUITASK(void const *argument);
/* USER CODE END FunctionPrototypes */
void StartDefaultTask(void const *argument);
@ -129,21 +109,6 @@ void MX_FREERTOS_Init(void)
/* USER CODE BEGIN RTOS_THREADS */
/* add threads, ... */
osThreadDef(instask, StartINSTASK, osPriorityAboveNormal, 0, 1024);
insTaskHandle = osThreadCreate(osThread(instask), NULL); // 由于是阻塞读取传感器,为姿态解算设置较高优先级,确保以1khz的频率执行
// 后续修改为读取传感器数据准备好的中断处理,
osThreadDef(motortask, StartMOTORTASK, osPriorityNormal, 0, 256);
motorTaskHandle = osThreadCreate(osThread(motortask), NULL);
osThreadDef(daemontask, StartDAEMONTASK, osPriorityNormal, 0, 128);
daemonTaskHandle = osThreadCreate(osThread(daemontask), NULL);
osThreadDef(robottask, StartROBOTTASK, osPriorityNormal, 0, 1024);
defaultTaskHandle = osThreadCreate(osThread(robottask), NULL);
osThreadDef(uitask, StartUITASK, osPriorityNormal, 0, 512);
defaultTaskHandle = osThreadCreate(osThread(uitask), NULL);
/* USER CODE END RTOS_THREADS */
}
@ -158,7 +123,7 @@ void MX_FREERTOS_Init(void)
void StartDefaultTask(void const *argument)
{
/* init code for USB_DEVICE */
MX_USB_DEVICE_Init();
MX_USB_DEVICE_Init(); // USB初始化
/* USER CODE BEGIN StartDefaultTask */
vTaskDelete(NULL); // 删除默认任务,防止占用CPU
/* USER CODE END StartDefaultTask */
@ -166,56 +131,9 @@ void StartDefaultTask(void const *argument)
/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
void StartINSTASK(void const *argument)
{
while (1)
{
// 1kHz
INS_Task();
VisionSend(); // 解算完成后发送视觉数据,但是当前的实现不太优雅,后续若添加硬件触发需要重新考虑结构的组织
osDelay(1);
}
}
void StartMOTORTASK(void const *argument)
{
// 若使用HT电机则取消本行注释,该接口会为注册了的电机设备创建线程
// HTMotorControlInit();
while (1)
{
// 500Hz
MotorControlTask();
osDelay(2);
}
}
void StartDAEMONTASK(void const *argument)
{
while (1)
{
// 100Hz
DaemonTask();
osDelay(10);
}
}
void StartROBOTTASK(void const *argument)
{
while (1)
{
// 200Hz-500Hz,若有额外的控制任务如平衡步兵可能需要提升至1kHz
RobotTask();
osDelay(5);
}
}
void StartUITASK(void const *argument)
{
My_UI_init();
while (1)
{
Referee_Interactive_task(); // 每次给裁判系统发送完一包数据后,挂起一次,防止卡在裁判系统发送中,详见Referee_Interactive_task函数的refereeSend();
osDelay(1); // 即使没有任何UI需要刷新,也挂起一次,防止卡在UITask中无法切换
}
}
/**
* @brief 注意,为方便统一管理,
* 所有的机器人任务都在applicatoin/robot_task.h中进行
*
*/
/* USER CODE END Application */

2
Src/main.c
View File

@ -119,7 +119,7 @@ int main(void)
MX_CRC_Init();
MX_DAC_Init();
/* USER CODE BEGIN 2 */
RobotInit();
RobotInit(); // 唯一的初始化函数
/* USER CODE END 2 */
/* Call init function for freertos objects (in freertos.c) */

9
application/robot.c
View File

@ -1,6 +1,7 @@
#include "bsp_init.h"
#include "robot.h"
#include "robot_def.h"
#include "robot_task.h"
// 编译warning,提醒开发者修改机器人参数
#ifndef ROBOT_DEF_PARAM_WARNING
@ -18,10 +19,6 @@
#include "robot_cmd.h"
#endif
#ifdef BALANCE_BAORD
#include "balance.h"
#endif // BALANCE_BOARD
void RobotInit()
{
@ -42,6 +39,8 @@ void RobotInit()
ChassisInit();
#endif
OSTaskInit(); // 创建基础任务
// 初始化完成,开启中断
__enable_irq();
}
@ -58,4 +57,4 @@ void RobotTask()
ChassisTask();
#endif
}
}

1
application/robot_def.h
View File

@ -20,7 +20,6 @@
#define ONE_BOARD // 单板控制整车
// #define CHASSIS_BOARD //底盘板
// #define GIMBAL_BOARD //云台板
// #define BALANCE_BOARD //启用平衡底盘,则默认双板且当前板位底盘,目前不支持!请勿使用!
#define VISION_USE_VCP // 使用虚拟串口发送视觉数据
// #define VISION_USE_UART // 使用串口发送视觉数据

111
application/robot_task.h Normal file
View File

@ -0,0 +1,111 @@
#pragma once
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"
#include "ins_task.h"
#include "motor_task.h"
#include "referee_task.h"
#include "master_process.h"
#include "daemon.h"
#include "HT04.h"
osThreadId insTaskHandle;
osThreadId robotTaskHandle;
osThreadId motorTaskHandle;
osThreadId daemonTaskHandle;
osThreadId uiTaskHandle;
void StartINSTASK(void const *argument);
void StartMOTORTASK(void const *argument);
void StartDAEMONTASK(void const *argument);
void StartROBOTTASK(void const *argument);
void StartUITASK(void const *argument);
/**
* @brief 初始化机器人任务,所有持续运行的任务都在这里初始化
*
*/
void OSTaskInit()
{
osThreadDef(instask, StartINSTASK, osPriorityAboveNormal, 0, 1024);
insTaskHandle = osThreadCreate(osThread(instask), NULL); // 由于是阻塞读取传感器,为姿态解算设置较高优先级,确保以1khz的频率执行
// 后续修改为读取传感器数据准备好的中断处理,
osThreadDef(motortask, StartMOTORTASK, osPriorityNormal, 0, 256);
motorTaskHandle = osThreadCreate(osThread(motortask), NULL);
osThreadDef(daemontask, StartDAEMONTASK, osPriorityNormal, 0, 128);
daemonTaskHandle = osThreadCreate(osThread(daemontask), NULL);
osThreadDef(robottask, StartROBOTTASK, osPriorityNormal, 0, 1024);
robotTaskHandle = osThreadCreate(osThread(robottask), NULL);
osThreadDef(uitask, StartUITASK, osPriorityNormal, 0, 512);
uiTaskHandle = osThreadCreate(osThread(uitask), NULL);
// 若使用HT电机则取消本行注释,该接口会为注册了的电机设备创建线程
// HTMotorControlInit();
}
void StartINSTASK(void const *argument)
{
static float ins_start, ins_dt;
INS_Init(); // 确保BMI088被正确初始化.
while (1)
{
// 1kHz
ins_start = DWT_GetTimeline_ms();
INS_Task();
ins_dt = DWT_GetTimeline_ms() - ins_start;
VisionSend(); // 解算完成后发送视觉数据,但是当前的实现不太优雅,后续若添加硬件触发需要重新考虑结构的组织
osDelay(1);
}
}
void StartMOTORTASK(void const *argument)
{
static float motor_dt, motor_start;
while (1)
{
motor_start = DWT_GetTimeline_ms();
MotorControlTask();
motor_dt = DWT_GetTimeline_ms() - motor_start;
osDelay(1);
}
}
void StartDAEMONTASK(void const *argument)
{
while (1)
{
// 100Hz
DaemonTask();
osDelay(10);
}
}
void StartROBOTTASK(void const *argument)
{
static float robot_dt, robot_start;
// 200Hz-500Hz,若有额外的控制任务如平衡步兵可能需要提升至1kHz
while (1)
{
robot_start = DWT_GetTimeline_ms();
RobotTask();
robot_dt = DWT_GetTimeline_ms() - robot_start;
osDelay(5);
}
}
void StartUITASK(void const *argument)
{
My_UI_init();
while (1)
{
Referee_Interactive_task(); // 每次给裁判系统发送完一包数据后,挂起一次,防止卡在裁判系统发送中,详见Referee_Interactive_task函数的refereeSend();
osDelay(1); // 即使没有任何UI需要刷新,也挂起一次,防止卡在UITask中无法切换
}
}

2
modules/algorithm/controller.c
View File

@ -150,7 +150,7 @@ float PIDCalculate(PIDInstance *pid, float measure, float ref)
if (pid->Improve & PID_ErrorHandle)
f_PID_ErrorHandle(pid);
pid->dt = DWT_GetDeltaT((void *)&pid->DWT_CNT); // 获取两次pid计算的时间间隔,用于积分和微分
pid->dt = DWT_GetDeltaT(&pid->DWT_CNT); // 获取两次pid计算的时间间隔,用于积分和微分
// 保存上次的测量值和误差,计算当前error
pid->Measure = measure;

5
modules/imu/ins_task.c
View File

@ -74,6 +74,11 @@ static void InitQuaternion(float *init_q4)
attitude_t *INS_Init(void)
{
if (!INS.init)
INS.init = 1;
else
return (attitude_t *)&INS.Gyro;
while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR)
;
IMU_Param.scale[X] = 1;

2
modules/imu/ins_task.h
View File

@ -62,6 +62,8 @@ typedef struct
float Pitch;
float Yaw;
float YawTotalAngle;
uint8_t init;
} INS_t;
/* 用于修正安装误差的参数 */