Line data    Source code

       1             : #include "Plane.h"
       2             : 
       3             : #if HAL_QUADPLANE_ENABLED
       4             : 
       5             : // Assistance hysteresis helpers
       6             : 
       7             : // Reset state
       8    12967675 : void VTOL_Assist::Assist_Hysteresis::reset()
       9             : {
      10    12967675 :     start_ms = 0;
      11    12967675 :     last_ms = 0;
      12    12967675 :     active = false;
      13    12967675 : }
      14             : 
      15             : // Update state, return true when first triggered
      16     2611856 : bool VTOL_Assist::Assist_Hysteresis::update(const bool trigger, const uint32_t &now_ms, const uint32_t &trigger_delay_ms, const uint32_t &clear_delay_ms)
      17             : {
      18     2611856 :     bool ret = false;
      19             : 
      20     2611856 :     if (trigger) {
      21       16906 :         last_ms = now_ms;
      22       16906 :         if (start_ms == 0) {
      23          49 :             start_ms = now_ms;
      24             :         }
      25       16906 :         if ((now_ms - start_ms) > trigger_delay_ms) {
      26             :             // trigger delay has elapsed
      27        9512 :             if (!active) {
      28             :                 // return true on first trigger
      29          48 :                 ret = true;
      30             :             }
      31        9512 :             active = true;
      32             :         }
      33             : 
      34     2594950 :     } else if (active) {
      35       13839 :         if ((last_ms == 0) || ((now_ms - last_ms) > clear_delay_ms)) {
      36             :             // Clear delay passed
      37          46 :             reset();
      38             :         }
      39             : 
      40             :     } else {
      41     2581111 :         reset();
      42             :     }
      43             : 
      44     2611856 :     return ret;
      45             : }
      46             : 
      47             : // Assistance not needed, reset any state
      48     3887327 : void VTOL_Assist::reset()
      49             : {
      50     3887327 :     force_assist = false;
      51     3887327 :     speed_assist = false;
      52     3887327 :     angle_error.reset();
      53     3887327 :     alt_error.reset();
      54     3887327 : }
      55             : 
      56             : /*
      57             :   return true if the quadplane should provide stability assistance
      58             :  */
      59     3472722 : bool VTOL_Assist::should_assist(float aspeed, bool have_airspeed)
      60             : {
      61     3472722 :     if (!plane.arming.is_armed_and_safety_off() || (state == STATE::ASSIST_DISABLED) || quadplane.tailsitter.is_control_surface_tailsitter()) {
      62             :         // disarmed or disabled by aux switch or because a control surface tailsitter
      63      834470 :         reset();
      64      834470 :         return false;
      65             :     }
      66             : 
      67     2856578 :     if (!quadplane.tailsitter.enabled() && !( (plane.control_mode->does_auto_throttle() && !plane.throttle_suppressed)
      68      218326 :                                                                       || is_positive(plane.get_throttle_input()) 
      69       28862 :                                                                       || plane.is_flying() ) ) {
      70             :         // not in a flight mode and condition where it would be safe to turn on vertical lift motors
      71             :         // skip this check for tailsitters because the forward and vertical motors are the same and are controlled directly by throttle input unlike other quadplanes
      72       26392 :         reset();
      73       26392 :         return false;
      74             :     }
      75             : 
      76     2611860 :     if (plane.flare_mode != Plane::FlareMode::FLARE_DISABLED) {
      77             :         // Never active in fixed wing flare
      78           0 :         reset();
      79           0 :         return false;
      80             :     }
      81             : 
      82     2611860 :     force_assist = state == STATE::FORCE_ENABLED;
      83             : 
      84     2611860 :     if (speed <= 0) {
      85             :         // all checks disabled via speed threshold, still allow force enabled
      86         358 :         speed_assist = false;
      87         358 :         alt_error.reset();
      88         358 :         angle_error.reset();
      89         358 :         return force_assist;
      90             :     }
      91             : 
      92             :     // assistance due to Q_ASSIST_SPEED
      93             :     // if option bit is enabled only allow assist with real airspeed sensor
      94     2766498 :     speed_assist = (have_airspeed && aspeed < speed) && 
      95      154996 :        (!quadplane.option_is_set(QuadPlane::Option::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || plane.ahrs.using_airspeed_sensor());
      96             : 
      97     2611502 :     const uint32_t now_ms = AP_HAL::millis();
      98     2611502 :     const uint32_t tigger_delay_ms = delay * 1000;
      99     2611502 :     const uint32_t clear_delay_ms = tigger_delay_ms * 2;
     100             : 
     101             :     /*
     102             :       optional assistance when altitude is too close to the ground
     103             :      */
     104     2611502 :     if (alt <= 0) {
     105             :         // Alt assist disabled
     106     2606887 :         alt_error.reset();
     107             : 
     108             :     } else {
     109        4615 :         const float height_above_ground = plane.relative_ground_altitude(RangeFinderUse::ASSIST);
     110        4615 :         if (alt_error.update(height_above_ground < alt, now_ms, tigger_delay_ms, clear_delay_ms)) {
     111           1 :             gcs().send_text(MAV_SEVERITY_WARNING, "Alt assist %.1fm", height_above_ground);
     112             :         }
     113             :     }
     114             : 
     115     2611502 :     if (angle <= 0 || plane.fly_inverted()) {
     116             :         // Angle assist disabled
     117        4261 :         angle_error.reset();
     118             : 
     119             :     } else {
     120             : 
     121             :         /*
     122             :         now check if we should provide assistance due to attitude error
     123             :         */
     124     2607241 :         const auto ahrs_roll_deg = plane.ahrs.get_roll_deg();
     125     2607241 :         const auto ahrs_pitch_deg = plane.ahrs.get_pitch_deg();
     126     2607241 :         constexpr float allowed_envelope_error_deg = 5.0;
     127             :         const bool inside_envelope =
     128     2607241 :             (fabsf(ahrs_roll_deg) <= (plane.aparm.roll_limit + allowed_envelope_error_deg)) &&
     129     5200196 :             (ahrs_pitch_deg < (plane.aparm.pitch_limit_max + allowed_envelope_error_deg)) &&
     130     2592955 :             (ahrs_pitch_deg > (plane.aparm.pitch_limit_min - allowed_envelope_error_deg));
     131             : 
     132             :         const bool inside_angle_error =
     133     5194869 :             (fabsf(plane.rollController.get_angle_error_deg()) < angle) &&
     134     2587628 :             (fabsf(plane.pitchController.get_angle_error_deg()) < angle);
     135             : 
     136     2607241 :         if (angle_error.update(!inside_envelope && !inside_angle_error, now_ms, tigger_delay_ms, clear_delay_ms)) {
     137          47 :             gcs().send_text(MAV_SEVERITY_WARNING, "Angle assist r=%d p=%d",
     138             :                             (int)ahrs_roll_deg,
     139             :                             (int)ahrs_pitch_deg);
     140             :         }
     141             :     }
     142             : 
     143     2611502 :     return force_assist || speed_assist || alt_error.is_active() || angle_error.is_active();
     144             : }
     145             : 
     146             : /*
     147             :   check if we are in VTOL recovery
     148             : */
     149     3776513 : bool VTOL_Assist::check_VTOL_recovery(void)
     150             : {
     151             :     const bool allow_fw_recovery =
     152     3776513 :         !option_is_set(OPTION::FW_FORCE_DISABLED) &&
     153     7427209 :         !quadplane.tailsitter.enabled() &&
     154     3650696 :         plane.control_mode != &plane.mode_qacro;
     155     3776513 :     if (!allow_fw_recovery) {
     156      134038 :         quadplane.force_fw_control_recovery = false;
     157      134038 :         quadplane.in_spin_recovery = false;
     158      134038 :         return false;
     159             :     }
     160             : 
     161             :     // see if the attitude is outside twice the Q_A_ANGLE_MAX
     162     3642475 :     const auto &ahrs = plane.ahrs;
     163     3642475 :     const float angle_max_cd = quadplane.attitude_control->lean_angle_max_cd();
     164     3642475 :     const float abs_angle_cd = fabsf(Vector2f{float(ahrs.roll_sensor), float(ahrs.pitch_sensor)}.length());
     165             : 
     166     3642475 :     if (abs_angle_cd > 2*angle_max_cd) {
     167             :         // we are 2x the angle limits, trigger fw recovery
     168       47583 :         quadplane.force_fw_control_recovery = true;
     169             :     }
     170             : 
     171     3642475 :     if (quadplane.force_fw_control_recovery) {
     172             :         // stop fixed wing recovery if inside Q_A_ANGLE_MAX
     173       64734 :         if (abs_angle_cd <= angle_max_cd) {
     174         108 :             quadplane.force_fw_control_recovery = false;
     175         108 :             quadplane.attitude_control->reset_target_and_rate(false);
     176             : 
     177         108 :             if (ahrs.groundspeed() > quadplane.wp_nav->get_default_speed_NE_ms()) {
     178             :                 /* if moving at high speed also reset position
     179             :                    controller and height controller
     180             : 
     181             :                    this avoids an issue where the position
     182             :                    controller may limit pitch after a strong
     183             :                    acceleration event
     184             :                 */
     185          90 :                 quadplane.pos_control->D_init_controller();
     186          90 :                 quadplane.pos_control->NE_init_controller();
     187             :             }
     188             :         }
     189             :     }
     190             : 
     191     7284950 :     if (!option_is_set(OPTION::SPIN_DISABLED) &&
     192     3642475 :         quadplane.force_fw_control_recovery) {
     193             :         // additionally check for needing spin recovery
     194       64626 :         const auto &gyro = plane.ahrs.get_gyro();
     195       64626 :         quadplane.in_spin_recovery =
     196       97742 :             fabsf(gyro.z) > radians(10) &&
     197       64850 :             fabsf(gyro.x) > radians(30) &&
     198       31734 :             fabsf(gyro.y) > radians(30) &&
     199      120981 :             gyro.x * gyro.z < 0 &&
     200       23239 :             plane.ahrs.get_pitch_deg() < -45;
     201             :     } else {
     202     3577849 :         quadplane.in_spin_recovery = false;
     203             :     }
     204             :     
     205     3642475 :     return quadplane.force_fw_control_recovery;
     206             : }
     207             : 
     208             : 
     209             : /* if we are in a spin then counter with rudder and elevator
     210             : 
     211             :    if roll rate and yaw rate are opposite and yaw rate is
     212             :    significant then put in full rudder to counter the yaw rate
     213             :    for spin recovery
     214             : */
     215     6652785 : void VTOL_Assist::output_spin_recovery(void)
     216             : {
     217     6652785 :     if (!quadplane.in_spin_recovery) {
     218     6650069 :         return;
     219             :     }
     220        2716 :     if (quadplane.motors->get_desired_spool_state() !=
     221             :         AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
     222             :         // if we and no longer running the VTOL motors we need to
     223             :         // clear the spin flag
     224           0 :         quadplane.in_spin_recovery = false;
     225           0 :         return;
     226             :     }
     227        2716 :     const Vector3f &gyro = plane.ahrs.get_gyro();
     228             : 
     229             :     // put in opposite rudder to counter yaw, and neutral
     230             :     // elevator until we're out of the spin
     231        2716 :     SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, gyro.z > 0 ? -SERVO_MAX : SERVO_MAX);
     232        2716 :     SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 0);
     233             : }
     234             : 
     235             : 
     236             : #endif  // HAL_QUADPLANE_ENABLED