From 53e7436525bc6250c136f7559712c6058b264e41 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 14 Aug 2021 13:03:42 +1000 Subject: [PATCH] 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.cpp | 73 +++++++++++++++++---------- libraries/AP_AHRS/AP_AHRS.h | 5 +- libraries/AP_AHRS/AP_AHRS_Backend.h | 3 -- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 62 +++++++++++++++++++++-- libraries/AP_AHRS/AP_AHRS_DCM.h | 11 +++- libraries/AP_AHRS/AP_AHRS_Logging.cpp | 5 +- 6 files changed, 121 insertions(+), 38 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 6d8e4f8148..0c48a52079 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -1332,8 +1332,14 @@ bool AP_AHRS::get_mag_field_correction(Vector3f &vec) const bool AP_AHRS::get_vert_pos_rate(float &velocity) const { switch (active_EKF_type()) { - case EKFType::NONE: - return false; + 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: @@ -1409,7 +1415,7 @@ bool AP_AHRS::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: { @@ -1502,7 +1508,7 @@ bool AP_AHRS::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: { @@ -1572,7 +1578,7 @@ bool AP_AHRS::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: { @@ -1738,17 +1744,15 @@ AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const } /* - fixed wing and rover when in fly_forward mode will fall back to - DCM if the EKF doesn't have GPS. This is the safest option as - DCM is very robust. Note that we also check the filter status - when fly_forward is false and we are disarmed. This is to ensure - that the arming checks do wait for good GPS position on fixed - wing and rover + fixed wing and rover will fall back to DCM if the EKF doesn't + have GPS. This is the safest option as DCM is very robust. Note + that we also check the filter status when fly_forward is false + and we are disarmed. This is to ensure that the arming checks do + wait for good GPS position on fixed wing and rover */ if (ret != EKFType::NONE && (_vehicle_class == VehicleClass::FIXED_WING || - _vehicle_class == VehicleClass::GROUND) && - (fly_forward || !hal.util->get_soft_armed())) { + _vehicle_class == VehicleClass::GROUND)) { bool should_use_gps = true; nav_filter_status filt_state; #if HAL_NAVEKF2_AVAILABLE @@ -2096,11 +2100,14 @@ void AP_AHRS::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeSt #endif } -// get speed limit -void AP_AHRS::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const +// get speed limit and XY navigation gain scale factor +void AP_AHRS::getControlLimits(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 @@ -2124,12 +2131,27 @@ void AP_AHRS::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainSca #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 factor for Z controllers + */ +float AP_AHRS::getControlScaleZ(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::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const @@ -2298,7 +2320,7 @@ bool AP_AHRS::attitudes_consistent(char *failure_msg, const uint8_t failure_msg_ // returns the time of the last yaw angle reset or 0 if no reset has ever occurred uint32_t AP_AHRS::getLastYawResetAngle(float &yawAng) { - switch (ekf_type()) { + switch (active_EKF_type()) { case EKFType::NONE: return 0; @@ -2329,7 +2351,7 @@ uint32_t AP_AHRS::getLastYawResetAngle(float &yawAng) // returns the time of the last reset or 0 if no reset has ever occurred uint32_t AP_AHRS::getLastPosNorthEastReset(Vector2f &pos) { - switch (ekf_type()) { + switch (active_EKF_type()) { case EKFType::NONE: return 0; @@ -2360,7 +2382,7 @@ uint32_t AP_AHRS::getLastPosNorthEastReset(Vector2f &pos) // returns the time of the last reset or 0 if no reset has ever occurred uint32_t AP_AHRS::getLastVelNorthEastReset(Vector2f &vel) const { - switch (ekf_type()) { + switch (active_EKF_type()) { case EKFType::NONE: return 0; @@ -2392,7 +2414,7 @@ uint32_t AP_AHRS::getLastVelNorthEastReset(Vector2f &vel) const // returns the time of the last reset or 0 if no reset has ever occurred uint32_t AP_AHRS::getLastPosDownReset(float &posDelta) { - switch (ekf_type()) { + switch (active_EKF_type()) { case EKFType::NONE: return 0; @@ -2523,7 +2545,7 @@ bool AP_AHRS::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: @@ -2624,13 +2646,12 @@ void AP_AHRS::set_terrain_hgt_stable(bool stable) } // get_location - updates the provided location with the latest calculated location -// returns true on success (i.e. the EKF knows it's latest position), false on failure +// returns true on success (i.e. the backend knows it's latest position), false on failure bool AP_AHRS::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 false; + return get_position(loc); #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index a59371b18d..6d6613bf20 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -203,7 +203,8 @@ public: void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) override; // get speed limit - void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const; + void getControlLimits(float &ekfGndSpdLimit, float &controlScaleXY) const; + float getControlScaleZ(void) const; // is the AHRS subsystem healthy? bool healthy() const override; @@ -330,7 +331,7 @@ public: void set_alt_measurement_noise(float noise) override; // active EKF type for logging - uint8_t get_active_AHRS_type(void) const override { + uint8_t get_active_AHRS_type(void) const { return uint8_t(active_EKF_type()); } diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index aeb6a7ffa5..9b74bde96a 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -429,9 +429,6 @@ public: return _rsem; } - // active AHRS type for logging - virtual uint8_t get_active_AHRS_type(void) const { return 0; } - // Logging to disk functions void Write_AHRS2(void) const; void Write_Attitude(const Vector3f &targets) const; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 8b81f3e66d..f51d31e316 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -108,6 +108,9 @@ AP_AHRS_DCM::update(bool skip_ins_update) update_trig(); backup_attitude(); + + // remember the last origin for fallback support + IGNORE_RETURN(AP::ahrs().get_origin(last_origin)); } /* @@ -744,6 +747,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; @@ -1047,10 +1051,13 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const loc.relative_alt = 0; loc.terrain_alt = 0; loc.offset(_position_offset_north, _position_offset_east); - if (AP::ahrs().get_fly_forward() && _have_position) { - float gps_delay_sec = 0; - gps.get_lag(gps_delay_sec); - loc.offset_bearing(gps.ground_course(), gps.ground_speed() * gps_delay_sec); + if (_have_position) { + const uint32_t now = AP_HAL::millis(); + float dt = 0; + 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; } @@ -1177,3 +1184,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; +} diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 042a839c7a..7d46f6858c 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -35,7 +35,6 @@ public: AP_AHRS_DCM(const AP_AHRS_DCM &other) = delete; AP_AHRS_DCM &operator=(const AP_AHRS_DCM&) = delete; - // return the smoothed gyro vector corrected for drift const Vector3f &get_gyro() const override { return _omega; @@ -118,6 +117,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: // these are experimentally derived from the simulator @@ -198,6 +203,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; @@ -223,4 +229,7 @@ private: // time when DCM was last reset uint32_t _last_startup_ms; + + // last origin we returned, for DCM fallback from EKF + Location last_origin; }; diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index 92ac94a279..31dd13ba1f 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -58,7 +58,7 @@ void AP_AHRS_Backend::Write_Attitude(const Vector3f &targets) const yaw : (uint16_t)wrap_360_cd(yaw_sensor), error_rp : (uint16_t)(get_error_rp() * 100), error_yaw : (uint16_t)(get_error_yaw() * 100), - active : get_active_AHRS_type(), + active : AP::ahrs().get_active_AHRS_type(), }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } @@ -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)); }