Motor_v2/Robot_V2/tasks.cpp

/**
 * @file tasks.cpp
 * @brief 周期任务调度与任务执行实现
 */

#include "tasks.h"
#include "Can.h"
#include "FOC.h"
#include "Filter.h"
#include "adc.h"
#include "ang_sensor.h"
#include "health.h"
#include "run_inhibit.h"
#include "systick.h"
#include "gd32c10x.h"

extern CAN_transmiter CAN0_transmiter;
extern sampler sampler;
extern angle_sensor KTH7823;
extern EncoderAngleKalman Angle_kalman;
extern ThermalController thermalController;
extern FOC_Controller FOC;
extern u32 motor_runtime_50us;
extern u32 cnt_test;

// 函数指针数组，包含各种控制模式对应的函数
static FOCFunctionPtr control_functions[] = {
	&FOC_Controller::drv_hold,		   // 0: 占空比置
	&FOC_Controller::angle_control,	   // 1: 位置闭环控制
	&FOC_Controller::velocity_control, // 2: 速度闭环控制
	&FOC_Controller::
		angle_control_with_velocity_limit, // 3:
										   // 角度闭环控制，带速度限制
	&FOC_Controller::MIT_Mode,			   // 4: MIT 模式
	&FOC_Controller::current_control,	   // 5: 电流闭环控制
	&FOC_Controller::velocity_open_loop,   // 6: 速度开环控制
	&FOC_Controller::PWM_out,		   // 7: 占空比输出
};

/**
 * @brief 判断任务标志中是否包含指定任务
 * @param[in] currentTasks 当前任务位图快照
 * @param[in] task 待检查任务枚举
 * @return bool true 表示该任务需要执行
 */
static bool has_task(u32 currentTasks, SystemTask task) {
	return (currentTasks & (1u << task)) != 0U;
}

// 按任务 DWT 负载计量：定位 CPU 占用分布（与全局 cpu_usage 同 1s 口径，
// 由 SysTick 统计块调用 task_busy_cycles_reset() 同步清零）。
// 120MHz 下 120000 周期 ≈ 1% CPU。仅统计任务本体，不含中断抢占时间。
static volatile u32 task_busy_cycles[TASK_COUNT] = {0};

u32 get_task_busy_cycles(u8 task) {
	return (task < TASK_COUNT) ? task_busy_cycles[task] : 0U;
}

void task_busy_cycles_reset(void) {
	for (u8 i = 0; i < TASK_COUNT; i++) {
		task_busy_cycles[i] = 0;
	}
}

// 执行任务并累计其忙碌周期
#define RUN_TIMED(task_enum, call)                                             \
	do {                                                                       \
		u32 t0_ = DWT->CYCCNT;                                                 \
		call;                                                                  \
		task_busy_cycles[task_enum] += DWT->CYCCNT - t0_;                      \
	} while (0)

/**
 * @brief 控制模式变化时复位所有控制环，避免积分项跨模式继承
 * @return void
 */
static void reset_control_loops_on_mode_change(void) {
	static int last_control_mode = -1;
	if (FOC.control_mode == last_control_mode) return;

	FOC.vel_loop.reset();
	FOC.angle_loop.reset();
	FOC.current_loop.reset();
	FOC.vel_inVelAngloop.reset();
	FOC.ang_inVelAngloop.reset();
	FOC.ang_inMITloop.reset();
	FOC.vel_inMITloop.reset();
	FOC.current_inMITloop.reset();
	last_control_mode = FOC.control_mode;
}

/**
 * @brief 执行 50us 电机控制任务，包含快速电压检测和控制函数派发
 * @return void
 */
