diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-22 14:58:06 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-22 14:58:06 +0200 |
commit | 6616aa6f993c0dc767c7fe7b2e616202c79667d5 (patch) | |
tree | 6fffcd5b6a37336af283043722b79fff22f1e464 | |
parent | 826d5687be209bc5e42ed97b8a84493913123c2a (diff) | |
download | px4-firmware-6616aa6f993c0dc767c7fe7b2e616202c79667d5.tar.gz px4-firmware-6616aa6f993c0dc767c7fe7b2e616202c79667d5.tar.bz2 px4-firmware-6616aa6f993c0dc767c7fe7b2e616202c79667d5.zip |
Fixed in-air restart, now obeys the right order
-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); |