AP_AHRS: allow for fallback to DCM in quadplanes

this adds the necessary functions to allow for flying a quadplane on
DCM as an emergency fallback. It sets the NavGainScalar to 0.5 to
reduce the VTOL controller gains to allow planes to cope with the
higher lag of DCM

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -119,6 +119,9 @@ AP_AHRS_DCM::update(bool skip_ins_update)
 
     // update takeoff/touchdown flags
     update_flags();
+
+    // remember the last origin for fallback support
+    IGNORE_RETURN(AP::ahrs().get_origin(last_origin));
 }
 
 /*
@@ -739,6 +742,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
         // use GPS for positioning with any fix, even a 2D fix
         _last_lat = _gps.location().lat;
         _last_lng = _gps.location().lng;
+        _last_pos_ms = AP_HAL::millis();
         _position_offset_north = 0;
         _position_offset_east = 0;
 
@@ -1036,10 +1040,13 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const
     loc.terrain_alt = 0;
     loc.offset(_position_offset_north, _position_offset_east);
     const AP_GPS &_gps = AP::gps();
-    if (_flags.fly_forward && _have_position) {
+    if (_have_position) {
-        float gps_delay_sec = 0;
+        uint32_t now = AP_HAL::millis();
-        _gps.get_lag(gps_delay_sec);
+        float dt = 0;
-        loc.offset_bearing(_gps.ground_course(), _gps.ground_speed() * gps_delay_sec);
+        _gps.get_lag(dt);
+        dt += constrain_float((now - _last_pos_ms) * 0.001, 0, 0.5);
+        Vector2f dpos = _last_velocity.xy() * dt;
+        loc.offset(dpos.x, dpos.y);
     }
     return _have_position;
 }
@@ -1160,3 +1167,50 @@ bool AP_AHRS_DCM::pre_arm_check(bool requires_position, char *failure_msg, uint8
     }
     return true;
 }
+
+/*
+  relative-origin functions for fallback in AP_InertialNav
+*/
+bool AP_AHRS_DCM::get_origin_fallback(Location &ret) const
+{
+    ret = last_origin;
+    if (ret.is_zero()) {
+        // use home if we never have had an origin
+        ret = AP::ahrs().get_home();
+    }
+    return !ret.is_zero();
+}
+
+bool AP_AHRS_DCM::get_relative_position_NED_origin(Vector3f &posNED) const
+{
+    Location origin;
+    if (!AP_AHRS_DCM::get_origin_fallback(origin)) {
+        return false;
+    }
+    Location loc;
+    if (!AP_AHRS_DCM::get_position(loc)) {
+        return false;
+    }
+    posNED = origin.get_distance_NED(loc);
+    return true;
+}
+
+bool AP_AHRS_DCM::get_relative_position_NE_origin(Vector2f &posNE) const
+{
+    Vector3f posNED;
+    if (!AP_AHRS_DCM::get_relative_position_NED_origin(posNED)) {
+        return false;
+    }
+    posNE = posNED.xy();
+    return true;
+}
+
+bool AP_AHRS_DCM::get_relative_position_D_origin(float &posD) const
+{
+    Vector3f posNED;
+    if (!AP_AHRS_DCM::get_relative_position_NED_origin(posNED)) {
+        return false;
+    }
+    posD = posNED.z;
+    return true;
+}

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -121,6 +121,12 @@ public:
     // requires_position should be true if horizontal position configuration should be checked (not used)
     bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const override;
 
+    // relative-origin functions for fallback in AP_InertialNav
+    bool get_origin_fallback(Location &ret) const;
+    bool get_relative_position_NED_origin(Vector3f &vec) const override;
+    bool get_relative_position_NE_origin(Vector2f &posNE) const override;
+    bool get_relative_position_D_origin(float &posD) const override;
+    
 private:
     float _ki;
     float _ki_yaw;
@@ -187,6 +193,7 @@ private:
     // the lat/lng where we last had GPS lock
     int32_t _last_lat;
     int32_t _last_lng;
