fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 03:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: detect if offboard control is lost and missing offboard states

Browse Source
This commit is contained in:
Julian Oes
2014-06-21 12:24:37 +02:00
parent 4e08457afe
commit 5330ffe458
2 changed files with 33 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+14
View File
@@ -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) {