From fedb9de6ef5b91b103784a2a2cf41f3ea696ffa3 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 2 Jun 2016 20:43:47 +0200 Subject: [PATCH] fixed offboard loss timing handling --- src/modules/commander/commander.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c010b7c6b2..7ea3aacf9e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1668,7 +1668,11 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status_flags.offboard_control_signal_lost) { status_flags.offboard_control_signal_lost = true; + status_changed = true; + } + /* check timer if offboard was there but now lost */ + if (!status_flags.offboard_control_loss_timeout && offboard_control_mode.timestamp != 0) { if (offboard_loss_timeout < FLT_EPSILON) { /* execute loss action immediately */ status_flags.offboard_control_loss_timeout = true; @@ -1676,9 +1680,12 @@ int commander_thread_main(int argc, char *argv[]) } else { /* wait for timeout if set */ status_flags.offboard_control_loss_timeout = offboard_control_mode.timestamp + - OFFBOARD_TIMEOUT + offboard_loss_timeout * 1e6f > hrt_absolute_time(); + OFFBOARD_TIMEOUT + offboard_loss_timeout * 1e6f < hrt_absolute_time(); + } + + if (status_flags.offboard_control_loss_timeout) { + status_changed = true; } - status_changed = true; } }