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"

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