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: 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/commander/commander.cpp
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) {

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

@ -541,6 +541,25 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
}
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:
break;
}