fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 17:33:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

state_machine_helper: fix infinite delay with intermittent failsafes

Browse Source
This commit is contained in:
Matthias Grob
2021-02-04 10:54:52 +01:00
parent 1736be41cf
commit e2e5fc85f8
2 changed files with 8 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander_params.c
+1 -1
View File
@@ -979,7 +979,7 @@ PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1);
* @group Commander * @group Commander
* @unit s * @unit s
* @min -1.0 * @min -1.0
* @max 60.0 * @max 25.0
* @decimal 3 * @decimal 3
*/ */
PARAM_DEFINE_FLOAT(COM_LL_DELAY, 15.0f); PARAM_DEFINE_FLOAT(COM_LL_DELAY, 15.0f);
src/modules/commander/state_machine_helper.cpp
+7 -2
View File
@@ -389,7 +389,11 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
void enable_failsafe(vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason) void enable_failsafe(vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason)
{ {
if (!old_failsafe && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { if (!old_failsafe && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
status->failsafe_timestamp = hrt_absolute_time(); // make sure intermittent failsafes don't lead to infinite delay by not constatnly reseting the timestamp
if (hrt_elapsed_time(&status->failsafe_timestamp) > 30_s) {
status->failsafe_timestamp = hrt_absolute_time();
}
mavlink_log_critical(mavlink_log_pub, "Failsafe enabled: %s", reason); mavlink_log_critical(mavlink_log_pub, "Failsafe enabled: %s", reason);
} }
@@ -774,7 +778,8 @@ void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, link_loss_actions_t link_loss_act, const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, link_loss_actions_t link_loss_act,
const float ll_delay) const float ll_delay)
{ {
if (hrt_elapsed_time(&status->failsafe_timestamp) < (ll_delay * 1_s)) { if (hrt_elapsed_time(&status->failsafe_timestamp) < (ll_delay * 1_s)
&& link_loss_act != link_loss_actions_t::DISABLED) {
// delay failsafe reaction by trying to hold position if configured // delay failsafe reaction by trying to hold position if configured
link_loss_act = link_loss_actions_t::AUTO_LOITER; link_loss_act = link_loss_actions_t::AUTO_LOITER;
} }