hpm6750/controller_yy_app/software/soft_motor_output.c

#include "soft_motor_output.h"
#include "hpm_math.h"
#include "auto_pilot.h"
#include "bsp_V8M_YY_pwm.h"
#include "bsp_V8M_pwm.h"
#include "control_rate.h"
#include "control_throttle.h"
#include "dcm.h"
#include "helpler_funtions.h"
#include "matrix.h"
#include "my_math.h"
#include "params.h"
#include "quaternion.h"
#include "soft_flash.h"
#include "soft_gs.h"
#include "soft_imu.h"
#include "soft_rc_input.h"
#include "soft_time.h"
#include "soft_timer.h"
#include "ver_config.h"
#include "test.h"
/*-------------------- Macros definition -------------------------------------*/

/* 控制量混控 */
#define PIDMIX(R, P, Y, T) ((pid_thr - 1250) * T + 1250 + pid_roll * R + pid_pitch * P + pid_yaw * Y)
#define PIDMIX_NOYAW(R, P, T) ((pid_thr - 1250) * T + 1250 + pid_roll * R + pid_pitch * P)

#define Min_PWM_Out (conf_par.idle_speed) // us
#define Max_PWM_Out 2000                  // us

/*-------------------- Variables definition ----------------------------------*/

/* pid 控制量 */
float pid_roll = 0.0f, pid_pitch = 0.0f, pid_thr = 0.0f, pid_yaw = 0.0f;

/* 8 个电机量 */
uint16_t motor[8] = {1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000};
uint16_t can_motor[8] = {1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000};

/* PWM超限后yaw能给出的最大值 */
static short yaw_limit = 0;

/* PWM饱和后触发航线限制 */
char yaw_output_restriciton = 0;

/* 电机输出是否平衡 */
bool motor_output_unbanlace = false;

/* 电机平滑输出用到的中间变量 */
static float pre_motor[8] = {1000.0f, 1000.0f, 1000.0f, 1000.0f,
                             1000.0f, 1000.0f, 1000.0f, 1000.0f};
static float temp_motor[8] = {1000.0f, 1000.0f, 1000.0f, 1000.0f,
                              1000.0f, 1000.0f, 1000.0f, 1000.0f};

/* 电机检测通道号和开始时间 */
static uint8_t motcheck_num = 0;
static uint32_t motcheck_starttime = 0;

/* 电机动力故障序号 */
static uint8_t _motor_failsafe_num = 0;

/*--------------------- Functions definition ---------------------------------*/
static void
set_motor_noyaw(uint16_t *p_motor, unsigned char motor_num);
static void get_max_yaw_value(uint16_t *p_motor, unsigned char motor_num);

/**
 * @brief 电机 PWM 输出初始化
 *
 */
void motor_output_initial(void)
{
    switch (ver_par.hardware_id)
    {
    case HW_V8M_YY:
        Bsp_V8M_YY_PwmInit();
        break;

    default:
        break;
    }
}

/**
 * @brief 设置电机输出
 *
 * @param s_ch 通道号 1 ~ 10
 * @param s_pwm 输出值
 */
void set_motor_pwm(uint8_t s_ch, uint16_t s_pwm)
{
    /* 根据不同的硬件选择不同的输出接口 */
    switch (ver_par.hardware_id)
    {
    case HW_V8M_YY:
        Bsp_V8M_YY_PwmSetCHValue(s_ch, s_pwm);
        break;

    default:
        break;
    }
}

/**
 * @brief 获取电机输出
 *
 * @param s_ch 通道号 1 ~ 10
 * @retval uint16_t 通道值
 */
uint16_t get_motor_pwm(uint8_t s_ch)
{
    uint16_t m_value = 0;
    /* 根据不同的硬件选择不同的输出接口 */
    switch (ver_par.hardware_id)
    {
    case HW_V8M_YY:
        m_value = Bsp_V8M_YY_PwmGetCHValue(s_ch);
        break;

    default:
        break;
    }

    return m_value;
}

/**
 * @brief 电调校准
 *
 */
void esc_calibrate_enable(void)
{

    /* 开始电调校准时间 */
    static uint32_t start_esc_clb_time = 0;

    char i = 0;

    // 获取校准之后的遥控器的值，好处是如果有的遥控器没经过校准时无法进入电调校准的
    get_rc_value();

    // 带油门，带横滚上电进入电调校准程序。防止有时油门反向直接进入了电调校准。
    if ((rc_in[RC_CH3] > 1950) && (rc_in[RC_CH1] > 1950))
    {
        // 电调校准模式下，不允许电机输出部分更新TIMx_CCRx的值。
        // 有一次出现没法进入校准，波形一直变化，是因为没有屏蔽掉定时器中断中电机输出部分的更新
        pilot_mode = PILOT_ESC_CLB;
        start_esc_clb_time = micros();

        // 带油门&横滚
        // 上电，说明用户希望初始化电调，保持一段时间的高PWM值，避免时间太短无法进入电调校准程序
        while (micros() - start_esc_clb_time < 0.5f * 1000000)
        {
            if (hz_50_flag == true)
            {
                hz_50_flag = false;

                // 根据机型，全部输出CH3油门量。
                for (i = MOTOR1; i <= conf_par.jixing / 10; i++)
                {
                    // 测试发现，校准电调时电调自己预留的10%的最小油门量，确保小油门能停转。（所以再用原始值来校准导致有些预留量大的电调1140无法转起来）
                    set_motor_pwm(i, rc_in[RC_CH3]);
                }
            }
        }
    }
}

/**
 * @brief 设置电机检测的电机号
 *
 * @param checkChNum
 */
void MotorCheck_SetCheckNum(uint8_t checkChNum)
{
    if (checkChNum >= MOTOR1 && checkChNum <= conf_par.jixing / 10)
    {
        motcheck_num = checkChNum;
        motcheck_starttime = micros();
    }
}

/**
 * @brief 未起飞前电机平滑输出
 *
 * @param num 电机各数
 * @param max_add 每步平滑限制幅度
 */
void ground_motor_slow_launch(uint8_t num, float max_add)
{
    if (ground_air_status == ON_GROUND)
    {
        for (uint8_t i = 0; i < num; i++)
        {
            temp_motor[i] = pre_motor[i] + constrain_float((motor[i] - pre_motor[i]), -max_add, max_add);
            pre_motor[i] = temp_motor[i];
            motor[i] = (int16_t)temp_motor[i];
        }
    }
}

/**
 * @brief 清零电机平滑输出用到的中间控制量
 *
 */
void reset_pre_motor(void)
{
    uint8_t i;

    for (i = 0; i < 8; i++)
    {
        pre_motor[i] = 1000.0f;
        temp_motor[i] = 1000.0f;
    }
}

/**
 * @brief 上锁状态下的电机输出
 *
 */
void locked_motor_output(void)
{
    if (thr_lock_status != LOCKED)
    {
        return;
    }
    // motcheck_value[0]无用
    static uint16_t motcheck_value[9] = {1000};
    uint8_t i = 0;

    // 每个电机检测 2 s
    if ((motcheck_num != 0) && (micros() - motcheck_starttime > 2 * 1000000))
    {
        motcheck_num = 0;
    }

    // 有电机需要检测时把需要检测的电机的PWM值增加，其他的电机PWM值赋值1000
    if (motcheck_num != 0)
    {
        for (i = 0; i <= 8; i++)
        {
            motcheck_value[i] = 1000;
        }

        // 直接输出怠速
        motcheck_value[motcheck_num] = conf_par.idle_speed;
    }
    else
    {
        for (i = 0; i <= 8; i++)
        {
            motcheck_value[i] = 1000;
        }
    }

    // 检测电机时根据设置的机型来输出响应的值
    for (i = 1; i <= (conf_par.jixing / 10); i++)
    {
        set_motor_pwm(i, motcheck_value[i]);
        can_motor[i - 1] = motcheck_value[i];
    }

    // 上锁时清零电机平滑用到的中间变量
    reset_pre_motor();
}

