diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index fafe8c6e9b..1b23bcfab4 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -381,6 +381,10 @@ "5": { "name": "emergency_battery_level", "description": "emergency battery level" + }, + "6": { + "name": "low_remaining_flight_time", + "description": "low remaining flight time" } } }, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp index 460ed302ed..f13977199e 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp @@ -285,12 +285,16 @@ void BatteryChecks::rtlEstimateCheck(const Context &context, Report &reporter, f rtl_time_estimate_s rtl_time_estimate; // Compare estimate of RTL time to estimate of remaining flight time + // add hysterisis: if already in the condition, only get out of it if the remaining flight time is significantly higher again + const float hysteresis_factor = reporter.failsafeFlags().battery_low_remaining_time ? 1.1f : 1.0f; + reporter.failsafeFlags().battery_low_remaining_time = _rtl_time_estimate_sub.copy(&rtl_time_estimate) - && (hrt_absolute_time() - rtl_time_estimate.timestamp) < 2_s + && (hrt_absolute_time() - rtl_time_estimate.timestamp) < 3_s && rtl_time_estimate.valid && context.isArmed() && PX4_ISFINITE(worst_battery_time_s) - && rtl_time_estimate.safe_time_estimate >= worst_battery_time_s; + && rtl_time_estimate.safe_time_estimate * hysteresis_factor >= worst_battery_time_s; + if (reporter.failsafeFlags().battery_low_remaining_time) { /* EVENT diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index e01b45276d..8f2c3d51ce 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -1030,3 +1030,17 @@ PARAM_DEFINE_INT32(COM_THROW_EN, 0); * @unit m/s */ PARAM_DEFINE_FLOAT(COM_THROW_SPEED, 5); + +/** + * Remaining flight time low failsafe + * + * Action the system takes when the remaining flight time is below + * the estimated time it takes to reach the RTL destination. + * + * @group Commander + * @value 0 None + * @value 1 Warning + * @value 3 Return + * @increment 1 + */ +PARAM_DEFINE_INT32(COM_FLTT_LOW_ACT, 3); diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 61cbff2beb..cbdb37ab8e 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -365,6 +365,36 @@ FailsafeBase::ActionOptions Failsafe::fromHighWindLimitActParam(int param_value) return options; } +FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int param_value) +{ + ActionOptions options{}; + + options.allow_user_takeover = UserTakeoverAllowed::Always; + options.cause = Cause::RemainingFlightTimeLow; + + switch (command_after_remaining_flight_time_low(param_value)) { + case command_after_remaining_flight_time_low::None: + options.action = Action::None; + break; + + case command_after_remaining_flight_time_low::Warning: + options.action = Action::Warn; + break; + + case command_after_remaining_flight_time_low::Return_mode: + options.action = Action::RTL; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + + default: + options.action = Action::None; + break; + + } + + return options; +} + void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, const failsafe_flags_s &status_flags) { @@ -444,9 +474,10 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, CHECK_FAILSAFE(status_flags, geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred()); - // Battery + // Battery flight time remaining failsafe + CHECK_FAILSAFE(status_flags, battery_low_remaining_time, - ActionOptions(Action::RTL).causedBy(Cause::BatteryLow).clearOn(ClearCondition::OnModeChangeOrDisarm)); + ActionOptions(fromRemainingFlightTimeLowActParam(_param_com_fltt_low_act.get()))); if ((_armed_time != 0) && (time_us < _armed_time + static_cast(_param_com_spoolup_time.get() * 1_s)) @@ -457,6 +488,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, CHECK_FAILSAFE(status_flags, battery_unhealthy, Action::Warn); } + // Battery low failsafe switch (status_flags.battery_warning) { case battery_status_s::BATTERY_WARNING_LOW: _last_state_battery_warning_low = checkFailsafe(_caller_id_battery_warning_low, _last_state_battery_warning_low, diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index 26981eb754..bb81dd02ba 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -141,6 +141,12 @@ private: Land_mode = 5 }; + enum class command_after_remaining_flight_time_low : int32_t { + None = 0, + Warning = 1, + Return_mode = 3 + }; + static ActionOptions fromNavDllOrRclActParam(int param_value); static ActionOptions fromGfActParam(int param_value); @@ -150,6 +156,7 @@ private: static ActionOptions fromQuadchuteActParam(int param_value); static Action fromOffboardLossActParam(int param_value, uint8_t &user_intended_mode); static ActionOptions fromHighWindLimitActParam(int param_value); + static ActionOptions fromRemainingFlightTimeLowActParam(int param_value); const int _caller_id_mode_fallback{genCallerId()}; bool _last_state_mode_fallback{false}; @@ -182,7 +189,8 @@ private: (ParamInt) _param_com_low_bat_act, (ParamInt) _param_com_obl_rc_act, (ParamInt) _param_com_qc_act, - (ParamInt) _param_com_wind_max_act + (ParamInt) _param_com_wind_max_act, + (ParamInt) _param_com_fltt_low_act ); }; diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index 88123548a7..4c85ee5c72 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -250,6 +250,11 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action events::send(events::ID("commander_failsafe_enter_crit_low_bat_warn"), {events::Log::Emergency, events::LogInternal::Info}, "Emergency battery level, land immediately"); + } else if (cause == Cause::RemainingFlightTimeLow) { + events::send(events::ID("commander_failsafe_enter_low_flight_time_warn"), + {events::Log::Warning, events::LogInternal::Info}, + "Low remaining flight time, return advised"); + } else { /* EVENT * @description No action is triggered. diff --git a/src/modules/commander/failsafe/framework.h b/src/modules/commander/failsafe/framework.h index 2d7b36ee7a..07d46cbabf 100644 --- a/src/modules/commander/failsafe/framework.h +++ b/src/modules/commander/failsafe/framework.h @@ -83,6 +83,7 @@ public: BatteryLow, BatteryCritical, BatteryEmergency, + RemainingFlightTimeLow, Count }; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index a81896cb38..bfa5d486be 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -208,32 +208,37 @@ void RTL::on_inactive() if ((now - _destination_check_time) > 2_s) { _destination_check_time = now; setRtlTypeAndDestination(); - - const bool global_position_recently_updated = _global_pos_sub.get().timestamp > 0 - && hrt_elapsed_time(&_global_pos_sub.get().timestamp) < 10_s; - - rtl_time_estimate_s estimated_time{}; - estimated_time.valid = false; - - if (_navigator->home_global_position_valid() && global_position_recently_updated) { - switch (_rtl_type) { - case RtlType::RTL_DIRECT: - estimated_time = _rtl_direct.calc_rtl_time_estimate(); - break; - - case RtlType::RTL_DIRECT_MISSION_LAND: - case RtlType::RTL_MISSION_FAST: - case RtlType::RTL_MISSION_FAST_REVERSE: - estimated_time = _rtl_mission_type_handle->calc_rtl_time_estimate(); - break; - - default: - break; - } - } - - _rtl_time_estimate_pub.publish(estimated_time); + publishRemainingTimeEstimate(); } + +} + +void RTL::publishRemainingTimeEstimate() +{ + const bool global_position_recently_updated = _global_pos_sub.get().timestamp > 0 + && hrt_elapsed_time(&_global_pos_sub.get().timestamp) < 10_s; + + rtl_time_estimate_s estimated_time{}; + estimated_time.valid = false; + + if (_navigator->home_global_position_valid() && global_position_recently_updated) { + switch (_rtl_type) { + case RtlType::RTL_DIRECT: + estimated_time = _rtl_direct.calc_rtl_time_estimate(); + break; + + case RtlType::RTL_DIRECT_MISSION_LAND: + case RtlType::RTL_MISSION_FAST: + case RtlType::RTL_MISSION_FAST_REVERSE: + estimated_time = _rtl_mission_type_handle->calc_rtl_time_estimate(); + break; + + default: + break; + } + } + + _rtl_time_estimate_pub.publish(estimated_time); } void RTL::on_activation() @@ -287,6 +292,14 @@ void RTL::on_active() default: break; } + + // Keep publishing remaining time estimates every 2 seconds + hrt_abstime now{hrt_absolute_time()}; + + if ((now - _destination_check_time) > 2_s) { + _destination_check_time = now; + publishRemainingTimeEstimate(); + } } void RTL::setRtlTypeAndDestination() diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index ab61aebe29..ac6a5a75d7 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -106,6 +106,12 @@ private: void setRtlTypeAndDestination(); + /** + * @brief Publish the remaining time estimate to go to the RTL landing point. + * + */ + void publishRemainingTimeEstimate(); + /** * @brief Find RTL destination. *