ArduPlane/VTOL_Assist.cpp

   1 #include "Plane.h"
   2
   3 #if HAL_QUADPLANE_ENABLED
   4
   5 // Assistance hysteresis helpers
   6
   7 // Reset state
   8 void VTOL_Assist::Assist_Hysteresis::reset()
   9 {
  10     start_ms = 0;
  11     last_ms = 0;
  12     active = false;
  13 }
  14
  15 // Update state, return true when first triggered
  16 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     bool ret = false;
  19
  20     if (trigger) {
  21         last_ms = now_ms;
  22         if (start_ms == 0) {
  23             start_ms = now_ms;
  24         }
  25         if ((now_ms - start_ms) > trigger_delay_ms) {
  26             // trigger delay has elapsed
  27             if (!active) {
  28                 // return true on first trigger
  29                 ret = true;
  30             }
  31             active = true;
  32         }
  33
  34     } else if (active) {
  35         if ((last_ms == 0) || ((now_ms - last_ms) > clear_delay_ms)) {
  36             // Clear delay passed
  37             reset();
  38         }
  39
  40     } else {
  41         reset();
  42     }
  43
  44     return ret;
  45 }
  46
  47 // Assistance not needed, reset any state
  48 void VTOL_Assist::reset()
  49 {
  50     force_assist = false;
  51     speed_assist = false;
  52     angle_error.reset();
  53     alt_error.reset();
  54 }
  55
  56 /*
  57   return true if the quadplane should provide stability assistance
  58  */
  59 bool VTOL_Assist::should_assist(float aspeed, bool have_airspeed)
  60 {
  61     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         reset();
  64         return false;
  65     }
  66
  67     if (!quadplane.tailsitter.enabled() && !( (plane.control_mode->does_auto_throttle() && !plane.throttle_suppressed)
  68                                                                       || is_positive(plane.get_throttle_input())
  69                                                                       || plane.is_flying() ) ) {
  70         // not in a flight mode and condition where it would be safe to turn on vertial lift motors
  71         // skip this check for tailsitters because the forward and vertial motors are the same and are controled directly by throttle imput unlike other quadplanes
  72         reset();
  73         return false;
  74     }
  75
  76     if (plane.flare_mode != Plane::FlareMode::FLARE_DISABLED) {
  77         // Never active in fixed wing flare
  78         reset();
  79         return false;
  80     }
  81
  82     force_assist = state == STATE::FORCE_ENABLED;
  83
  84     if (speed <= 0) {
  85         // all checks disabled via speed threshold, still allow force enabled
  86         speed_assist = false;
  87         alt_error.reset();
  88         angle_error.reset();
  89         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     speed_assist = (have_airspeed && aspeed < speed) &&
  95        (!quadplane.option_is_set(QuadPlane::OPTION::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || plane.ahrs.using_airspeed_sensor());
  96
  97     const uint32_t now_ms = AP_HAL::millis();
  98     const uint32_t tigger_delay_ms = delay * 1000;
  99     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     if (alt <= 0) {
 105         // Alt assist disabled
 106         alt_error.reset();
 107
 108     } else {
 109         const float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
 110         if (alt_error.update(height_above_ground < alt, now_ms, tigger_delay_ms, clear_delay_ms)) {
 111             gcs().send_text(MAV_SEVERITY_WARNING, "Alt assist %.1fm", height_above_ground);
 112         }
 113     }
 114
 115     if (angle <= 0) {
 116         // Angle assist disabled
 117         angle_error.reset();
 118
 119     } else {
 120
 121         /*
 122         now check if we should provide assistance due to attitude error
 123         */
 124         const uint16_t allowed_envelope_error_cd = 500U;
 125         const bool inside_envelope = (labs(plane.ahrs.roll_sensor) <= (plane.aparm.roll_limit*100 + allowed_envelope_error_cd)) &&
 126                                      (plane.ahrs.pitch_sensor < (plane.aparm.pitch_limit_max*100 + allowed_envelope_error_cd)) &&
 127                                      (plane.ahrs.pitch_sensor > (plane.aparm.pitch_limit_min*100 - allowed_envelope_error_cd));
 128
 129         const int32_t max_angle_cd = 100U*angle;
 130         const bool inside_angle_error = (labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) < max_angle_cd) &&
 131                                         (labs(plane.ahrs.pitch_sensor - plane.nav_pitch_cd) < max_angle_cd);
 132
 133         if (angle_error.update(!inside_envelope && !inside_angle_error, now_ms, tigger_delay_ms, clear_delay_ms)) {
 134             gcs().send_text(MAV_SEVERITY_WARNING, "Angle assist r=%d p=%d",
 135                                          (int)(plane.ahrs.roll_sensor/100),
 136                                          (int)(plane.ahrs.pitch_sensor/100));
 137         }
 138     }
 139
 140     return force_assist || speed_assist || alt_error.is_active() || angle_error.is_active();
 141 }
 142
 143 #endif  // HAL_QUADPLANE_ENABLED