static void _I6_motor_output_mix(uint8_t failsafe_motor_num, uint16_t *motor_value)
{
    uint16_t _motor_tmp[6] = {1000, 1000, 1000, 1000, 1000, 1000};
    switch (failsafe_motor_num)
    {
    case 1:
    case 4:
        /* 停掉失效的侧桨叶 */
        motor_value[0] = Min_PWM_Out;
        motor_value[3] = Min_PWM_Out;
        motor_value[1] = PIDMIX_NOYAW(0.866f, -1.5f, 1.2f);
        motor_value[2] = PIDMIX_NOYAW(0.866f, 1.5f, 1.2f);
        motor_value[4] = PIDMIX_NOYAW(-0.866f, 1.5f, 1.2f);
        motor_value[5] = PIDMIX_NOYAW(-0.866f, -1.5f, 1.2f);
        set_motor_noyaw(motor_value, 6);
        motor_value[0] = 1000;
        motor_value[3] = 1000;
        break;

    case 2:
    case 5:
        /* 停掉失效的侧桨叶 */
        motor_value[1] = Min_PWM_Out;
        motor_value[4] = Min_PWM_Out;
        motor_value[0] = PIDMIX_NOYAW(0.866f, -1.5f, 1.2f);
        motor_value[2] = PIDMIX_NOYAW(1.732f, 0.f, 1.2f);
        motor_value[3] = PIDMIX_NOYAW(-0.866f, 1.5, 1.2f);
        motor_value[5] = PIDMIX_NOYAW(-1.732f, 0.0f, 1.2f);
        set_motor_noyaw(motor_value, 6);
        motor_value[1] = 1000;
        motor_value[4] = 1000;
        break;
    case 3:
    case 6: /* 停掉失效的侧桨叶 */
        motor_value[2] = Min_PWM_Out;
        motor_value[5] = Min_PWM_Out;
        motor_value[0] = PIDMIX_NOYAW(-0.866f, -1.5f, 1.2f);
        motor_value[1] = PIDMIX_NOYAW(1.732f, 0, 1.2f);
        motor_value[3] = PIDMIX_NOYAW(0.866f, 1.5f, 1.2f);
        motor_value[4] = PIDMIX_NOYAW(-1.732f, 0, 1.2f);
        set_motor_noyaw(motor_value, 6);
        motor_value[2] = 1000;
        motor_value[5] = 1000;
        break;

    default:
    {
        const float yaw_ratio[6] = {-1.f, 1.f, -1.f, 1.f, -1.f, 1.f};
        motor_value[0] = PIDMIX_NOYAW(-0.0f, -1.0f, 1);
        motor_value[1] = PIDMIX_NOYAW(+7.0f / 8.0f, -1.0f / 2.0f, 1);
        motor_value[2] = PIDMIX_NOYAW(+7.0f / 8.0f, +1.0f / 2.0f, 1);
        motor_value[3] = PIDMIX_NOYAW(+0.0f, +1.0f, 1);
        motor_value[4] = PIDMIX_NOYAW(-7.0f / 8.0f, +1.0f / 2.0f, 1);
        motor_value[5] = PIDMIX_NOYAW(-7.0f / 8.0f, -1.0f / 2.0f, 1);

        set_motor_noyaw(motor_value, 6);

        for (uint8_t i = 0; i < 6; ++i)
        {
            _motor_tmp[i] = motor_value[i] + pid_yaw * yaw_ratio[i];
        }
        get_max_yaw_value(_motor_tmp, 6);

        for (uint8_t i = 0; i < 6; ++i)
        {
            motor_value[i] += yaw_limit * yaw_ratio[i];
        }
    }
    break;
    }
}

static void _X4_motor_output_mix(uint8_t failsafe_motor_num, uint16_t *motor_value)
{
    uint16_t _motor_tmp[4];
    switch (failsafe_motor_num)
    {
    default:
        motor_value[0] = PIDMIX_NOYAW(-1.0f, -1.0f, 1);
        motor_value[1] = PIDMIX_NOYAW(+1.0f, -1.0f, 1);
        motor_value[2] = PIDMIX_NOYAW(+1.0f, +1.0f, 1);
        motor_value[3] = PIDMIX_NOYAW(-1.0f, +1.0f, 1);

        set_motor_noyaw(motor_value, 4);

        const float yaw_ratio[4] = {-1.f, 1.f, -1.f, 1.f};
        for (int i = 0; i < 4; ++i)
        {
            _motor_tmp[i] = motor_value[i] + pid_yaw * yaw_ratio[i];
        }

        get_max_yaw_value(_motor_tmp, 4);

        for (int i = 0; i < 4; ++i)
        {
            motor_value[i] = motor_value[i] + yaw_limit * yaw_ratio[i];
        }
        break;
    }
}

static void _I4_motor_output_mix(uint8_t failsafe_motor_num, uint16_t *motor_value)
{
    uint16_t _motor_tmp[4];
    switch (failsafe_motor_num)
    {
    default:
    {
        motor_value[0] = PIDMIX_NOYAW(0, -1.0f, 1);
        motor_value[1] = PIDMIX_NOYAW(+1.0f, 0, 1);
        motor_value[2] = PIDMIX_NOYAW(0, +1.0f, 1);
        motor_value[3] = PIDMIX_NOYAW(-1.0f, 0, 1);

        set_motor_noyaw(motor_value, 4);

        const float yaw_ratio[4] = {-1.f, 1.f, -1.f, 1.f};

        for (int i = 0; i < 4; ++i)
        {
            _motor_tmp[i] = motor_value[i] + pid_yaw * yaw_ratio[i];
        }

        get_max_yaw_value(_motor_tmp, 4);

        for (int i = 0; i < 4; ++i)
        {
            motor_value[i] = motor_value[i] + yaw_limit * yaw_ratio[i];
        }
    }
    break;
    }
}

static void _X6_motor_output_mix(uint8_t failsafe_motor_num, uint16_t *motor_value)
{
    uint16_t _motor_tmp[6] = {1000, 1000, 1000, 1000, 1000, 1000};
    switch (failsafe_motor_num)
    {
    case 1:
    case 4:
        /* 停掉失效的侧桨叶 */
        motor_value[3] = Min_PWM_Out;
        motor_value[0] = Min_PWM_Out;
        motor_value[1] = PIDMIX_NOYAW(0.0f, -1.732f, 1.2f);
        motor_value[2] = PIDMIX_NOYAW(1.5f, 0.866f, 1.2f);
        motor_value[4] = PIDMIX_NOYAW(0.0f, 1.732f, 1.2f);
        motor_value[5] = PIDMIX_NOYAW(-1.5f, -0.866f, 1.2f);
        set_motor_noyaw(motor_value, 6);
        motor_value[3] = 1000;
        motor_value[0] = 1000;
        break;

    case 2:
    case 5:
        /* 停掉失效的侧桨叶 */
        motor_value[1] = Min_PWM_Out;
        motor_value[4] = Min_PWM_Out;
        motor_value[0] = PIDMIX_NOYAW(0.f, -1.732f, 1.2f);
        motor_value[2] = PIDMIX_NOYAW(1.5f, -0.866f, 1.2f);
        motor_value[3] = PIDMIX_NOYAW(0.f, 1.732f, 1.2f);
        motor_value[5] = PIDMIX_NOYAW(-1.5f, 0.866f, 1.2f);
        set_motor_noyaw(motor_value, 6);
        motor_value[1] = 1000;
        motor_value[4] = 1000;
        break;

    case 3:
    case 6:
        /* 停掉失效的侧桨叶 */
        motor_value[2] = Min_PWM_Out;
        motor_value[5] = Min_PWM_Out;
        motor_value[0] = PIDMIX_NOYAW(-1.5f, -0.866f, 1.2f);
        motor_value[1] = PIDMIX_NOYAW(1.5f, -0.866f, 1.2f);
        motor_value[3] = PIDMIX_NOYAW(1.5f, 0.866f, 1.2f);
        motor_value[4] = PIDMIX_NOYAW(-1.5f, 0.866f, 1.2f);
        set_motor_noyaw(motor_value, 6);
        motor_value[2] = 1000;
        motor_value[5] = 1000;
        break;

    default:
    {
        const float yaw_ratio[6] = {-1.f, 1.f, -1.f, 1.f, -1.f, 1.f};

        motor_value[0] = PIDMIX_NOYAW(-0.5f, -0.866f, 1);
        motor_value[1] = PIDMIX_NOYAW(0.5f, -0.866f, 1);
        motor_value[2] = PIDMIX_NOYAW(1.0f, 0.0f, 1);
        motor_value[3] = PIDMIX_NOYAW(0.5f, 0.866f, 1);
        motor_value[4] = PIDMIX_NOYAW(-0.5f, 0.866f, 1);
        motor_value[5] = PIDMIX_NOYAW(-1.0f, 0.0f, 1);

        set_motor_noyaw(motor_value, 6);

        for (uint8_t i = 0; i < 6; ++i)
        {
            _motor_tmp[i] = motor_value[i] + pid_yaw * yaw_ratio[i];
        }
        get_max_yaw_value(_motor_tmp, 6);

        for (uint8_t i = 0; i < 6; ++i)
        {
            motor_value[i] += yaw_limit * yaw_ratio[i];
        }
    }
    break;
    }
}

