dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Fixed in-air restart, now obeys the right order

This commit is contained in:
Lorenz Meier 2013-09-22 14:58:06 +02:00
parent 826d5687be
commit 6616aa6f99
2 changed files with 15 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/drivers/px4io/px4io.cpp
View File

@ -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)");

22
src/modules/commander/commander.cpp
View File

@ -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);