diff options
| -rw-r--r-- | src/drivers/px4io/px4io.cpp | 3 | ||||
| -rw-r--r-- | src/modules/commander/commander.cpp | 22 |
2 files changed, 15 insertions, 10 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 9597dad9a..952453a8c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -591,6 +591,9 @@ PX4IO::init() if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + /* get a status update from IO */ + io_get_status(); + mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); log("INAIR RESTART RECOVERY (needs commander app running)"); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fd9067e90..01b7b84d0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -824,16 +824,6 @@ int commander_thread_main(int argc, char *argv[]) check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed); - orb_check(cmd_sub, &updated); - - if (updated) { - /* got command */ - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - - /* handle it */ - handle_command(&status, &safety, &control_mode, &cmd, &armed); - } - /* update safety topic */ orb_check(safety_sub, &updated); @@ -1165,6 +1155,18 @@ int commander_thread_main(int argc, char *argv[]) } } + + /* handle commands last, as the system needs to be updated to handle them */ + orb_check(cmd_sub, &updated); + + if (updated) { + /* got command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* handle it */ + handle_command(&status, &safety, &control_mode, &cmd, &armed); + } + /* evaluate the navigation state machine */ transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position); |