fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 03:13:34 +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-27 12:53:07 +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_LAND] = "LAND";
nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND";
nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION";
nav_states_str[NAVIGATION_STATE_OFFBOARD] = "OFFBOARD";
/* pthread for slow low prio thread */ /* pthread for slow low prio thread */
pthread_t commander_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); 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); orb_check(telemetry_sub, &updated);
if (updated) { if (updated) {
src/modules/commander/state_machine_helper.cpp
+19
View File
@@ -541,6 +541,25 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
} }
break; break;
case MAIN_STATE_OFFBOARD:
/* require offboard control, otherwise stay where you are */
if (status->offboard_control_signal_lost && !status->rc_signal_lost) {
status->failsafe = true;
status->nav_state = NAVIGATION_STATE_POSCTL;
} else if (status->offboard_control_signal_lost && status->rc_signal_lost) {
status->failsafe = true;
if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
} else {
status->nav_state = NAVIGATION_STATE_OFFBOARD;
}
default: default:
break; break;
} }