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

README.md

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

Src/freertos.c

@@ -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)
+/**
-{
+ * @brief 注意,为方便统一管理,
-  while (1)
+ *        所有的机器人任务都在applicatoin/robot_task.h中进行
-  {
+ * 
-    // 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中无法切换
-  }
-}
 /* USER CODE END Application */

Src/main.c

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

application/robot.c

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

application/robot_def.h

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

application/robot_task.h

@@ -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中无法切换
+    }
+}

modules/algorithm/controller.c

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

modules/imu/ins_task.c

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

modules/imu/ins_task.h

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