static void _H6_motor_output_mix(uint8_t failsafe_motor_num, uint16_t *motor_value)
{
    uint16_t _motor_tmp[6] = {1000, 1000, 1000, 1000, 1000, 1000};
    switch (failsafe_motor_num)
    {
    default:
    {
        motor_value[0] = PIDMIX_NOYAW(-0.5f, -0.866f, 1);
        motor_value[1] = PIDMIX_NOYAW(0.5f, -0.866f, 1);
        motor_value[2] = PIDMIX_NOYAW(2.0f, 0.0f, 1);
        motor_value[3] = PIDMIX_NOYAW(0.5f, 0.866f, 1);
        motor_value[4] = PIDMIX_NOYAW(-0.5f, 0.866f, 1);
        motor_value[5] = PIDMIX_NOYAW(-2.0f, 0.0f, 1);

        set_motor_noyaw(motor_value, 6);

        const float yaw_ratio[6] = {-1.f, 1.f, -2.f, 1.f, -1.f, 2.f};

        for (uint8_t i = 0; i < 6; ++i)
        {
            _motor_tmp[i] = motor_value[i] + pid_yaw * yaw_ratio[i];
        }
        get_max_yaw_value(_motor_tmp, 6);

        for (uint8_t i = 0; i < 6; ++i)
        {
            motor_value[i] += yaw_limit * yaw_ratio[i];
        }
    }
    break;
    }
}

