Fixed in-air restart, now obeys the right order

1 files changed, 12 insertions, 10 deletions

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