+    uint32_t _last_pos_ms;
 
     // position offset from last GPS lock
     float _position_offset_north;
@@ -212,4 +219,6 @@ private:
 
     // time when DCM was last reset
     uint32_t _last_startup_ms;
+
+    Location last_origin;
 };

libraries/AP_AHRS/AP_AHRS_Logging.cpp

@@ -110,7 +110,8 @@ void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const
         control_yaw     : (uint16_t)wrap_360_cd(targets.z),
         yaw             : (uint16_t)wrap_360_cd(yaw_sensor),
         error_rp        : (uint16_t)(get_error_rp() * 100),
-        error_yaw       : (uint16_t)(get_error_yaw() * 100)
+        error_yaw       : (uint16_t)(get_error_yaw() * 100),
+        active          : AP::ahrs().get_active_AHRS_type(),
     };
     AP::logger().WriteBlock(&pkt, sizeof(pkt));
 }

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -1122,8 +1122,14 @@ bool AP_AHRS_NavEKF::get_mag_field_correction(Vector3f &vec) const
 bool AP_AHRS_NavEKF::get_vert_pos_rate(float &velocity) const
 {
     switch (active_EKF_type()) {
-    case EKFType::NONE:
+    case EKFType::NONE: {
+        Vector3f velned;
+        if (!AP_AHRS_DCM::get_velocity_NED(velned)) {
             return false;
+        }
+        velocity = velned.z;
+        return true;
+    }
 
 #if HAL_NAVEKF2_AVAILABLE
     case EKFType::TWO:
@@ -1199,7 +1205,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NED_origin(Vector3f &vec) const
 {
     switch (active_EKF_type()) {
     case EKFType::NONE:
-        return false;
+        return AP_AHRS_DCM::get_relative_position_NED_origin(vec);
 
 #if HAL_NAVEKF2_AVAILABLE
     case EKFType::TWO: {
@@ -1292,7 +1298,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NE_origin(Vector2f &posNE) const
 {
     switch (active_EKF_type()) {
     case EKFType::NONE:
-        return false;
+        return AP_AHRS_DCM::get_relative_position_NE_origin(posNE);
 
 #if HAL_NAVEKF2_AVAILABLE
     case EKFType::TWO: {
@@ -1362,7 +1368,7 @@ bool AP_AHRS_NavEKF::get_relative_position_D_origin(float &posD) const
 {
     switch (active_EKF_type()) {
     case EKFType::NONE:
-        return false;
+        return AP_AHRS_DCM::get_relative_position_D_origin(posD);
 
 #if HAL_NAVEKF2_AVAILABLE
     case EKFType::TWO: {
@@ -1528,17 +1534,15 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::active_EKF_type(void) const
     }
 
     /*
-      fixed wing and rover when in fly_forward mode will fall back to
+      fixed wing and rover will fall back to DCM if the EKF doesn't
-      DCM if the EKF doesn't have GPS. This is the safest option as
+      have GPS. This is the safest option as DCM is very robust. Note
-      DCM is very robust. Note that we also check the filter status
+      that we also check the filter status when fly_forward is false
-      when fly_forward is false and we are disarmed. This is to ensure
+      and we are disarmed. This is to ensure that the arming checks do
-      that the arming checks do wait for good GPS position on fixed
+      wait for good GPS position on fixed wing and rover
-      wing and rover
      */
     if (ret != EKFType::NONE &&
         (_vehicle_class == AHRS_VEHICLE_FIXED_WING ||
-         _vehicle_class == AHRS_VEHICLE_GROUND) &&
+         _vehicle_class == AHRS_VEHICLE_GROUND)) {
-        (_flags.fly_forward || !hal.util->get_soft_armed())) {
         bool should_use_gps = true;
         nav_filter_status filt_state;
 #if HAL_NAVEKF2_AVAILABLE
@@ -1889,8 +1893,11 @@ void AP_AHRS_NavEKF::writeExtNavVelData(const Vector3f &vel, float err, uint32_t
 // get speed limit
 void AP_AHRS_NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
 {
-    switch (ekf_type()) {
+    switch (active_EKF_type()) {
     case EKFType::NONE:
+        // lower gains in VTOL controllers when flying on DCM
+        ekfGndSpdLimit = 50.0;
+        ekfNavVelGainScaler = 0.5;
         break;
 
 #if HAL_NAVEKF2_AVAILABLE
@@ -1914,12 +1921,27 @@ void AP_AHRS_NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVel
 #endif
 #if HAL_EXTERNAL_AHRS_ENABLED
     case EKFType::EXTERNAL:
-        // no limits
+        // no limit on gains, large vel limit
+        ekfGndSpdLimit = 400;
+        ekfNavVelGainScaler = 1;
         break;
 #endif
     }
 }
 
+/*
+  get gain recuction factor for Z controllers
+ */
+float AP_AHRS_NavEKF::getEkfControlScaleZ(void) const
+{
+    if (active_EKF_type() == EKFType::NONE) {
+        // when flying on DCM lower gains by 4x to cope with the high
+        // lag
+        return 0.25;
+    }
+    return 1;
+}
+
 // get compass offset estimates
 // true if offsets are valid
 bool AP_AHRS_NavEKF::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
@@ -2088,7 +2110,7 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
 // returns the time of the last yaw angle reset or 0 if no reset has ever occurred
 uint32_t AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng)
 {
-    switch (ekf_type()) {
+    switch (active_EKF_type()) {
 
     case EKFType::NONE:
         return 0;
@@ -2119,7 +2141,7 @@ uint32_t AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng)
 // returns the time of the last reset or 0 if no reset has ever occurred
 uint32_t AP_AHRS_NavEKF::getLastPosNorthEastReset(Vector2f &pos)
 {
-    switch (ekf_type()) {
+    switch (active_EKF_type()) {
 
     case EKFType::NONE:
         return 0;
@@ -2150,7 +2172,7 @@ uint32_t AP_AHRS_NavEKF::getLastPosNorthEastReset(Vector2f &pos)
 // returns the time of the last reset or 0 if no reset has ever occurred
 uint32_t AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const
 {
-    switch (ekf_type()) {
+    switch (active_EKF_type()) {
 
     case EKFType::NONE:
         return 0;
@@ -2182,7 +2204,7 @@ uint32_t AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const
 // returns the time of the last reset or 0 if no reset has ever occurred
 uint32_t AP_AHRS_NavEKF::getLastPosDownReset(float &posDelta)
 {
-    switch (ekf_type()) {
+    switch (active_EKF_type()) {
     case EKFType::NONE:
         return 0;
 
@@ -2313,7 +2335,7 @@ bool AP_AHRS_NavEKF::get_origin(Location &ret) const
 {
     switch (ekf_type()) {
     case EKFType::NONE:
-        return false;
+        return AP_AHRS_DCM::get_origin_fallback(ret);
 
 #if HAL_NAVEKF2_AVAILABLE
     case EKFType::TWO:
@@ -2417,10 +2439,9 @@ void AP_AHRS_NavEKF::set_terrain_hgt_stable(bool stable)
 //  returns true on success (i.e. the EKF knows it's latest position), false on failure
 bool AP_AHRS_NavEKF::get_location(struct Location &loc) const
 {
-    switch (ekf_type()) {
+    switch (active_EKF_type()) {
     case EKFType::NONE:
-        // We are not using an EKF so no data
+        return get_position(loc);
-        return false;
 
 #if HAL_NAVEKF2_AVAILABLE
     case EKFType::TWO:

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -191,8 +191,9 @@ public:
     // Write velocity data from an external navigation system
     void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) override;
 
-    // get speed limit
+    // get speed limits and controller scaling
     void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
+    float getEkfControlScaleZ(void) const;
 
     // is the AHRS subsystem healthy?
     bool healthy() const override;