void unlocked_motor_output(void)
{
    if (thr_lock_status != UNLOCKED)
    {
        return;
    }
    uint16_t limit_motor[8] = {1000, 1000, 1000, 1000,
                               1000, 1000, 1000, 1000};

    if (ground_air_status == ON_GROUND)
    {
        // 在地上的时候积分不参与运算
        clear_rate_i_item(&pid_m_roll);
        clear_rate_i_item(&pid_m_pitch);
        clear_rate_i_item(&pid_m_yaw);

        if (rc_in[RC_CH3] < 1200)
        {
            pid_roll = 0.0f;
            pid_pitch = 0.0f;
            pid_yaw = 0.0f;
        }
    }

    switch (conf_par.jixing)
    {
    case FOUR_I4:
        _I4_motor_output_mix(_motor_failsafe_num, motor);
        break;
    case FOUR_X4:
        _X4_motor_output_mix(_motor_failsafe_num, motor);
        break;
    case THREE_Y6D:
        motor[0] = PIDMIX_NOYAW(-0.0f, +1.0f, 1); // REAR
        motor[1] = PIDMIX_NOYAW(-1.0f, -1.0f, 1); // RIGHT
        motor[2] = PIDMIX_NOYAW(+1.0f, -1.0f, 1); // LEFT
        motor[3] = PIDMIX_NOYAW(+0.0f, +1.0f, 1); // UNDER_REAR
        motor[4] = PIDMIX_NOYAW(-1.0f, -1.0f, 1); // UNDER_RIGHT
        motor[5] = PIDMIX_NOYAW(+1.0f, -1.0f, 1); // UNDER_LEFT

        set_motor_noyaw(motor, 6);

        limit_motor[0] = motor[0] + (short)(pid_yaw * -1.0f); // REAR 后尾电机
        limit_motor[1] = motor[1] + (short)(pid_yaw * -1.0f); // RIGHT 右边电机
        limit_motor[2] = motor[2] + (short)(pid_yaw * -1.0f); // LEFT 左边电机
        limit_motor[3] = motor[3] + (short)(pid_yaw * 1.0f);  // FRONT 前面电机
        limit_motor[4] = motor[4] + (short)(pid_yaw * 1.0f);  // LEFT 左边电机
        limit_motor[5] = motor[5] + (short)(pid_yaw * 1.0f);  // FRONT 前面电机

        get_max_yaw_value(limit_motor, 6);

        motor[0] += (yaw_limit * -1); // REAR	 后尾电机
        motor[1] += (yaw_limit * -1); // RIGHT 右边电机
        motor[2] += (yaw_limit * -1); // LEFT	 左边电机
        motor[3] += (yaw_limit * 1);  // FRONT 前面电机
        motor[4] += (yaw_limit * 1);  // LEFT	 左边电机
        motor[5] += (yaw_limit * 1);  // FRONT 前面电机

        break;
    case THREE_YI6D:
        motor[0] = PIDMIX_NOYAW(-0.0f, -1.0f, 1); // REAR
        motor[1] = PIDMIX_NOYAW(+1.0f, +1.0f, 1); // RIGHT
        motor[2] = PIDMIX_NOYAW(-1.0f, +1.0f, 1); // LEFT
        motor[3] = PIDMIX_NOYAW(+0.0f, -1.0f, 1); // UNDER_REAR
        motor[4] = PIDMIX_NOYAW(+1.0f, +1.0f, 1); // UNDER_RIGHT
        motor[5] = PIDMIX_NOYAW(-1.0f, +1.0f, 1); // UNDER_LEFT

        set_motor_noyaw(motor, 6);

        limit_motor[0] = motor[0] + (short)(pid_yaw * -1.0f); // REAR 后尾电机
        limit_motor[1] = motor[1] + (short)(pid_yaw * -1.0f); // RIGHT 右边电机
        limit_motor[2] = motor[2] + (short)(pid_yaw * -1.0f); // LEFT 左边电机
        limit_motor[3] = motor[3] + (short)(pid_yaw * 1.0f);  // FRONT 前面电机
        limit_motor[4] = motor[4] + (short)(pid_yaw * 1.0f);  // LEFT 左边电机
        limit_motor[5] = motor[5] + (short)(pid_yaw * 1.0f);  // FRONT 前面电机

        get_max_yaw_value(limit_motor, 6);

        motor[0] += (yaw_limit * -1); // REAR	 后尾电机
        motor[1] += (yaw_limit * -1); // RIGHT 右边电机
        motor[2] += (yaw_limit * -1); // LEFT	 左边电机
        motor[3] += (yaw_limit * 1);  // FRONT 前面电机
        motor[4] += (yaw_limit * 1);  // LEFT	 左边电机
        motor[5] += (yaw_limit * 1);  // FRONT 前面电机

        break;

    case SIX_I6:
        _I6_motor_output_mix(_motor_failsafe_num, motor);
        break;

    case SIX_X6:
        _X6_motor_output_mix(_motor_failsafe_num, motor);
        break;

    case SIX_H6:
        _H6_motor_output_mix(_motor_failsafe_num, motor);
        break;
    // 上全正，下全反，跟DJI一样
    case FOUR_X8D:
        motor[0] = PIDMIX_NOYAW(-1.0f, -1.0f, 1);
        motor[1] = PIDMIX_NOYAW(+1.0f, -1.0f, 1);
        motor[2] = PIDMIX_NOYAW(+1.0f, +1.0f, 1);
        motor[3] = PIDMIX_NOYAW(-1.0f, +1.0f, 1);
        motor[4] = PIDMIX_NOYAW(-1.0f, -1.0f, 1);
        motor[5] = PIDMIX_NOYAW(+1.0f, -1.0f, 1);
        motor[6] = PIDMIX_NOYAW(+1.0f, +1.0f, 1);
        motor[7] = PIDMIX_NOYAW(-1.0f, +1.0f, 1);

        set_motor_noyaw(motor, 8);

        limit_motor[0] = motor[0] + (short)(pid_yaw * -1.0f); // REAR 后尾电机
        limit_motor[1] = motor[1] + (short)(pid_yaw * -1.0f); // RIGHT 右边电机
        limit_motor[2] = motor[2] + (short)(pid_yaw * -1.0f); // LEFT 左边电机
        limit_motor[3] = motor[3] + (short)(pid_yaw * -1.0f); // FRONT 前面电机
        limit_motor[4] = motor[4] + (short)(pid_yaw * 1.0f);  // LEFT 左边电机
        limit_motor[5] = motor[5] + (short)(pid_yaw * 1.0f);  // FRONT 前面电机
        limit_motor[6] = motor[6] + (short)(pid_yaw * 1.0f);  // LEFT 左边电机
        limit_motor[7] = motor[7] + (short)(pid_yaw * 1.0f);  // FRONT 前面电机

        get_max_yaw_value(limit_motor, 8);

        motor[0] += (yaw_limit * -1); // REAR	 后尾电机
        motor[1] += (yaw_limit * -1); // RIGHT 右边电机
        motor[2] += (yaw_limit * -1); // LEFT	 左边电机
        motor[3] += (yaw_limit * -1); // FRONT 前面电机
        motor[4] += (yaw_limit * 1);  // LEFT	 左边电机
        motor[5] += (yaw_limit * 1);  // FRONT 前面电机
        motor[6] += (yaw_limit * 1);  // LEFT	 左边电机
        motor[7] += (yaw_limit * 1);  // FRONT 前面电机

        break;
    // 上跟四轴X一样，下正好跟上相反
    case FOUR_X8M:
        motor[0] = PIDMIX_NOYAW(-1.0f, -1.0f, 1);
        motor[1] = PIDMIX_NOYAW(+1.0f, -1.0f, 1);
        motor[2] = PIDMIX_NOYAW(+1.0f, +1.0f, 1);
        motor[3] = PIDMIX_NOYAW(-1.0f, +1.0f, 1);
        motor[4] = PIDMIX_NOYAW(-1.0f, -1.0f, 1);
        motor[5] = PIDMIX_NOYAW(+1.0f, -1.0f, 1);
        motor[6] = PIDMIX_NOYAW(+1.0f, +1.0f, 1);
        motor[7] = PIDMIX_NOYAW(-1.0f, +1.0f, 1);

        set_motor_noyaw(motor, 8);

        limit_motor[0] = motor[0] + (short)(pid_yaw * -1.0f); // REAR 后尾电机
        limit_motor[1] = motor[1] + (short)(pid_yaw * 1.0f);  // RIGHT 右边电机
        limit_motor[2] = motor[2] + (short)(pid_yaw * -1.0f); // LEFT 左边电机
        limit_motor[3] = motor[3] + (short)(pid_yaw * 1.0f);  // FRONT 前面电机
        limit_motor[4] = motor[4] + (short)(pid_yaw * 1.0f);  // LEFT 左边电机
        limit_motor[5] = motor[5] + (short)(pid_yaw * -1.0f); // FRONT 前面电机
        limit_motor[6] = motor[6] + (short)(pid_yaw * 1.0f);  // LEFT 左边电机
        limit_motor[7] = motor[7] + (short)(pid_yaw * -1.0f); // FRONT 前面电机

        get_max_yaw_value(limit_motor, 8);

        motor[0] += (yaw_limit * -1); // REAR	 后尾电机
        motor[1] += (yaw_limit * 1);  // RIGHT 右边电机
        motor[2] += (yaw_limit * -1); // LEFT	 左边电机
        motor[3] += (yaw_limit * 1);  // FRONT 前面电机
        motor[4] += (yaw_limit * 1);  // LEFT	 左边电机
        motor[5] += (yaw_limit * -1); // FRONT 前面电机
        motor[6] += (yaw_limit * 1);  // LEFT	 左边电机
        motor[7] += (yaw_limit * -1); // FRONT 前面电机
        break;
    // 和 x8m 相反
    case FOUR_X8MR:
        motor[0] = PIDMIX_NOYAW(-1.0f, -1.0f, 1);
        motor[1] = PIDMIX_NOYAW(+1.0f, -1.0f, 1);
        motor[2] = PIDMIX_NOYAW(+1.0f, +1.0f, 1);
        motor[3] = PIDMIX_NOYAW(-1.0f, +1.0f, 1);
        motor[4] = PIDMIX_NOYAW(-1.0f, -1.0f, 1);
        motor[5] = PIDMIX_NOYAW(+1.0f, -1.0f, 1);
        motor[6] = PIDMIX_NOYAW(+1.0f, +1.0f, 1);
        motor[7] = PIDMIX_NOYAW(-1.0f, +1.0f, 1);

        set_motor_noyaw(motor, 8);

        limit_motor[0] = motor[0] + (short)(pid_yaw * 1.0f);  // REAR 后尾电机
        limit_motor[1] = motor[1] + (short)(pid_yaw * -1.0f); // RIGHT 右边电机
        limit_motor[2] = motor[2] + (short)(pid_yaw * 1.0f);  // LEFT 左边电机
        limit_motor[3] = motor[3] + (short)(pid_yaw * -1.0f); // FRONT 前面电机
        limit_motor[4] = motor[4] + (short)(pid_yaw * -1.0f); // LEFT 左边电机
        limit_motor[5] = motor[5] + (short)(pid_yaw * 1.0f);  // FRONT 前面电机
        limit_motor[6] = motor[6] + (short)(pid_yaw * -1.0f); // LEFT 左边电机
        limit_motor[7] = motor[7] + (short)(pid_yaw * 1.0f);  // FRONT 前面电机

        get_max_yaw_value(limit_motor, 8);

        motor[0] += (yaw_limit * 1);  // REAR	 后尾电机
        motor[1] += (yaw_limit * -1); // RIGHT 右边电机
        motor[2] += (yaw_limit * 1);  // LEFT	 左边电机
        motor[3] += (yaw_limit * -1); // FRONT 前面电机
        motor[4] += (yaw_limit * -1); // LEFT	 左边电机
        motor[5] += (yaw_limit * 1);  // FRONT 前面电机
        motor[6] += (yaw_limit * -1); // LEFT	 左边电机
        motor[7] += (yaw_limit * 1);  // FRONT 前面电机
        break;

        // 上全反，下全正，跟DJI一样
    case FOUR_X8DR:
        motor[0] = PIDMIX_NOYAW(-1.0f, -1.0f, 1);
        motor[1] = PIDMIX_NOYAW(+1.0f, -1.0f, 1);
        motor[2] = PIDMIX_NOYAW(+1.0f, +1.0f, 1);
        motor[3] = PIDMIX_NOYAW(-1.0f, +1.0f, 1);
        motor[4] = PIDMIX_NOYAW(-1.0f, -1.0f, 1);
        motor[5] = PIDMIX_NOYAW(+1.0f, -1.0f, 1);
        motor[6] = PIDMIX_NOYAW(+1.0f, +1.0f, 1);
        motor[7] = PIDMIX_NOYAW(-1.0f, +1.0f, 1);

        set_motor_noyaw(motor, 8);

        limit_motor[0] = motor[0] + (short)(pid_yaw * 1.0f);  // REAR 后尾电机
        limit_motor[1] = motor[1] + (short)(pid_yaw * 1.0f);  // RIGHT 右边电机
        limit_motor[2] = motor[2] + (short)(pid_yaw * 1.0f);  // LEFT 左边电机
        limit_motor[3] = motor[3] + (short)(pid_yaw * 1.0f);  // FRONT 前面电机
        limit_motor[4] = motor[4] + (short)(pid_yaw * -1.0f); // LEFT 左边电机
        limit_motor[5] = motor[5] + (short)(pid_yaw * -1.0f); // FRONT 前面电机
        limit_motor[6] = motor[6] + (short)(pid_yaw * -1.0f); // LEFT 左边电机
        limit_motor[7] = motor[7] + (short)(pid_yaw * -1.0f); // FRONT 前面电机

        get_max_yaw_value(limit_motor, 8);

        motor[0] += (yaw_limit * 1);  // REAR	 后尾电机
        motor[1] += (yaw_limit * 1);  // RIGHT 右边电机
        motor[2] += (yaw_limit * 1);  // LEFT	 左边电机
        motor[3] += (yaw_limit * 1);  // FRONT 前面电机
        motor[4] += (yaw_limit * -1); // LEFT	 左边电机
        motor[5] += (yaw_limit * -1); // FRONT 前面电机
        motor[6] += (yaw_limit * -1); // LEFT	 左边电机
        motor[7] += (yaw_limit * -1); // FRONT 前面电机

        break;

    case EIGHT_I8:
        motor[0] = PIDMIX_NOYAW(-0.0f, -1.0f, 1);                 // REAR_R
        motor[1] = PIDMIX_NOYAW(+7.0f / 10.0f, -7.0f / 10.0f, 1); // FRONT_R
        motor[2] = PIDMIX_NOYAW(+1.0f, -0.0f, 1);                 // REAR_L
        motor[3] = PIDMIX_NOYAW(+7.0f / 10.0f, +7.0f / 10.0f, 1); // FRONT_L
        motor[4] = PIDMIX_NOYAW(+0.0f, +1.0f, 1);                 // UNDER_REAR_R
        motor[5] = PIDMIX_NOYAW(-7.0f / 10.0f, +7.0f / 10.0f, 1); // UNDER_FRONT_R
        motor[6] = PIDMIX_NOYAW(-1.0f, +0.0f, 1);                 // UNDER_REAR_L
        motor[7] = PIDMIX_NOYAW(-7.0f / 10.0f, -7.0f / 10.0f, 1); // UNDER_FRONT_L

        set_motor_noyaw(motor, 8);

        limit_motor[0] = motor[0] + (short)(pid_yaw * -1.0f); // REAR 后尾电机
        limit_motor[1] = motor[1] + (short)(pid_yaw * 1.0f);  // RIGHT 右边电机
        limit_motor[2] = motor[2] + (short)(pid_yaw * -1.0f); // LEFT 左边电机
        limit_motor[3] = motor[3] + (short)(pid_yaw * 1.0f);  // FRONT 前面电机
        limit_motor[4] = motor[4] + (short)(pid_yaw * -1.0f); // LEFT 左边电机
        limit_motor[5] = motor[5] + (short)(pid_yaw * 1.0f);  // FRONT 前面电机
        limit_motor[6] = motor[6] + (short)(pid_yaw * -1.0f); // LEFT 左边电机
        limit_motor[7] = motor[7] + (short)(pid_yaw * 1.0f);  // FRONT 前面电机

        get_max_yaw_value(limit_motor, 8);

        motor[0] += (yaw_limit * -1); // REAR	 后尾电机
        motor[1] += (yaw_limit * 1);  // RIGHT 右边电机
        motor[2] += (yaw_limit * -1); // LEFT	 左边电机
        motor[3] += (yaw_limit * 1);  // FRONT 前面电机
        motor[4] += (yaw_limit * -1); // LEFT	 左边电机
        motor[5] += (yaw_limit * 1);  // FRONT 前面电机
        motor[6] += (yaw_limit * -1); // LEFT	 左边电机
        motor[7] += (yaw_limit * 1);  // FRONT 前面电机

        break;
    case EIGHT_X8:
        motor[0] = PIDMIX_NOYAW(-1.0f / 2.0f, -1.0f, 1); // REAR_R
        motor[1] = PIDMIX_NOYAW(+1.0f / 2.0f, -1.0f, 1); // FRONT_R
        motor[2] = PIDMIX_NOYAW(+1.0f, -1.0f / 2.0f, 1); // REAR_L
        motor[3] = PIDMIX_NOYAW(+1.0f, +1.0f / 2.0f, 1); // FRONT_L
        motor[4] = PIDMIX_NOYAW(+1.0f / 2.0f, +1.0f, 1); // UNDER_REAR_R
        motor[5] = PIDMIX_NOYAW(-1.0f / 2.0f, +1.0f, 1); // UNDER_FRONT_R
        motor[6] = PIDMIX_NOYAW(-1.0f, +1.0f / 2.0f, 1); // UNDER_REAR_L
        motor[7] = PIDMIX_NOYAW(-1.0f, -1.0f / 2.0f, 1); // UNDER_FRONT_L

        set_motor_noyaw(motor, 8);

        limit_motor[0] = motor[0] + (short)(pid_yaw * -1.0f); // REAR 后尾电机
        limit_motor[1] = motor[1] + (short)(pid_yaw * 1.0f);  // RIGHT 右边电机
        limit_motor[2] = motor[2] + (short)(pid_yaw * -1.0f); // LEFT 左边电机
        limit_motor[3] = motor[3] + (short)(pid_yaw * 1.0f);  // FRONT 前面电机
        limit_motor[4] = motor[4] + (short)(pid_yaw * -1.0f); // LEFT 左边电机
        limit_motor[5] = motor[5] + (short)(pid_yaw * 1.0f);  // FRONT 前面电机
        limit_motor[6] = motor[6] + (short)(pid_yaw * -1.0f); // LEFT 左边电机
        limit_motor[7] = motor[7] + (short)(pid_yaw * 1.0f);  // FRONT 前面电机

        get_max_yaw_value(limit_motor, 8);

        motor[0] += (yaw_limit * -1); // REAR	 后尾电机
        motor[1] += (yaw_limit * 1);  // RIGHT 右边电机
        motor[2] += (yaw_limit * -1); // LEFT	 左边电机
        motor[3] += (yaw_limit * 1);  // FRONT 前面电机
        motor[4] += (yaw_limit * -1); // LEFT	 左边电机
        motor[5] += (yaw_limit * 1);  // FRONT 前面电机
        motor[6] += (yaw_limit * -1); // LEFT	 左边电机
        motor[7] += (yaw_limit * 1);  // FRONT 前面电机

        break;
    default:
        break;
    }

    ground_motor_slow_launch(conf_par.jixing / 10, 0.6f);
    for (uint8_t chNum = MOTOR1; chNum <= conf_par.jixing / 10; chNum++)
    {
        set_motor_pwm(chNum, motor[chNum - 1]);
        can_motor[chNum - 1] = motor[chNum - 1];
    }
}


