SkyLiCoder
da6acdcc89
Manual mode now receives angular velocity (degrees/s) via 0xA6 frame instead of absolute angle reference. The firmware integrates velocity * dt into gimbal angle ref. Motors are no longer auto-enabled at DMMotorInit — they stay disabled until a mode switch out of DEBUG. Extract ROBOT_CMD_DELAY macro to replace magic number. Fix "volity" typo → "velocity" and update docs/protocol comments accordingly.
119 lines
3.1 KiB
C
119 lines
3.1 KiB
C
#pragma once
|
|
|
|
#include "FreeRTOS.h"
|
|
#include "task.h"
|
|
#include "main.h"
|
|
#include "cmsis_os.h"
|
|
|
|
#include "robot.h"
|
|
#include "ins_task.h"
|
|
#include "motor_task.h"
|
|
#include "daemon.h"
|
|
#include "buzzer.h"
|
|
|
|
#include "bsp_log.h"
|
|
|
|
osThreadId_t insTaskHandle;
|
|
osThreadId_t robotTaskHandle;
|
|
osThreadId_t motorTaskHandle;
|
|
osThreadId_t daemonTaskHandle;
|
|
|
|
void StartINSTASK(void *argument);
|
|
void StartMOTORTASK(void *argument);
|
|
void StartDAEMONTASK(void *argument);
|
|
void StartROBOTTASK(void *argument);
|
|
|
|
void OSTaskInit()
|
|
{
|
|
const osThreadAttr_t insTask_attr = {
|
|
.name = "instask",
|
|
.priority = osPriorityAboveNormal,
|
|
.stack_size = 1024,
|
|
};
|
|
insTaskHandle = osThreadNew(StartINSTASK, NULL, &insTask_attr);
|
|
|
|
const osThreadAttr_t motorTask_attr = {
|
|
.name = "motortask",
|
|
.priority = osPriorityNormal,
|
|
.stack_size = 1024,
|
|
};
|
|
motorTaskHandle = osThreadNew(StartMOTORTASK, NULL, &motorTask_attr);
|
|
|
|
const osThreadAttr_t daemonTask_attr = {
|
|
.name = "daemontask",
|
|
.priority = osPriorityNormal,
|
|
.stack_size = 512,
|
|
};
|
|
daemonTaskHandle = osThreadNew(StartDAEMONTASK, NULL, &daemonTask_attr);
|
|
|
|
const osThreadAttr_t robotTask_attr = {
|
|
.name = "robottask",
|
|
.priority = osPriorityNormal,
|
|
.stack_size = 1024,
|
|
};
|
|
robotTaskHandle = osThreadNew(StartROBOTTASK, NULL, &robotTask_attr);
|
|
}
|
|
|
|
__attribute__((noreturn)) void StartINSTASK(void *argument)
|
|
{
|
|
static float ins_start, ins_dt;
|
|
INS_Init();
|
|
LOGINFO("[freeRTOS] INS Task Start");
|
|
for (;;)
|
|
{
|
|
ins_start = DWT_GetTimeline_ms();
|
|
INS_Task();
|
|
ins_dt = DWT_GetTimeline_ms() - ins_start;
|
|
if (ins_dt > 1)
|
|
LOGERROR("[freeRTOS] INS Task DELAY dt=%.3f", ins_dt);
|
|
osDelay(1);
|
|
}
|
|
}
|
|
|
|
__attribute__((noreturn)) void StartMOTORTASK(void *argument)
|
|
{
|
|
static float motor_start, motor_dt;
|
|
LOGINFO("[freeRTOS] MOTOR Task Start");
|
|
for (;;)
|
|
{
|
|
motor_start = DWT_GetTimeline_ms();
|
|
MotorControlTask();
|
|
motor_dt = DWT_GetTimeline_ms() - motor_start;
|
|
if (motor_dt > 1)
|
|
LOGERROR("[freeRTOS] MOTOR Task DELAY dt=%.3f", motor_dt);
|
|
osDelay(1);
|
|
}
|
|
}
|
|
|
|
__attribute__((noreturn)) void StartDAEMONTASK(void *argument)
|
|
{
|
|
static float daemon_start, daemon_dt;
|
|
BuzzerInit();
|
|
LOGINFO("[freeRTOS] Daemon Task Start");
|
|
for (;;)
|
|
{
|
|
daemon_start = DWT_GetTimeline_ms();
|
|
DaemonTask();
|
|
BuzzerTask();
|
|
daemon_dt = DWT_GetTimeline_ms() - daemon_start;
|
|
if (daemon_dt > 10)
|
|
LOGERROR("[freeRTOS] Daemon Task DELAY dt=%.3f", daemon_dt);
|
|
osDelay(10);
|
|
}
|
|
}
|
|
|
|
__attribute__((noreturn)) void StartROBOTTASK(void *argument)
|
|
{
|
|
static float robot_start, robot_dt;
|
|
LOGINFO("[freeRTOS] ROBOT core Task Start");
|
|
for (;;)
|
|
{
|
|
robot_start = DWT_GetTimeline_ms();
|
|
RobotTask();
|
|
robot_dt = DWT_GetTimeline_ms() - robot_start;
|
|
if (robot_dt > 5)
|
|
LOGERROR("[freeRTOS] ROBOT core Task DELAY dt=%.3f", robot_dt);
|
|
osDelay(ROBOT_CMD_DELAY);
|
|
}
|
|
}
|