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 "systick.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;
}

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

	if (FOC.control_mode >= 8) return;

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

/**
 * @brief 执行高频采样和电机控制相关任务
 * @param[in] currentTasks 当前任务位图快照
 * @return void
 */
static void run_fast_sample_tasks(u32 currentTasks) {
	if (has_task(currentTasks, TASK_CURRENT_UPDATE)) {
		sampler.update_current();
	}
	if (has_task(currentTasks, TASK_ANGLE_UPDATE)) {
		KTH7823.update();
		FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle());
	}
	if (has_task(currentTasks, TASK_VELOCITY_UPDATE)) {
		FOC.rt_velocity = Angle_kalman.getVelocity();
	}
	if (has_task(currentTasks, 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)) {
		sampler.update_mcu_temp();
	}
	if (has_task(currentTasks, TASK_DRV_TEMP_UPDATE)) {
		sampler.update_drv_temp();
	}
	if (has_task(currentTasks, TASK_THERMAL_CONTROL)) {
		thermalController.update();
	}
	if (has_task(currentTasks, TASK_MAGNETIC_CHECK)) {
		angsens_MG_check();
	}
	if (has_task(currentTasks, TASK_VBUS_UPDATE)) {
		power_manage();
	}
}

/**
 * @brief 执行 CAN 周期发送任务并喂狗
 * @return void
 */
static void run_can_task(void) {
	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();
}

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

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

	// 齿槽转矩校准 tick（DEBUG，平时不启用）
	#if FOC_DEBUG
	FOC.calibrate_cogging_tick();
	#endif

	run_housekeeping_tasks(currentTasks);

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