242 lines
7.4 KiB
C
242 lines
7.4 KiB
C
/* USER CODE BEGIN Header */
|
|
/**
|
|
******************************************************************************
|
|
* File Name : freertos.c
|
|
* Description : Code for freertos applications
|
|
******************************************************************************
|
|
* @attention
|
|
*
|
|
* Copyright (c) 2026 STMicroelectronics.
|
|
* All rights reserved.
|
|
*
|
|
* This software is licensed under terms that can be found in the LICENSE file
|
|
* in the root directory of this software component.
|
|
* If no LICENSE file comes with this software, it is provided AS-IS.
|
|
*
|
|
******************************************************************************
|
|
*/
|
|
/* USER CODE END Header */
|
|
|
|
/* Includes ------------------------------------------------------------------*/
|
|
#include "FreeRTOS.h"
|
|
#include "task.h"
|
|
#include "main.h"
|
|
#include "cmsis_os.h"
|
|
|
|
/* Private includes ----------------------------------------------------------*/
|
|
/* USER CODE BEGIN Includes */
|
|
#include "bsp_can.h"
|
|
#include "CAN_receive.h"
|
|
#include "chassis_control.h"
|
|
#include "bsp_rc.h"
|
|
#include "remote_control.h"
|
|
/* USER CODE END Includes */
|
|
|
|
/* Private typedef -----------------------------------------------------------*/
|
|
typedef StaticQueue_t osStaticMessageQDef_t;
|
|
/* USER CODE BEGIN PTD */
|
|
|
|
/* USER CODE END PTD */
|
|
|
|
/* Private define ------------------------------------------------------------*/
|
|
/* USER CODE BEGIN PD */
|
|
|
|
/* USER CODE END PD */
|
|
|
|
/* Private macro -------------------------------------------------------------*/
|
|
/* USER CODE BEGIN PM */
|
|
|
|
/* USER CODE END PM */
|
|
|
|
/* Private variables ---------------------------------------------------------*/
|
|
/* USER CODE BEGIN Variables */
|
|
|
|
/* USER CODE END Variables */
|
|
/* Definitions for CAN_Receive_Tas */
|
|
osThreadId_t CAN_Receive_TasHandle;
|
|
const osThreadAttr_t CAN_Receive_Tas_attributes = {
|
|
.name = "CAN_Receive_Tas",
|
|
.stack_size = 128 * 4,
|
|
.priority = (osPriority_t) osPriorityHigh,
|
|
};
|
|
/* Definitions for Chassis_Control */
|
|
osThreadId_t Chassis_ControlHandle;
|
|
const osThreadAttr_t Chassis_Control_attributes = {
|
|
.name = "Chassis_Control",
|
|
.stack_size = 128 * 4,
|
|
.priority = (osPriority_t) osPriorityNormal,
|
|
};
|
|
/* Definitions for Command_Task */
|
|
osThreadId_t Command_TaskHandle;
|
|
const osThreadAttr_t Command_Task_attributes = {
|
|
.name = "Command_Task",
|
|
.stack_size = 128 * 4,
|
|
.priority = (osPriority_t) osPriorityNormal,
|
|
};
|
|
/* Definitions for chassis_speed_queue */
|
|
osMessageQueueId_t chassis_speed_queueHandle;
|
|
uint8_t chassis_speed_queueBuffer[ 5 * sizeof( Chassis_Speed ) ]; // 队列缓冲区：5个Chassis_Speed(vx,vy,vw)
|
|
osStaticMessageQDef_t chassis_speed_queueControlBlock;
|
|
const osMessageQueueAttr_t chassis_speed_queue_attributes = {
|
|
.name = "chassis_speed_queue",
|
|
.cb_mem = &chassis_speed_queueControlBlock,
|
|
.cb_size = sizeof(chassis_speed_queueControlBlock),
|
|
.mq_mem = &chassis_speed_queueBuffer,
|
|
.mq_size = sizeof(chassis_speed_queueBuffer)
|
|
};
|
|
|
|
/* Private function prototypes -----------------------------------------------*/
|
|
/* USER CODE BEGIN FunctionPrototypes */
|
|
|
|
/* USER CODE END FunctionPrototypes */
|
|
|
|
void StartCAN_Receive_Task(void *argument);
|
|
void StartChassis_Control_Task(void *argument);
|
|
void StartCommand_Task(void *argument);
|
|
|
|
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
|
|
|
|
/**
|
|
* @brief FreeRTOS initialization
|
|
* @param None
|
|
* @retval None
|
|
*/
|
|
void MX_FREERTOS_Init(void) {
|
|
/* USER CODE BEGIN Init */
|
|
|
|
/* USER CODE END Init */
|
|
|
|
/* USER CODE BEGIN RTOS_MUTEX */
|
|
/* add mutexes, ... */
|
|
/* USER CODE END RTOS_MUTEX */
|
|
|
|
/* USER CODE BEGIN RTOS_SEMAPHORES */
|
|
/* add semaphores, ... */
|
|
/* USER CODE END RTOS_SEMAPHORES */
|
|
|
|
/* USER CODE BEGIN RTOS_TIMERS */
|
|
/* start timers, add new ones, ... */
|
|
/* USER CODE END RTOS_TIMERS */
|
|
|
|
/* Create the queue(s) */
|
|
/* creation of chassis_speed_queue */
|
|
chassis_speed_queueHandle = osMessageQueueNew (5, sizeof(Chassis_Speed), &chassis_speed_queue_attributes);
|
|
|
|
/* USER CODE BEGIN RTOS_QUEUES */
|
|
/* add queues, ... */
|
|
/* USER CODE END RTOS_QUEUES */
|
|
|
|
/* Create the thread(s) */
|
|
/* creation of CAN_Receive_Tas */
|
|
CAN_Receive_TasHandle = osThreadNew(StartCAN_Receive_Task, NULL, &CAN_Receive_Tas_attributes);
|
|
|
|
/* creation of Chassis_Control */
|
|
Chassis_ControlHandle = osThreadNew(StartChassis_Control_Task, NULL, &Chassis_Control_attributes);
|
|
|
|
/* creation of Command_Task */
|
|
Command_TaskHandle = osThreadNew(StartCommand_Task, NULL, &Command_Task_attributes);
|
|
|
|
/* USER CODE BEGIN RTOS_THREADS */
|
|
/* add threads, ... */
|
|
/* USER CODE END RTOS_THREADS */
|
|
|
|
/* USER CODE BEGIN RTOS_EVENTS */
|
|
/* add events, ... */
|
|
/* USER CODE END RTOS_EVENTS */
|
|
|
|
}
|
|
|
|
/* USER CODE BEGIN Header_StartCAN_Receive_Task */
|
|
/**
|
|
* @brief Function implementing the CAN_Receive_Tas thread.
|
|
* @param argument: Not used
|
|
* @retval None
|
|
*/
|
|
/* USER CODE END Header_StartCAN_Receive_Task */
|
|
void StartCAN_Receive_Task(void *argument)
|
|
{
|
|
/* USER CODE BEGIN StartCAN_Receive_Task */
|
|
|
|
/* Infinite loop */
|
|
for(;;)
|
|
{
|
|
osDelay(1);
|
|
}
|
|
/* USER CODE END StartCAN_Receive_Task */
|
|
}
|
|
|
|
/* USER CODE BEGIN Header_StartChassis_Control_Task */
|
|
/**
|
|
* @brief Function implementing the Chassis_Control thread.
|
|
* @param argument: Not used
|
|
* @retval None
|
|
*/
|
|
/* USER CODE END Header_StartChassis_Control_Task */
|
|
void StartChassis_Control_Task(void *argument)
|
|
{
|
|
/* USER CODE BEGIN StartChassis_Control_Task */
|
|
chassis_control_init(); // 初始化底盘电机PID
|
|
Chassis_Speed speed; // 接收遥控器下发的目标速度
|
|
|
|
/* Infinite loop */
|
|
for(;;)
|
|
{
|
|
// 非阻塞接收遥控器速度指令，有新的就更新目标速度
|
|
if (osMessageQueueGet(chassis_speed_queueHandle, &speed, NULL, 0) == osOK)
|
|
{
|
|
chassis_set_target_speed(&speed);
|
|
}
|
|
// 执行底盘控制：运动学解算 + PID计算 + CAN发送
|
|
chassis_control_task();
|
|
osDelay(1); // 1ms 控制周期
|
|
}
|
|
/* USER CODE END StartChassis_Control_Task */
|
|
}
|
|
|
|
/* USER CODE BEGIN Header_StartCommand_Task */
|
|
/**
|
|
* @brief Function implementing the Command_Task thread.
|
|
* @param argument: Not used
|
|
* @retval None
|
|
*/
|
|
/* USER CODE END Header_StartCommand_Task */
|
|
void StartCommand_Task(void *argument)
|
|
{
|
|
/* USER CODE BEGIN StartCommand_Task */
|
|
remote_control_init();
|
|
const RC_ctrl_t *rc;
|
|
Chassis_Speed speed;
|
|
/* Infinite loop */
|
|
for(;;)
|
|
{
|
|
rc = get_remote_control_point(); // 获取遥控器数据
|
|
|
|
// 遥控器通道映射：ch[3]=左摇杆上下(前后), ch[2]=左摇杆左右(左右)
|
|
// 范围 [-660, 660] 映射到 [-MAX_CHASSIS_SPEED, MAX_CHASSIS_SPEED] m/s
|
|
// 摇杆死区：RC_DEADZONE 以内视为0，消除摇杆中位抖动
|
|
speed.vx = (abs(rc->rc.ch[2]) > RC_DEADZONE) ? (float)rc->rc.ch[3] * MAX_CHASSIS_SPEED / 660.0f : 0.0f; // 前后速度
|
|
speed.vy = (abs(rc->rc.ch[3]) > RC_DEADZONE) ? (float)rc->rc.ch[2] * MAX_CHASSIS_SPEED / 660.0f : 0.0f; // 左右速度
|
|
speed.vw = 0; // 旋转速度默认为0
|
|
|
|
// 右侧开关切换模式
|
|
if (rc->rc.s[1] == RC_SW_UP)
|
|
{
|
|
speed.vw = -2.0f; // 上拨：小陀螺模式，原地旋转
|
|
}
|
|
else if (rc->rc.s[1] == RC_SW_DOWN)
|
|
{
|
|
speed.vx = 0; // 下拨：停止模式，所有速度置零
|
|
speed.vy = 0;
|
|
speed.vw = 0;
|
|
}
|
|
|
|
osMessageQueuePut(chassis_speed_queueHandle, &speed, 0, 0); // 发送速度到底盘控制任务
|
|
osDelay(10);
|
|
}
|
|
/* USER CODE END StartCommand_Task */
|
|
}
|
|
|
|
/* Private application code --------------------------------------------------*/
|
|
/* USER CODE BEGIN Application */
|
|
|
|
/* USER CODE END Application */ |