static void run_motor_control_task(void) {
	motor_runtime_50us++;

	static u8 vcheck_div = 0;
	// 快速电压检测每 100us 执行一次，避免每 50us 都读取处理。
	if (++vcheck_div >= 2u) {
		vcheck_div = 0;
		FOC.fast_voltage_check();
	}

	// 运行闸门：全工程唯一执行停机/唤醒功率级的地方。放在模式判断之前，
	// 这样即使 control_mode 非法（>=8）也照样封住输出。
	run_inhibit.update();
	if (!run_inhibit.ready()) {
		return; // 被禁止运行，或刚解除还在驱动唤醒建立期（输出已由闸门保持 0）
	}

	if (FOC.control_mode >= 8) return;

	// 模式合法时再派发控制函数，防止数组越界。
	reset_control_loops_on_mode_change();
	(FOC.*control_functions[FOC.control_mode])();
}

/**
 * @brief 执行高频采样和电机控制相关任务
 * @param[in] currentTasks 当前任务位图快照
 * @return void
 */
static void run_fast_sample_tasks(u32 currentTasks) {
	if (has_task(currentTasks, TASK_CURRENT_UPDATE)) {
		RUN_TIMED(TASK_CURRENT_UPDATE, sampler.update_current());
	}
	if (has_task(currentTasks, TASK_ANGLE_UPDATE)) {
		RUN_TIMED(TASK_ANGLE_UPDATE, KTH7823.update());
		RUN_TIMED(TASK_ANGLE_UPDATE,
				  FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle()));
	}
	if (has_task(currentTasks, TASK_VELOCITY_UPDATE)) {
		RUN_TIMED(TASK_VELOCITY_UPDATE,
				  FOC.rt_velocity = Angle_kalman.getVelocity());
	}
	if (has_task(currentTasks, TASK_MOTOR_CONTROL)) {
		RUN_TIMED(TASK_MOTOR_CONTROL, run_motor_control_task());
	}
}

/**
 * @brief 执行温度、热管理、磁检测和母线电压等低频维护任务
 * @param[in] currentTasks 当前任务位图快照
 * @return void
 */
static void run_housekeeping_tasks(u32 currentTasks) {
	if (has_task(currentTasks, TASK_MCU_TEMP_UPDATE)) {
		RUN_TIMED(TASK_MCU_TEMP_UPDATE, sampler.update_mcu_temp());
	}
	if (has_task(currentTasks, TASK_DRV_TEMP_UPDATE)) {
		RUN_TIMED(TASK_DRV_TEMP_UPDATE, sampler.update_drv_temp());
	}
	if (has_task(currentTasks, TASK_THERMAL_CONTROL)) {
		RUN_TIMED(TASK_THERMAL_CONTROL, thermalController.update());
	}
	if (has_task(currentTasks, TASK_MAGNETIC_CHECK)) {
		RUN_TIMED(TASK_MAGNETIC_CHECK, angsens_MG_check());
	}
	if (has_task(currentTasks, TASK_VBUS_UPDATE)) {
		RUN_TIMED(TASK_VBUS_UPDATE, power_manage());
	}
}

/**
 * @brief 执行 CAN 周期发送任务并喂狗
 * @return void
 */
static void run_can_task(void) {
	u32 t0 = DWT->CYCCNT;
	CAN0_transmiter.test_u16 = CAN0_transmiter.get_node_id();
	cnt_test = 0;
	CAN0_transmiter.can0_send_flag = SET;
	CAN0_transmiter.communication_check();
	fwdgt_counter_reload();
	task_busy_cycles[TASK_CAN_TRANSMIT] += DWT->CYCCNT - t0;
}

/**
 * @brief 根据任务标志调度周期性业务任务
 * @param[in] currentTasks 当前任务位图快照
 * @return void
 */
void do_task(u32 currentTasks) {
	busy_task_enter(); // DWT: 记录忙碌起始周期
	cnt_test++;

	// 高频任务先执行，保持电机控制优先级。
	run_fast_sample_tasks(currentTasks);

	run_housekeeping_tasks(currentTasks);

	// CAN 发送放在最后，避免通信处理影响前面的实时控制任务。
	if (has_task(currentTasks, TASK_CAN_TRANSMIT)) {
		run_can_task();
	}
	busy_task_exit(); // DWT: 累加忙碌周期
}