diff options
| author | Julian Oes <julian@oes.ch> | 2014-06-21 12:24:37 +0200 |
|---|---|---|
| committer | Julian Oes <julian@oes.ch> | 2014-06-27 12:53:07 +0200 |
| commit | 5330ffe458bd32bcf0e733c93fa2e659fbaaeeb3 (patch) | |
| tree | e3ca46cff146fc0f8a3fa9b1f1993735faa6d4c3 /src/modules/commander/commander.cpp | |
| parent | 4e08457afec89e41bb14fb09293c885f988bfd0c (diff) | |
| download | px4-firmware-5330ffe458bd32bcf0e733c93fa2e659fbaaeeb3.tar.gz px4-firmware-5330ffe458bd32bcf0e733c93fa2e659fbaaeeb3.tar.bz2 px4-firmware-5330ffe458bd32bcf0e733c93fa2e659fbaaeeb3.zip | |
commander: detect if offboard control is lost and missing offboard states
Diffstat (limited to 'src/modules/commander/commander.cpp')
| -rw-r--r-- | src/modules/commander/commander.cpp | 14 |
1 files changed, 14 insertions, 0 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fab80e325..0c9ce4418 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -661,6 +661,7 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; + nav_states_str[NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -932,6 +933,19 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } + if (sp_offboard.timestamp != 0 && + sp_offboard.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) { + if (status.offboard_control_signal_lost) { + status.offboard_control_signal_lost = false; + status_changed = true; + } + } else { + if (!status.offboard_control_signal_lost) { + status.offboard_control_signal_lost = true; + status_changed = true; + } + } + orb_check(telemetry_sub, &updated); if (updated) { |