#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