/**
 * @brief 电机输出饱和保护，牺牲高度控制
 *
 * @param p_motor 电机输出量
 * @param motor_num 电机个数
 */
void set_motor_noyaw(uint16_t *p_motor, unsigned char motor_num)
{
    short min_pwm_out = 2000, max_pwm_out = 1000;
    short pwm_out_error = 0;
    unsigned char i = 0;
    // 起飞后最低转速保护1050
    short min_thr_constrain = Min_PWM_Out;

    for (i = 0; i < motor_num; i++)
    {
        // 找出电机输出的最大值和最小值
        if (p_motor[i] > max_pwm_out)
        {
            max_pwm_out = p_motor[i];
        }

        if (p_motor[i] < min_pwm_out)
        {
            min_pwm_out = p_motor[i];
        }
    }

    // 如果最大油门大于2000，则牺牲油门控制量
    if (max_pwm_out > Max_PWM_Out)
    {
        pwm_out_error = max_pwm_out - Max_PWM_Out;
    }

    if (min_pwm_out < min_thr_constrain)
    {
        pwm_out_error = min_pwm_out - min_thr_constrain;
    }
    // 将所有电机量同时减掉pwm_out_error
    if (pwm_out_error != 0)
    {
        for (i = 0; i < motor_num; i++)
        {
            p_motor[i] -= pwm_out_error;

            if (p_motor[i] > Max_PWM_Out)
            {
                p_motor[i] = Max_PWM_Out;
            }
            else if (p_motor[i] < min_thr_constrain)
            {
                p_motor[i] = min_thr_constrain;
            }
        }
    }
}

