/* =============================================================================
 * imu_task.c - IMU_Task 强定义(两板通用,1kHz,优先级最高)
 * 启动:  恒温 PID/加热 PWM(TIM10_CH1=PF6) -> attitude_init(传感器检测
 *        有界重试;QuaternionEKF 无需静止校准,任务入口已先行上报心跳)
 * 循环:  attitude_update()(1kHz QuaternionEKF 姿态解算)+ ist8310_poll()
 *        (磁力计 200Hz 轮询,硬件 I2C3)+ 恒温控制(目标 40 度;
 *        BMI088 失败或过温时强制关加热,legacy 的 while(1) 死等已移除)
 * 底盘板:本任务随 INS 移植同样运行(姿态供跟随/功能扩展,诊断可见)
 * ========================================================================== */
#include "app_tasks.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "task.h"
#include "attitude.h"
#include "bmi088.h"
#include "ist8310.h"
#include "pid.h"
#include "tim.h"
#include "robot_param.h"

extern TIM_HandleTypeDef htim10;

volatile uint32_t dbg_imu_loop = 0;  // 诊断:IMU 任务循环计数

/* IMU_Task 强定义(云台板,1kHz):
 * 1. BMI088/IST8310 初始化(阻塞,含 ID 检测与报警)
 * 2. 陀螺零偏校准(机器人须静止)
 * 3. 循环:1kHz 姿态解算 + 恒温控制(TIM10_CH1/PF6 PWM,目标 40℃)
 * 过温处理与 legacy 不同:不再死循环,而是关加热并由监控任务绿灯报警 */

static PID_PositionTypeDef imu_temp_pid;

void IMU_Task(void const * argument)
{
    monitor_heartbeat(HB_IMU);  // 初始化期间先行报活,防止看门狗误判

    /* 恒温 PID(legacy IMUTemperatureControl_PIDInit) */
    PID_PositionStructureInit(&imu_temp_pid, IMU_TEMPERATURE_TARGET);
    PID_PositionSetParameter(&imu_temp_pid, IMU_TEMP_KP, IMU_TEMP_KI, IMU_TEMP_KD);
    PID_PositionSetEkRange(&imu_temp_pid, -1, 1);
    PID_PositionSetOUTRange(&imu_temp_pid, 0, 100);

    /* 加热 PWM:与 legacy 相同的 1kHz/千分位配置 */
    __HAL_TIM_SET_PRESCALER(&htim10, 168 - 1);
    __HAL_TIM_SET_AUTORELOAD(&htim10, 1000 - 1);
    __HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, 0);
    HAL_TIM_PWM_Start(&htim10, TIM_CHANNEL_1);

    attitude_init();  // 含阻塞式校准

    TickType_t last_wake = xTaskGetTickCount();
    for (;;)
    {
        attitude_update();
        ist8310_poll();  // 磁力计 200Hz 轮询(硬件 I2C3,阻塞约 0.7ms,内部 5ms 分频)

        /* 恒温控制(legacy 比例为 PID.OUT*10,对应 ARR=999 的千分位)
         * BMI088 检测失败时温度恒为 0,禁止加热(否则会全功率加热失控) */
        if (bmi088_online && bmi088_temperature <= IMU_TEMPERATURE_LIMIT)
        {
            PID_PositionCalc(&imu_temp_pid, bmi088_temperature);
            __HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, (uint32_t)(imu_temp_pid.OUT * 10.0f));
        }
        else
            imu_heater_off();

        monitor_heartbeat(HB_IMU);
        dbg_imu_loop++;
        vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(1));
    }
}