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

commander force offboard control update when first entering mode

Browse Source 
- fixes #12241
This commit is contained in:
Daniel Agar
2019-06-12 20:25:05 -04:00
committed by GitHub
parent 048cca7bc4
commit 6816f2a4ce
1 changed files with 16 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+16 -2
View File
@@ -1486,8 +1486,19 @@ Commander::run()
sp_man_sub.update(&sp_man);
if (offboard_control_mode_sub.updated()) {
offboard_control_mode_s old = offboard_control_mode;
// if this is the first time entering OFFBOARD the subscription may not be active yet
bool force_offboard_update = false;
if (commander_state_s::MAIN_STATE_OFFBOARD) {
if (offboard_control_mode.timestamp == 0) {
offboard_control_mode_sub.forceInit();
force_offboard_update = true;
}
}
if (offboard_control_mode_sub.updated() || force_offboard_update) {
const offboard_control_mode_s old = offboard_control_mode;
offboard_control_mode_sub.copy(&offboard_control_mode);
if (old.ignore_thrust != offboard_control_mode.ignore_thrust ||
@@ -1499,13 +1510,16 @@ Commander::run()
old.ignore_velocity != offboard_control_mode.ignore_velocity ||
old.ignore_acceleration_force != offboard_control_mode.ignore_acceleration_force ||
old.ignore_alt_hold != offboard_control_mode.ignore_alt_hold) {
status_changed = true;
}
}
if (offboard_control_mode.timestamp != 0 &&
offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
if (status_flags.offboard_control_signal_lost) {
status_flags.offboard_control_signal_lost = false;
status_flags.offboard_control_loss_timeout = false;
status_changed = true;