fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: add vehicle-status nav_state change timestamp

Browse Source
This commit is contained in:
Daniel Agar 2019-11-19 09:07:23 -05:00
parent 01c7a475ff
commit a2bd65460d
3 changed files with 7 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/vehicle_status.msg
View File

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

5
src/modules/commander/Commander.cpp
View File

@ -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;

2
src/modules/commander/state_machine_helper.cpp
View File

@ -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;