forked from Archive/PX4-Autopilot
Commander: make low remaining flight time configurable and fix clearing condition (#22863)
* Commander: make low remaining flight time configurable and do not clear - add _ACT param to disable/warning/RTL this feature - publish rtl flight time estimate also in RTL, and thus fix re-validation - make failure message clearer, distinguish from battery low * battery check: add hysteresis for declaring battery_low_remaining_time false again --------- Signed-off-by: Silvan Fuhrer <silvan@auterion.com> Co-authored-by: KonradRudin <98741601+KonradRudin@users.noreply.github.com>
This commit is contained in:
parent
7fe5ee64fe
commit
00cc68baa1
|
@ -381,6 +381,10 @@
|
||||||
"5": {
|
"5": {
|
||||||
"name": "emergency_battery_level",
|
"name": "emergency_battery_level",
|
||||||
"description": "emergency battery level"
|
"description": "emergency battery level"
|
||||||
|
},
|
||||||
|
"6": {
|
||||||
|
"name": "low_remaining_flight_time",
|
||||||
|
"description": "low remaining flight time"
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
},
|
},
|
||||||
|
|
|
@ -285,12 +285,16 @@ void BatteryChecks::rtlEstimateCheck(const Context &context, Report &reporter, f
|
||||||
rtl_time_estimate_s rtl_time_estimate;
|
rtl_time_estimate_s rtl_time_estimate;
|
||||||
|
|
||||||
// Compare estimate of RTL time to estimate of remaining flight time
|
// 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)
|
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
|
&& rtl_time_estimate.valid
|
||||||
&& context.isArmed()
|
&& context.isArmed()
|
||||||
&& PX4_ISFINITE(worst_battery_time_s)
|
&& 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) {
|
if (reporter.failsafeFlags().battery_low_remaining_time) {
|
||||||
/* EVENT
|
/* EVENT
|
||||||
|
|
|
@ -1030,3 +1030,17 @@ PARAM_DEFINE_INT32(COM_THROW_EN, 0);
|
||||||
* @unit m/s
|
* @unit m/s
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_FLOAT(COM_THROW_SPEED, 5);
|
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);
|
||||||
|
|
|
@ -365,6 +365,36 @@ FailsafeBase::ActionOptions Failsafe::fromHighWindLimitActParam(int param_value)
|
||||||
return options;
|
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,
|
void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
|
||||||
const failsafe_flags_s &status_flags)
|
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());
|
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,
|
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)
|
if ((_armed_time != 0)
|
||||||
&& (time_us < _armed_time + static_cast<hrt_abstime>(_param_com_spoolup_time.get() * 1_s))
|
&& (time_us < _armed_time + static_cast<hrt_abstime>(_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);
|
CHECK_FAILSAFE(status_flags, battery_unhealthy, Action::Warn);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Battery low failsafe
|
||||||
switch (status_flags.battery_warning) {
|
switch (status_flags.battery_warning) {
|
||||||
case battery_status_s::BATTERY_WARNING_LOW:
|
case battery_status_s::BATTERY_WARNING_LOW:
|
||||||
_last_state_battery_warning_low = checkFailsafe(_caller_id_battery_warning_low, _last_state_battery_warning_low,
|
_last_state_battery_warning_low = checkFailsafe(_caller_id_battery_warning_low, _last_state_battery_warning_low,
|
||||||
|
|
|
@ -141,6 +141,12 @@ private:
|
||||||
Land_mode = 5
|
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 fromNavDllOrRclActParam(int param_value);
|
||||||
|
|
||||||
static ActionOptions fromGfActParam(int param_value);
|
static ActionOptions fromGfActParam(int param_value);
|
||||||
|
@ -150,6 +156,7 @@ private:
|
||||||
static ActionOptions fromQuadchuteActParam(int param_value);
|
static ActionOptions fromQuadchuteActParam(int param_value);
|
||||||
static Action fromOffboardLossActParam(int param_value, uint8_t &user_intended_mode);
|
static Action fromOffboardLossActParam(int param_value, uint8_t &user_intended_mode);
|
||||||
static ActionOptions fromHighWindLimitActParam(int param_value);
|
static ActionOptions fromHighWindLimitActParam(int param_value);
|
||||||
|
static ActionOptions fromRemainingFlightTimeLowActParam(int param_value);
|
||||||
|
|
||||||
const int _caller_id_mode_fallback{genCallerId()};
|
const int _caller_id_mode_fallback{genCallerId()};
|
||||||
bool _last_state_mode_fallback{false};
|
bool _last_state_mode_fallback{false};
|
||||||
|
@ -182,7 +189,8 @@ private:
|
||||||
(ParamInt<px4::params::COM_LOW_BAT_ACT>) _param_com_low_bat_act,
|
(ParamInt<px4::params::COM_LOW_BAT_ACT>) _param_com_low_bat_act,
|
||||||
(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
|
(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
|
||||||
(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act,
|
(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act,
|
||||||
(ParamInt<px4::params::COM_WIND_MAX_ACT>) _param_com_wind_max_act
|
(ParamInt<px4::params::COM_WIND_MAX_ACT>) _param_com_wind_max_act,
|
||||||
|
(ParamInt<px4::params::COM_FLTT_LOW_ACT>) _param_com_fltt_low_act
|
||||||
);
|
);
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
|
@ -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},
|
events::send(events::ID("commander_failsafe_enter_crit_low_bat_warn"), {events::Log::Emergency, events::LogInternal::Info},
|
||||||
"Emergency battery level, land immediately");
|
"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 {
|
} else {
|
||||||
/* EVENT
|
/* EVENT
|
||||||
* @description No action is triggered.
|
* @description No action is triggered.
|
||||||
|
|
|
@ -83,6 +83,7 @@ public:
|
||||||
BatteryLow,
|
BatteryLow,
|
||||||
BatteryCritical,
|
BatteryCritical,
|
||||||
BatteryEmergency,
|
BatteryEmergency,
|
||||||
|
RemainingFlightTimeLow,
|
||||||
|
|
||||||
Count
|
Count
|
||||||
};
|
};
|
||||||
|
|
|
@ -208,32 +208,37 @@ void RTL::on_inactive()
|
||||||
if ((now - _destination_check_time) > 2_s) {
|
if ((now - _destination_check_time) > 2_s) {
|
||||||
_destination_check_time = now;
|
_destination_check_time = now;
|
||||||
setRtlTypeAndDestination();
|
setRtlTypeAndDestination();
|
||||||
|
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::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()
|
void RTL::on_activation()
|
||||||
|
@ -287,6 +292,14 @@ void RTL::on_active()
|
||||||
default:
|
default:
|
||||||
break;
|
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()
|
void RTL::setRtlTypeAndDestination()
|
||||||
|
|
|
@ -106,6 +106,12 @@ private:
|
||||||
|
|
||||||
void setRtlTypeAndDestination();
|
void setRtlTypeAndDestination();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Publish the remaining time estimate to go to the RTL landing point.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
void publishRemainingTimeEstimate();
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Find RTL destination.
|
* @brief Find RTL destination.
|
||||||
*
|
*
|
||||||
|
|
Loading…
Reference in New Issue