|
#include "Plane.h" /* 计算控制面的速度缩放因子。 这个因子用于改变PID的缩放，使其随速度变化。 在高速情况下，将控制面移动得更少，而在低速情况下，将它们移动得更多。 */ float Plane::calc_speed_scaler(void) { float aspeed, speed_scaler; if (ahrs.airspeed_estimate(aspeed)) { if (aspeed > auto_state.highest_airspeed && hal.util->get_soft_armed()) { auto_state.highest_airspeed = aspeed; } // 确保在整个配置的空速范围内都有缩放效果。
const float airspeed_min = MAX(aparm.airspeed_min, MIN_AIRSPEED_MIN); const float scale_min = MIN(0.5, g.scaling_speed / (2.0 * aparm.airspeed_max)); const float scale_max = MAX(2.0, g.scaling_speed / (0.7 * airspeed_min)); if (aspeed > 0.0001f) { speed_scaler = g.scaling_speed / aspeed; } else { speed_scaler = scale_max; } speed_scaler = constrain_float(speed_scaler, scale_min, scale_max); #if HAL_QUADPLANE_ENABLED if (quadplane.in_vtol_mode() && hal.util->get_soft_armed()) { // 当处于VTOL模式时，在低速情况下限制控制面的移动，以防止不稳定。 float threshold = airspeed_min * 0.5; if (aspeed < threshold) { float new_scaler = linear_interpolate(0.001, g.scaling_speed / threshold, aspeed, 0, threshold); speed_scaler = MIN(speed_scaler, new_scaler);
// 对积分器进行衰减，以防止在低速时的积分器持续发生作用以影响高速状态的飞行性能。 rollController.decay_I(); pitchController.decay_I(); yawController.decay_I(); } } #endif } else if (hal.util->get_soft_armed()) { // scale assumed surface movement using throttle output // 使用油门输出来缩放预估的控制面移动。 float throttle_out = MAX(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 1); speed_scaler = sqrtf(THROTTLE_CRUISE / throttle_out); //对速度面进行调节 在工程实践上，由于空速测量的限制，可能没有真正的速度信息，这个案件的限制更加严格。 speed_scaler = constrain_float(speed_scaler, 0.6f, 1.67f); } else {
// 如果没有速度估计且未解锁，则使用单位标度。 speed_scaler = 1; } if (!plane.ahrs.airspeed_sensor_enabled() && (plane.g2.flight_options & FlightOptions::SURPRESS_TKOFF_SCALING) && (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF)) { //scaling is surpressed during climb phase of automatic takeoffs with no airspeed sensor being used due to problems with inaccurate airspeed estimates // 如果由于不准确的空速估计而没有使用空速传感器，则在自动起飞的爬升阶段抑制标度。 return MIN(speed_scaler, 1.0f) ; } return speed_scaler; } /* 如果当前的设置和模式允许摇杆混控，则返回真。 */ bool Plane::stick_mixing_enabled(void) { if (!rc().has_valid_input()) { // 如果没有有效的遥控器信号不会进行摇杆混控 return false; } #if AP_FENCE_ENABLED const bool stickmixing = fence_stickmixing(); #else const bool stickmixing = true; #endif #if HAL_QUADPLANE_ENABLED if (control_mode == &mode_qrtl && quadplane.poscontrol.get_state() >= QuadPlane::QPOS_POSITION1) { // 用户可能在重新定位。 return false; } if (quadplane.in_vtol_land_poscontrol()) { // 用户可能在重新定位。 return false; } #endif if (control_mode->does_auto_throttle() && plane.control_mode->does_auto_navigation()) { // 处于自动模式。检查摇杆混控标志。 if (g.stick_mixing != StickMixing::NONE && g.stick_mixing != StickMixing::VTOL_YAW && stickmixing) { return true; } else { return false; } } if (failsafe.rc_failsafe && g.fs_action_short == FS_ACTION_SHORT_FBWA) { // 在FBWA滑翔模式下不进行摇杆混控 return false; } // 在非自动模式，始终进行摇杆混控 return true; }
/* 这是主要的横滚稳定函数。 它接收先前设置的导航横滚角(nav_roll)， 计算滚动伺服输出(roll servo_out)来尝试将飞机在给定的横滚角度上稳定。 */ void Plane::stabilize_roll(float speed_scaler) { if (fly_inverted()) {
// 倒飞。需要处理滚转传感器的数据包影响导航横滚角的数据包，这会迷惑PID代码。 // 处理这种情况的最简单方法是确保两个变量从零开始朝同一方向变化。 nav_roll_cd += 18000; if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000; } const float roll_out = stabilize_roll_get_roll_out(speed_scaler); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_out); } float Plane::stabilize_roll_get_roll_out(float speed_scaler) { #if HAL_QUADPLANE_ENABLED if (!quadplane.use_fw_attitude_controllers()) { // 使用VTOL速率进行控制，以确保一致性。 const auto &pid_info = quadplane.attitude_control->get_rate_roll_pid().get_pid_info(); const float roll_out = rollController.get_rate_out(degrees(pid_info.target), speed_scaler); /* 当将固定翼控制器从VTOL控制器转换到时， 需要将积分器衰减，以防止对立的积分器在两个控制器之间平衡 */ rollController.decay_I(); return roll_out; } #endif bool disable_integrator = false; if (control_mode == &mode_stabilize && channel_roll->get_control_in() != 0) { disable_integrator = true; } return rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, speed_scaler, disable_integrator, ground_mode && !(plane.g2.flight_options & FlightOptions::DISABLE_GROUND_PID_SUPPRESSION)); } /* 这是主要的俯仰稳定功能。 它使用先前设置的导航俯仰角计算舵机输出值， 以尝试将飞机稳定在给定的姿态上。 */ void Plane::stabilize_pitch(float speed_scaler) { int8_t force_elevator = takeoff_tail_hold(); if (force_elevator != 0) { // 在起飞过程中，我们会将升降舵。只需将百分比转换为-4500~4500百分子度。 SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 45*force_elevator); return; } const float pitch_out = stabilize_pitch_get_pitch_out(speed_scaler); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_out); } float Plane::stabilize_pitch_get_pitch_out(float speed_scaler) { #if HAL_QUADPLANE_ENABLED if (!quadplane.use_fw_attitude_controllers()) { // 使用VTOL速率进行控制，以确保一致性。 const auto &pid_info = quadplane.attitude_control->get_rate_pitch_pid().get_pid_info(); const int32_t pitch_out = pitchController.get_rate_out(degrees(pid_info.target), speed_scaler); /* 当将固定翼控制器从VTOL控制器转换到固定翼模式时，
需要将积分器衰减，以防止对立的积分器在两个控制器之间平衡。 */ pitchController.decay_I(); return pitch_out; } #endif
// 如果 LANDING_FLARE RCx_OPTION 开关已设置并处于固定翼模式下， //并且使用手动油门以及怠速油门，飞行选项 FORCE_FLARE_ATTITUDE 已设置， //则将俯仰角设为 LAND_PITCH_CD。 #if HAL_QUADPLANE_ENABLED const bool quadplane_in_transition = quadplane.in_transition(); #else const bool quadplane_in_transition = false; #endif int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch; bool disable_integrator = false; if (control_mode == &mode_stabilize && channel_pitch->get_control_in() != 0) { disable_integrator = true; } /* 强制着陆姿态角，如果满足以下条件： - 滑行开关处于高位。 - 油门杆处于零推力位置。 - 处于固定翼非自动油门模式。 */ if (!quadplane_in_transition && !control_mode->is_vtol_mode() && !control_mode->does_auto_throttle() && flare_mode == FlareMode::ENABLED_PITCH_TARGET && throttle_at_zero()) { demanded_pitch = landing.get_pitch_cd(); } return pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator, ground_mode && !(plane.g2.flight_options & FlightOptions::DISABLE_GROUND_PID_SUPPRESSION)); } /* 这使用户可以在稳定模式下控制飞机 */ void Plane::stabilize_stick_mixing_direct() { if (!stick_mixing_enabled() || control_mode == &mode_acro || control_mode == &mode_fbwa || control_mode == &mode_autotune || control_mode == &mode_fbwb || control_mode == &mode_cruise || #if HAL_QUADPLANE_ENABLED control_mode == &mode_qstabilize || control_mode == &mode_qhover || control_mode == &mode_qloiter || control_mode == &mode_qland || control_mode == &mode_qrtl || control_mode == &mode_qacro || #if QAUTOTUNE_ENABLED control_mode == &mode_qautotune || #endif #endif control_mode == &mode_training) { return; } float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); aileron = channel_roll->stick_mixing(aileron); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron); if ((control_mode == &mode_loiter) && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { // loiter is using altitude control based on the pitch stick, don't use it again here // 盘旋（loiter）模式使用的海拔控制是基于俯仰杆的，请不要在这里重复使用。 return; } float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator); elevator = channel_pitch->stick_mixing(elevator); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator); } /* 使用户能够在稳定模式下控制飞机，使用FBW风格的控制。 */ void Plane::stabilize_stick_mixing_fbw() { if (!stick_mixing_enabled() || control_mode == &mode_acro || control_mode == &mode_fbwa || control_mode == &mode_autotune || control_mode == &mode_fbwb || control_mode == &mode_cruise || #if HAL_QUADPLANE_ENABLED control_mode == &mode_qstabilize || control_mode == &mode_qhover || control_mode == &mode_qloiter || control_mode == &mode_qland || control_mode == &mode_qacro || #if QAUTOTUNE_ENABLED control_mode == &mode_qautotune || #endif #endif // HAL_QUADPLANE_ENABLED control_mode == &mode_training) { return; } /*进行FBW风格的摇杆混合。我们不以线性方式处理它。
然而，在输入达到最大值的一半之前，在nav_roll和nav_pitch上使用线性加法。
超过这个范围后，它会变为非线性，并最终达到2倍的最大值，
以确保用户可以通过摇杆混合在任何方向上控制飞机。*/ float roll_input = channel_roll->norm_input(); if (roll_input > 0.5f) { roll_input = (3*roll_input - 1); } else if (roll_input < -0.5f) { roll_input = (3*roll_input + 1); } nav_roll_cd += roll_input * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); if ((control_mode == &mode_loiter) && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { // Loiter模式基于俯仰杆使用高度控制，请勿在此处再次使用。 return; } float pitch_input = channel_pitch->norm_input(); if (pitch_input > 0.5f) { pitch_input = (3*pitch_input - 1); } else if (pitch_input < -0.5f) { pitch_input = (3*pitch_input + 1); } if (fly_inverted()) { pitch_input = -pitch_input; } if (pitch_input > 0) { nav_pitch_cd += pitch_input * aparm.pitch_limit_max_cd; } else { nav_pitch_cd += -(pitch_input * pitch_limit_min_cd); } nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); }
/* 偏航轴增稳。有三种操作模式： - 在地面转向的情况下，保持特定的航向。 - 通在地面转向的情况下，进行速率控制。 - 用于协调飞行的偏航控制 */ void Plane::stabilize_yaw(float speed_scaler) { if (landing.is_flaring()) { // 在进行抬头过程中，启用地面转向控制。 steering_control.ground_steering = true; } else {
// 否则，当没有输入控制且我们低于GROUND_STEER_ALT(飞机在接近着陆并准备好进行地面控制时， // 控制应该从空中控制方式转换为地面控制方式的高度值)时，使用地面转向控制。 steering_control.ground_steering = (channel_roll->get_control_in() == 0 && fabsf(relative_altitude) < g.ground_steer_alt); if (!landing.is_ground_steering_allowed()) { // 在着陆接近阶段，请勿使用地面转向控制 steering_control.ground_steering = false; } }
/* 首先计算舵舵杆的转向控制（steering_control.steering）以用于前轮或后轮。 当进行起落过程中（飞机机翼保持水平）或在FBWA模式下（高度低于GROUND_STEER_ALT）进行航向保持时， 我们使用舵舵杆的“航向保持”模式为舵进行控制。 */ if (landing.is_flaring() || (steer_state.hold_course_cd != -1 && steering_control.ground_steering)) { calc_nav_yaw_course(); } else if (steering_control.ground_steering) { calc_nav_yaw_ground(); } /*
现在计算舵舵杆的舵向控制（steering_control.rudder）。 */ calc_nav_yaw_coordinated(speed_scaler); }
/* 在训练模式下特别提供的稳定性功能 */ void Plane::stabilize_training(float speed_scaler) { const float rexpo = roll_in_expo(false); const float pexpo = pitch_in_expo(false); if (training_manual_roll) { SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo); } else { // 计算保持所需的控制量 stabilize_roll(speed_scaler); if ((nav_roll_cd > 0 && rexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)) || (nav_roll_cd < 0 && rexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_aileron))) { // 允许用户使飞机退出横滚状态 SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo); } } if (training_manual_pitch) { SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo); } else { stabilize_pitch(speed_scaler); if ((nav_pitch_cd > 0 && pexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)) || (nav_pitch_cd < 0 && pexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_elevator))) { // 允许用户使飞机回到水平飞行状态 SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo); } } stabilize_yaw(speed_scaler); }
/* 这是ACRO模式的稳定功能。它对滚转和俯仰轴进行速率稳定控制。 */ void Plane::stabilize_acro(float speed_scaler) { const float rexpo = roll_in_expo(true); const float pexpo = pitch_in_expo(true); float roll_rate = (rexpo/SERVO_MAX) * g.acro_roll_rate; float pitch_rate = (pexpo/SERVO_MAX) * g.acro_pitch_rate; /*
检查是否需要在俯仰极限附近进行特殊的滚转操纵处理。 */ if (g.acro_locking && is_zero(roll_rate)) { /* 由于没有滚转操纵杆输入， 所以我们将进入"滚转锁定"模式， 并保持在释放操纵杆时的滚转状态。 */ if (!acro_state.locked_roll) { acro_state.locked_roll = true; acro_state.locked_roll_err = 0; } else { acro_state.locked_roll_err += ahrs.get_gyro().x * G_Dt; } int32_t roll_error_cd = -ToDeg(acro_state.locked_roll_err)*100; nav_roll_cd = ahrs.roll_sensor + roll_error_cd;
// 尝试将集成角度误差减少到零。我们将“stabilze”设置为true，这将禁用滚转积分器。 SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_servo_out(roll_error_cd, speed_scaler, true, false)); } else { /*
用户没有进行任何俯仰操纵杆输入， 因此我们会在他们释放操纵杆的时候锁定俯仰角度。 */ acro_state.locked_roll = false; SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_rate_out(roll_rate, speed_scaler)); } if (g.acro_locking && is_zero(pitch_rate)) { /* 用户没有进行俯仰输入，因此我们会在用户释放摇杆的时候锁定俯仰。 在用户没有进行俯仰输入的情况下，可以通过将俯仰锁定在用户释放摇杆时的姿态点来实现。
这意味着飞机将保持当前的俯仰姿态，并在用户不再主动控制俯仰时保持稳定。 */ if (!acro_state.locked_pitch) { acro_state.locked_pitch = true; acro_state.locked_pitch_cd = ahrs.pitch_sensor; } // 尝试保持锁定的俯仰姿态。请注意，我们启用了俯仰积分器，这有助于倒飞。 /*尝试保持锁定的俯仰姿态。请注意，我们启用了俯仰积分器，它有助于倒飞。 要保持锁定的俯仰姿态，可以使用姿态控制器和反馈控制技术。 姿态控制器根据期望的俯仰姿态和测量到的实际俯仰姿态之间的误差生成控制输出，以使飞机的俯仰角逐渐趋近于期望值。 考虑到启用了俯仰积分器和可能进行倒飞，该积分器可以帮助保持飞机的俯仰姿态。 积分器通过积累俯仰误差并添加到控制输出中，可以在俯仰持续变化的情况下提供额外的控制力，以稳定飞机的俯仰姿态*/ nav_pitch_cd = acro_state.locked_pitch_cd; SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor, speed_scaler, false, false)); } else { /*
如果用户有非零俯仰输入，则使用纯速率控制器。 用户输入了非零的俯仰输入，并要求使用纯速率控制器。 在这种情况下，纯速率控制器用于控制飞机的偏航速率，而不会对俯仰或滚转产生影响。 纯速率控制器通过根据期望的偏航速率和测量到的实际偏航速率之间的误差产生控制输出，以使得飞机的偏航速率逐渐趋近于期望值。 为了应对用户输入的非零俯仰输入，可以使用纯速率控制器来保持飞机的偏航稳定。 即使有俯仰输入，纯速率控制器仍然只专注于控制偏航速率，并将飞机保持在水平方向上。 在实现纯速率控制器时，需要考虑飞行控制系统和硬件的支持程度。 可能需要根据飞机的特定控制逻辑和用户输入来调整控制器的设计参数和增益设置，以实现准确和稳定的偏航速率控制。 */ acro_state.locked_pitch = false; SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_rate_out(pitch_rate, speed_scaler)); } steering_control.steering = rudder_input(); if (g.acro_yaw_rate > 0 && yawController.rate_control_enabled()) { // 用户要求使用按ACRO_YAW_RATE缩放的偏航速率进行偏航速率控制 // ACRO_YAW_RATE是姿态控制中的一个参数，用于设置遥控器控制下的飞行器偏航速率 /*用户要求使用偏航速率控制，并且根据 ACRO_YAW_RATE 缩放偏航速率。 在飞行控制中，偏航速率控制是指控制飞机偏航运动的变化速率。ACRO_YAW_RATE 是一个参数，用于调整偏航速率的比例。 使用偏航速率控制，可以通过调整控制器输出或调整姿态稳定系统的增益来控制飞机的偏航速率。
根据用户的要求，可以使用 ACRO_YAW_RATE 参数来缩放偏航速率的大小，以满足用户的需求。*/ const float rudd_expo = rudder_in_expo(true); const float yaw_rate = (rudd_expo/SERVO_MAX) * g.acro_yaw_rate; steering_control.steering = steering_control.rudder = yawController.get_rate_out(yaw_rate, speed_scaler, false); } else if (plane.g2.flight_options & FlightOptions::ACRO_YAW_DAMPER) { // 使用偏航控制器。 calc_nav_yaw_coordinated(speed_scaler); } else { /*
手动舵。 */ steering_control.rudder = steering_control.steering; } } |
|
|
