From b60d6348ed5246c508c847e7ab0a95d8ce8ac2c6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 11 May 2022 11:17:13 +0900 Subject: [PATCH] Copter: add dead reckon failsafe --- ArduCopter/Copter.cpp | 3 ++ ArduCopter/Copter.h | 11 ++++++- ArduCopter/Parameters.cpp | 14 +++++++++ ArduCopter/Parameters.h | 2 ++ ArduCopter/events.cpp | 60 +++++++++++++++++++++++++++++++++++++++ ArduCopter/system.cpp | 5 ++-- 6 files changed, 91 insertions(+), 4 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 656fea9d30..75936390c3 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -561,6 +561,9 @@ void Copter::three_hz_loop() // check if we've lost terrain data failsafe_terrain_check(); + // check for deadreckoning failsafe + failsafe_deadreckon_check(); + #if AC_FENCE == ENABLED // check if we have breached a fence fence_check(); diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index f1e9871a21..b0c88eb4e3 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -391,12 +391,20 @@ private: uint8_t ekf : 1; // true if ekf failsafe has occurred uint8_t terrain : 1; // true if the missing terrain data failsafe has occurred uint8_t adsb : 1; // true if an adsb related failsafe has occurred + uint8_t deadreckon : 1; // true if a dead reckoning failsafe has triggered } failsafe; bool any_failsafe_triggered() const { - return failsafe.radio || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb; + return failsafe.radio || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb || failsafe.deadreckon; } + // dead reckoning state + struct { + bool active; // true if dead reckoning (position estimate using estimated airspeed, no position or velocity source) + bool timeout; // true if dead reckoning has timedout and EKF's position and velocity estimate should no longer be trusted + uint32_t start_ms; // system time that EKF began deadreckoning + } dead_reckoning; + // Motor Output MOTOR_CLASS *motors; const struct AP_Param::GroupInfo *motors_var_info; @@ -734,6 +742,7 @@ private: void failsafe_terrain_set_status(bool data_ok); void failsafe_terrain_on_event(); void gpsglitch_check(); + void failsafe_deadreckon_check(); void set_mode_RTL_or_land_with_pause(ModeReason reason); void set_mode_SmartRTL_or_RTL(ModeReason reason); void set_mode_SmartRTL_or_land_with_pause(ModeReason reason); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 30bd08baad..08a95b6061 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1111,6 +1111,20 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced AP_GROUPINFO("SURFTRAK_MODE", 51, ParametersG2, surftrak_mode, (uint8_t)Copter::SurfaceTracking::Surface::GROUND), + // @Param: FS_DR_ENABLE + // @DisplayName: DeadReckon Failsafe Action + // @Description: Failsafe action taken immediately as deadreckoning starts. Deadreckoning starts when EKF loses position and velocity source and relies only on wind estimates + // @Values: 0:Disabled/NoAction,1:Land, 2:RTL, 3:SmartRTL or RTL, 4:SmartRTL or Land, 6:Auto DO_LAND_START or RTL + // @User: Standard + AP_GROUPINFO("FS_DR_ENABLE", 52, ParametersG2, failsafe_dr_enable, (uint8_t)Copter::FailsafeAction::RTL), + + // @Param: FS_DR_TIMEOUT + // @DisplayName: DeadReckon Failsafe Timeout + // @Description: DeadReckoning is available for this many seconds after losing position and/or velocity source. After this timeout elapses the EKF failsafe will trigger in modes requiring a position estimate + // @Range: 0 120 + // @User: Standard + AP_GROUPINFO("FS_DR_TIMEOUT", 53, ParametersG2, failsafe_dr_timeout, 30), + AP_GROUPEND }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index a8813e5dbe..ee5a99aef7 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -667,6 +667,8 @@ public: AP_Float pilot_y_rate; AP_Float pilot_y_expo; AP_Int8 surftrak_mode; + AP_Int8 failsafe_dr_enable; + AP_Int16 failsafe_dr_timeout; }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 89d9f1c849..6d9a6f72df 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -309,6 +309,66 @@ void Copter::gpsglitch_check() } } +// dead reckoning alert and failsafe +void Copter::failsafe_deadreckon_check() +{ + // update dead reckoning state + const char* dr_prefix_str = "Dead Reckoning"; + + // get EKF filter status + bool ekf_dead_reckoning = inertial_nav.get_filter_status().flags.dead_reckoning; + + // alert user to start or stop of dead reckoning + const uint32_t now_ms = AP_HAL::millis(); + if (dead_reckoning.active != ekf_dead_reckoning) { + dead_reckoning.active = ekf_dead_reckoning; + if (dead_reckoning.active) { + dead_reckoning.start_ms = now_ms; + gcs().send_text(MAV_SEVERITY_CRITICAL,"%s started", dr_prefix_str); + } else { + dead_reckoning.start_ms = 0; + dead_reckoning.timeout = false; + gcs().send_text(MAV_SEVERITY_CRITICAL,"%s stopped", dr_prefix_str); + } + } + + // check for timeout + if (dead_reckoning.active && !dead_reckoning.timeout) { + const uint32_t dr_timeout_ms = uint32_t(constrain_float(g2.failsafe_dr_timeout * 1000.0f, 0.0f, UINT32_MAX)); + if (now_ms - dead_reckoning.start_ms > dr_timeout_ms) { + dead_reckoning.timeout = true; + gcs().send_text(MAV_SEVERITY_CRITICAL,"%s timeout", dr_prefix_str); + } + } + + // exit immediately if deadreckon failsafe is disabled + if (g2.failsafe_dr_enable <= 0) { + failsafe.deadreckon = false; + return; + } + + // check for failsafe action + if (failsafe.deadreckon != ekf_dead_reckoning) { + failsafe.deadreckon = ekf_dead_reckoning; + + // only take action in modes requiring position estimate + if (failsafe.deadreckon && copter.flightmode->requires_GPS()) { + + // log error + AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_DEADRECKON, LogErrorCode::FAILSAFE_OCCURRED); + + // immediately disarm while landed + if (should_disarm_on_failsafe()) { + arming.disarm(AP_Arming::Method::DEADRECKON_FAILSAFE); + return; + } + + // take user specified action + do_failsafe_action((FailsafeAction)g2.failsafe_dr_enable.get(), ModeReason::DEADRECKON_FAILSAFE); + } + } +} + // set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts // this is always called from a failsafe so we trigger notification to pilot void Copter::set_mode_RTL_or_land_with_pause(ModeReason reason) diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 97467dd253..6ff9bb1f6e 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -273,7 +273,7 @@ bool Copter::ekf_has_relative_position() const return false; } - // return immediately if neither optflow nor visual odometry nor wind estimatation is enabled + // return immediately if neither optflow nor visual odometry is enabled and dead reckoning is inactive bool enabled = false; #if AP_OPTICALFLOW_ENABLED if (optflow.enabled()) { @@ -285,8 +285,7 @@ bool Copter::ekf_has_relative_position() const enabled = true; } #endif - Vector3f airspeed_vec_bf; - if (AP::ahrs().airspeed_vector_true(airspeed_vec_bf)) { + if (dead_reckoning.active && !dead_reckoning.timeout) { enabled = true; } if (!enabled) {