From a2bd65460dfc545a68d796e2be20081f1f5c3fb9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 19 Nov 2019 09:07:23 -0500 Subject: [PATCH] commander: add vehicle-status nav_state change timestamp --- msg/vehicle_status.msg | 1 + src/modules/commander/Commander.cpp | 5 +++++ src/modules/commander/state_machine_helper.cpp | 2 +- 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index b60f4969ab..240281a4ce 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -58,6 +58,7 @@ uint8 VEHICLE_TYPE_ROVER = 3 # Encodes the complete system state and is set by the commander app. uint8 nav_state # set navigation state machine to specified value +uint64 nav_state_timestamp # time when current nav_state activated uint8 arming_state # current arming state uint8 hil_state # current hil state bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 688ac8dd85..2af6617656 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -563,6 +563,7 @@ Commander::Commander() : status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT; internal_state.main_state = commander_state_s::MAIN_STATE_MANUAL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; + status.nav_state_timestamp = hrt_absolute_time(); status.arming_state = vehicle_status_s::ARMING_STATE_INIT; /* mark all signals lost as long as they haven't been found */ @@ -2366,6 +2367,10 @@ Commander::run() (offboard_loss_rc_actions_t)_param_com_obl_rc_act.get(), (position_nav_loss_actions_t)_param_com_posctl_navl.get()); + if (nav_state_changed) { + status.nav_state_timestamp = hrt_absolute_time(); + } + if (status.failsafe != failsafe_old) { status_changed = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 219085789b..17d8871c98 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -407,7 +407,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ const offboard_loss_rc_actions_t offb_loss_rc_act, const position_nav_loss_actions_t posctl_nav_loss_act) { - navigation_state_t nav_state_old = status->nav_state; + const navigation_state_t nav_state_old = status->nav_state; const bool data_link_loss_act_configured = data_link_loss_act > link_loss_actions_t::DISABLED; const bool rc_loss_act_configured = rc_loss_act > link_loss_actions_t::DISABLED;