/**
 * @brief 电机输出饱和保护，牺牲航向
 *
 * @param p_motor 电机值
 * @param motor_num 电机个数
 */
void get_max_yaw_value(uint16_t *p_motor, unsigned char motor_num)
{
    static unsigned int motor_restriction_time = 0;
    short min_pwm_out = 2000, max_pwm_out = 1000;
    unsigned char i = 0;
    short pwm_out_error = 0;
    // 起飞后最低转速保护1050
    short min_thr_constrain = Min_PWM_Out;

    if (ground_air_status == IN_AIR)
    {
        min_thr_constrain = 1050;
    }
    else 
    {
        min_thr_constrain = Min_PWM_Out;
    }
    
    for (i = 0; i < motor_num; i++)
    {
        // 找出电机输出的最大值和最小值
        if (p_motor[i] > max_pwm_out)
        {
            max_pwm_out = p_motor[i];
        }

        if (p_motor[i] < min_pwm_out)
        {
            min_pwm_out = p_motor[i];
        }
    }
    // 如果最大油门大于2000，则牺牲航向控制
    if (max_pwm_out > Max_PWM_Out)
    {
        pwm_out_error = max_pwm_out - Max_PWM_Out;
    }

    if (min_pwm_out < min_thr_constrain)
    {
        pwm_out_error = min_thr_constrain - min_pwm_out;
    }

    // 如果输出油门到达限幅，则触发航向只做减速控制
    if (max_pwm_out > Max_PWM_Out || min_pwm_out < min_thr_constrain)
    {
        if (micros() - motor_restriction_time > 500000)
        {
            yaw_output_restriciton = 1;
            pid_m_yaw.angle_i_item = 0.0f;
        }
    }
    else
    {
        yaw_output_restriciton = 0;
        motor_restriction_time = micros();
    }

    if (pid_yaw >= 0.0f)
        yaw_limit = (short)pid_yaw - pwm_out_error;
    else
        yaw_limit = (short)pid_yaw + pwm_out_error;

    // 如果飞机倾斜角度大于 35 度，则不加入航向控制量
    float tilt_angle = acosf(cosf(pid_m_roll.angle_c * DEG_TO_RAD) *
                             cosf(pid_m_pitch.angle_c * DEG_TO_RAD)) *  RAD_TO_DEG;
    if (fabsf(tilt_angle) > 35.0f)
    {
        yaw_limit = 0.0f;
        pid_m_yaw.angle_i_item = 0.0f;
    }
}

/**
 * @brief 获取航向输出控制受限标志
 *
 * @return uint8_t
 */
uint8_t MotorOutput_GetYawRestrictionStatus(void)
{
    return yaw_output_restriciton;
}



uint8_t Motor_GetFailsafeNum(void)
{
    return _motor_failsafe_num;
}


#ifdef SOFT_MOTOR_TEST
//============================================================================
// 测试配置
//============================================================================
#define TEST_MOTOR_COUNT    4           // 四轴电机数量
#define TEST_DURATION_MS    5000        // 每个测试持续时间 5秒
#define TEST_STEP_DELAY_MS  2000        // 测试步骤间隔 2秒

// 测试模式
typedef enum {
    TEST_MODE_IDLE = 0,         // 怠速测试
    TEST_MODE_HOVER,            // 悬停测试
    TEST_MODE_ROLL,             // 横滚测试
    TEST_MODE_PITCH,            // 俯仰测试
    TEST_MODE_YAW,              // 偏航测试
    TEST_MODE_THROTTLE,         // 油门测试
    TEST_MODE_SATURATION,       // 饱和保护测试
    TEST_MODE_END               // 结束
} test_mode_t;

// 测试结果
typedef struct {
    uint16_t motor_values[8];   // 电机输出值
    float pid_roll;             // 横滚PID输出
    float pid_pitch;            // 俯仰PID输出
    float pid_yaw;              // 偏航PID输出
    float pid_thr;              // 油门PID输出
    uint8_t yaw_restricted;     // 偏航受限标志
    uint32_t timestamp;         // 时间戳
} motor_test_data_t;

//============================================================================
// 全局测试变量
//============================================================================
static motor_test_data_t g_test_data[100];  // 存储测试数据
static uint16_t g_data_index = 0;
static bool g_test_running = false;

//============================================================================
// 辅助函数
//============================================================================

/**
 * @brief 打印电机输出值
 */
static void print_motor_values(uint16_t *motors, uint8_t count, const char* title)
{
    printf("%s: ", title);
    for (int i = 0; i < count; i++) {
        printf("M%d=%d ", i+1, motors[i]);
    }
    printf("\n");
}

/**
 * @brief 记录测试数据
 */
static void record_test_data(void)
{
    if (g_data_index < sizeof(g_test_data)/sizeof(g_test_data[0])) {
        g_test_data[g_data_index].timestamp = micros();
        
        // 读取当前电机输出
        for (int i = 0; i < TEST_MOTOR_COUNT; i++) {
            g_test_data[g_data_index].motor_values[i] = get_motor_pwm(i+1);
        }
        
        // 记录PID值（从全局变量获取）
        extern float pid_roll, pid_pitch, pid_yaw, pid_thr;
        g_test_data[g_data_index].pid_roll = pid_roll;
        g_test_data[g_data_index].pid_pitch = pid_pitch;
        g_test_data[g_data_index].pid_yaw = pid_yaw;
        g_test_data[g_data_index].pid_thr = pid_thr;
        
        // 记录偏航受限标志
        g_test_data[g_data_index].yaw_restricted = MotorOutput_GetYawRestrictionStatus();
        
        g_data_index++;
    }
}

/**
 * @brief 打印测试总结
 */
static void print_test_summary(void)
{
    printf("\n========== Test Summary ==========\n");
    printf("Total records: %d\n", g_data_index);
    
    if (g_data_index > 0) {
        printf("\nLast record:\n");
        printf("  Time: %lu us\n", g_test_data[g_data_index-1].timestamp);
        printf("  Motors: ");
        for (int i = 0; i < TEST_MOTOR_COUNT; i++) {
            printf("%d ", g_test_data[g_data_index-1].motor_values[i]);
        }
        printf("\n");
        printf("  PID: roll=%.2f, pitch=%.2f, yaw=%.2f, thr=%.2f\n",
               g_test_data[g_data_index-1].pid_roll,
               g_test_data[g_data_index-1].pid_pitch,
               g_test_data[g_data_index-1].pid_yaw,
               g_test_data[g_data_index-1].pid_thr);
        printf("  Yaw restricted: %d\n", g_test_data[g_data_index-1].yaw_restricted);
    }
    printf("==================================\n");
}

