commander: add vehicle-status nav_state change timestamp

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, ...)

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;
 

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;