//============================================================================
// 测试函数
//============================================================================

/**
 * @brief 测试1: 怠速输出测试
 * @note 测试电机在锁状态下的怠速输出
 */
void test_motor_idle_output(void)
{
    printf("\n=== Test 1: Idle Output Test ===\n");
    printf("Expected: All motors output idle speed (%d us)\n", Min_PWM_Out);
    
    // 确保处于锁定状态
    thr_lock_status = LOCKED;
    
    // 运行电机输出函数
    for (int i = 0; i < 100; i++) {
        locked_motor_output();
        delay_ms(10);
        
        if (i % 20 == 0) {
            uint16_t motors[4];
            for (int j = 0; j < TEST_MOTOR_COUNT; j++) {
                motors[j] = get_motor_pwm(j+1);
            }
            print_motor_values(motors, TEST_MOTOR_COUNT, "  Motors");
        }
    }
    
    printf("Idle output test completed\n");
}

/**
 * @brief 测试2: 悬停状态混控测试
 * @note 模拟悬停状态，测试电机混控输出
 */
void test_motor_hover_mix(void)
{
    printf("\n=== Test 2: Hover Mix Test ===\n");
    printf("Setting: roll=0, pitch=0, yaw=0, throttle=middle\n");
    
    // 设置悬停状态
    extern float pid_roll, pid_pitch, pid_yaw, pid_thr;
    pid_roll = 0.0f;
    pid_pitch = 0.0f;
    pid_yaw = 0.0f;
    pid_thr = 1500.0f;  // 悬停油门
    
    // 设置解锁定状态
    thr_lock_status = UNLOCKED;
    ground_air_status = IN_AIR;
    
    // 运行电机输出
    for (int i = 0; i < 50; i++) {
        unlocked_motor_output();
        delay_ms(20);
        
        if (i % 10 == 0) {
            uint16_t motors[4];
            for (int j = 0; j < TEST_MOTOR_COUNT; j++) {
                motors[j] = get_motor_pwm(j+1);
            }
            print_motor_values(motors, TEST_MOTOR_COUNT, "  Hover");
        }
    }
    
    printf("Hover mix test completed\n");
}

/**
 * @brief 测试3: 横滚响应测试
 * @note 测试横滚控制时的电机响应
 */
void test_motor_roll_response(void)
{
    printf("\n=== Test 3: Roll Response Test ===\n");
    
    extern float pid_roll, pid_pitch, pid_yaw, pid_thr;
    pid_pitch = 0.0f;
    pid_yaw = 0.0f;
    pid_thr = 1500.0f;
    
    thr_lock_status = UNLOCKED;
    ground_air_status = IN_AIR;
    
    // 测试正横滚
    printf("Positive roll (+100):\n");
    pid_roll = 100.0f;
    for (int i = 0; i < 25; i++) {
        unlocked_motor_output();
        delay_ms(20);
        if (i % 10 == 0) {
            uint16_t motors[4];
            for (int j = 0; j < TEST_MOTOR_COUNT; j++) {
                motors[j] = get_motor_pwm(j+1);
            }
            print_motor_values(motors, TEST_MOTOR_COUNT, "    Motors");
        }
    }
    
    delay_ms(1000);
    
    // 测试负横滚
    printf("Negative roll (-100):\n");
    pid_roll = -100.0f;
    for (int i = 0; i < 25; i++) {
        unlocked_motor_output();
        delay_ms(20);
        if (i % 10 == 0) {
            uint16_t motors[4];
            for (int j = 0; j < TEST_MOTOR_COUNT; j++) {
                motors[j] = get_motor_pwm(j+1);
            }
            print_motor_values(motors, TEST_MOTOR_COUNT, "    Motors");
        }
    }
    
    printf("Roll response test completed\n");
}

/**
 * @brief 测试4: 俯仰响应测试
 */
void test_motor_pitch_response(void)
{
    printf("\n=== Test 4: Pitch Response Test ===\n");
    
    extern float pid_roll, pid_pitch, pid_yaw, pid_thr;
    pid_roll = 0.0f;
    pid_yaw = 0.0f;
    pid_thr = 1500.0f;
    
    thr_lock_status = UNLOCKED;
    ground_air_status = IN_AIR;
    
    // 测试正俯仰
    printf("Positive pitch (+100):\n");
    pid_pitch = 100.0f;
    for (int i = 0; i < 25; i++) {
        unlocked_motor_output();
        delay_ms(20);
        if (i % 10 == 0) {
            uint16_t motors[4];
            for (int j = 0; j < TEST_MOTOR_COUNT; j++) {
                motors[j] = get_motor_pwm(j+1);
            }
            print_motor_values(motors, TEST_MOTOR_COUNT, "    Motors");
        }
    }
    
    delay_ms(1000);
    
    // 测试负俯仰
    printf("Negative pitch (-100):\n");
    pid_pitch = -100.0f;
    for (int i = 0; i < 25; i++) {
        unlocked_motor_output();
        delay_ms(20);
        if (i % 10 == 0) {
            uint16_t motors[4];
            for (int j = 0; j < TEST_MOTOR_COUNT; j++) {
                motors[j] = get_motor_pwm(j+1);
            }
            print_motor_values(motors, TEST_MOTOR_COUNT, "    Motors");
        }
    }
    
    printf("Pitch response test completed\n");
}

/**
 * @brief 测试5: 偏航响应测试
 */
void test_motor_yaw_response(void)
{
    printf("\n=== Test 5: Yaw Response Test ===\n");
    
    extern float pid_roll, pid_pitch, pid_yaw, pid_thr;
    pid_roll = 0.0f;
    pid_pitch = 0.0f;
    pid_thr = 1500.0f;
    
    thr_lock_status = UNLOCKED;
    ground_air_status = IN_AIR;
    
    // 测试正偏航
    printf("Positive yaw (+100):\n");
    pid_yaw = 100.0f;
    for (int i = 0; i < 25; i++) {
        unlocked_motor_output();
        delay_ms(20);
        if (i % 10 == 0) {
            uint16_t motors[4];
            for (int j = 0; j < TEST_MOTOR_COUNT; j++) {
                motors[j] = get_motor_pwm(j+1);
            }
            print_motor_values(motors, TEST_MOTOR_COUNT, "    Motors");
        }
    }
    
    delay_ms(1000);
    
    // 测试负偏航
    printf("Negative yaw (-100):\n");
    pid_yaw = -100.0f;
    for (int i = 0; i < 25; i++) {
        unlocked_motor_output();
        delay_ms(20);
        if (i % 10 == 0) {
            uint16_t motors[4];
            for (int j = 0; j < TEST_MOTOR_COUNT; j++) {
                motors[j] = get_motor_pwm(j+1);
            }
            print_motor_values(motors, TEST_MOTOR_COUNT, "    Motors");
        }
    }
    
    printf("Yaw response test completed\n");
}

/**
 * @brief 测试6: 油门响应测试
 */
void test_motor_throttle_response(void)
{
    printf("\n=== Test 6: Throttle Response Test ===\n");
    
    extern float pid_roll, pid_pitch, pid_yaw, pid_thr;
    pid_roll = 0.0f;
    pid_pitch = 0.0f;
    pid_yaw = 0.0f;
    
    thr_lock_status = UNLOCKED;
    ground_air_status = IN_AIR;
    
    // 测试低油门
    printf("Low throttle (1200):\n");
    pid_thr = 1200.0f;
    for (int i = 0; i < 25; i++) {
        unlocked_motor_output();
        delay_ms(20);
        if (i % 10 == 0) {
            uint16_t motors[4];
            for (int j = 0; j < TEST_MOTOR_COUNT; j++) {
                motors[j] = get_motor_pwm(j+1);
            }
            print_motor_values(motors, TEST_MOTOR_COUNT, "    Motors");
        }
    }
    
    delay_ms(1000);
    
    // 测试中油门
    printf("Mid throttle (1500):\n");
    pid_thr = 1500.0f;
    for (int i = 0; i < 25; i++) {
        unlocked_motor_output();
        delay_ms(20);
        if (i % 10 == 0) {
            uint16_t motors[4];
            for (int j = 0; j < TEST_MOTOR_COUNT; j++) {
                motors[j] = get_motor_pwm(j+1);
            }
            print_motor_values(motors, TEST_MOTOR_COUNT, "    Motors");
        }
    }
    
    delay_ms(1000);
    
    // 测试高油门
    printf("High throttle (1800):\n");
    pid_thr = 1800.0f;
    for (int i = 0; i < 25; i++) {
        unlocked_motor_output();
        delay_ms(20);
        if (i % 10 == 0) {
            uint16_t motors[4];
            for (int j = 0; j < TEST_MOTOR_COUNT; j++) {
                motors[j] = get_motor_pwm(j+1);
            }
            print_motor_values(motors, TEST_MOTOR_COUNT, "    Motors");
        }
    }
    
    printf("Throttle response test completed\n");
}

/**
 * @brief 测试7: 饱和保护测试
 * @note 测试当电机输出达到极限时的饱和保护机制
 */
void test_motor_saturation_protection(void)
{
    printf("\n=== Test 7: Saturation Protection Test ===\n");
    
    extern float pid_roll, pid_pitch, pid_yaw, pid_thr;
    
    thr_lock_status = UNLOCKED;
    ground_air_status = IN_AIR;
    
    // 设置高油门和大横滚，触发饱和
    printf("Testing saturation (high throttle + large roll):\n");
    pid_thr = 1900.0f;
    pid_roll = 300.0f;
    pid_pitch = 0.0f;
    pid_yaw = 0.0f;
    
    for (int i = 0; i < 50; i++) {
        unlocked_motor_output();
        delay_ms(20);
        
        if (i % 10 == 0) {
            uint16_t motors[4];
            for (int j = 0; j < TEST_MOTOR_COUNT; j++) {
                motors[j] = get_motor_pwm(j+1);
            }
            print_motor_values(motors, TEST_MOTOR_COUNT, "    Motors");
            printf("    Yaw restricted: %d\n", MotorOutput_GetYawRestrictionStatus());
        }
    }
    
    printf("Saturation protection test completed\n");
}

/**
 * @brief 测试8: 电机平滑输出测试
 */
void test_motor_smoothing(void)
{
    printf("\n=== Test 8: Motor Smoothing Test ===\n");
    
    extern float pid_roll, pid_pitch, pid_yaw, pid_thr;
    
    thr_lock_status = UNLOCKED;
    ground_air_status = ON_GROUND;  // 地面状态启用平滑
    
    printf("Testing motor output smoothing (ground mode):\n");
    
    // 突然改变PID值，观察平滑效果
    pid_roll = 0.0f;
    pid_pitch = 0.0f;
    pid_yaw = 0.0f;
    pid_thr = 1500.0f;
    
    for (int i = 0; i < 30; i++) {
        unlocked_motor_output();
        delay_ms(50);
        
        if (i == 10) {
            printf("  Step change: roll=200\n");
            pid_roll = 200.0f;
        }
        
        if (i % 5 == 0) {
            uint16_t motors[4];
            for (int j = 0; j < TEST_MOTOR_COUNT; j++) {
                motors[j] = get_motor_pwm(j+1);
            }
            print_motor_values(motors, TEST_MOTOR_COUNT, "    Motors");
        }
    }
    
    printf("Motor smoothing test completed\n");
}

//============================================================================
// 主测试函数
//============================================================================

/**
 * @brief 运行完整的电机测试
 */
void run_motor_output_test(void)
{
    printf("\n");
    printf("╔═══════════════════════════════════════════════════════════╗\n");
    printf("║           Quadcopter Motor Output Test Suite             ║\n");
    printf("╚═══════════════════════════════════════════════════════════╝\n");
    
    // 初始化
    motor_output_initial();
    board_delay_ms(100);
    
    printf("\nMotor output initialized\n");
    printf("Motor count: %d\n", conf_par.jixing / 10);
    printf("Idle speed: %d us\n", Min_PWM_Out);
    printf("Max PWM: %d us\n", Max_PWM_Out);
    
    g_data_index = 0;
    g_test_running = true;
    
    // 运行测试
    test_motor_idle_output();
    board_delay_ms(1000);
    
    test_motor_hover_mix();
    board_delay_ms(1000);
    
    test_motor_roll_response();
    board_delay_ms(1000);
    
    test_motor_pitch_response();
    board_delay_ms(1000);
    
    test_motor_yaw_response();
    board_delay_ms(1000);
    
    test_motor_throttle_response();
    board_delay_ms(1000);
    
    test_motor_saturation_protection();
    board_delay_ms(1000);
    
    test_motor_smoothing();
    
    // 打印总结
    print_test_summary();
    
    // 恢复安全状态
    thr_lock_status = LOCKED;
    locked_motor_output();
    
    printf("\nAll tests completed!\n");
    g_test_running = false;
}

/**
 * @brief 简单的单步测试
 * @param step 测试步骤: 1-怠速, 2-悬停, 3-横滚, 4-俯仰, 5-偏航
 */
void run_motor_simple_test(uint8_t step)
{
    extern float pid_roll, pid_pitch, pid_yaw, pid_thr;
    
    motor_output_initial();
    board_delay_ms(100);
    
    switch(step) {
        case 1:
            printf("Idle test\n");
            thr_lock_status = LOCKED;
            for (int i = 0; i < 100; i++) {
                locked_motor_output();
                board_delay_ms(10);
            }
            break;
            
        case 2:
            printf("Hover test\n");
            thr_lock_status = UNLOCKED;
            ground_air_status = IN_AIR;
            pid_roll = 0; pid_pitch = 0; pid_yaw = 0; pid_thr = 1500;
            for (int i = 0; i < 100; i++) {
                unlocked_motor_output();
                board_delay_ms(10);
            }
            break;
            
        case 3:
            printf("Roll test\n");
            thr_lock_status = UNLOCKED;
            ground_air_status = IN_AIR;
            pid_roll = 100; pid_pitch = 0; pid_yaw = 0; pid_thr = 1500;
            for (int i = 0; i < 100; i++) {
                unlocked_motor_output();
                board_delay_ms(10);
            }
            break;
            
        case 4:
            printf("Pitch test\n");
            thr_lock_status = UNLOCKED;
            ground_air_status = IN_AIR;
            pid_roll = 0; pid_pitch = 100; pid_yaw = 0; pid_thr = 1500;
            for (int i = 0; i < 100; i++) {
                unlocked_motor_output();
                board_delay_ms(10);
            }
            break;
            
        case 5:
            printf("Yaw test\n");
            thr_lock_status = UNLOCKED;
            ground_air_status = IN_AIR;
            pid_roll = 0; pid_pitch = 0; pid_yaw = 100; pid_thr = 1500;
            for (int i = 0; i < 100; i++) {
                unlocked_motor_output();
                board_delay_ms(10);
            }
            break;
            
        default:
            printf("Invalid test step\n");
            break;
    }
    
    // 恢复锁定
    thr_lock_status = LOCKED;
    locked_motor_output();
    
    printf("Test completed\n");
}
#endif