From dec13e7f215caa3ee002fbc3b6400ae82e249b2e Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Sun, 16 Mar 2014 21:38:23 -0700
Subject: Use single set of arm/disarm code

Set mode and arm/disarm commands now call a single method to arm
disarm, thus removing code duplication. Also calls updated
arming_state_transition method such that the arming state logic does
not need to be duplicated outside of arming_state_transition.
---
 src/modules/commander/commander.cpp | 115 +++++++++++-------------------------
 1 file changed, 34 insertions(+), 81 deletions(-)

(limited to 'src/modules/commander')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index d114a2e5c..e010c233f 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -137,7 +137,7 @@ enum MAV_MODE_FLAG {
 };
 
 /* Mavlink file descriptors */
-static int mavlink_fd;
+static int mavlink_fd = 0;
 
 /* flags */
 static bool commander_initialized = false;
@@ -217,11 +217,10 @@ void print_reject_arm(const char *msg);
 
 void print_status();
 
-int arm();
-int disarm();
-
 transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
 
+transition_result_t armDisarm(bool arm, const int mavlink_fd, const char* armedBy);
+
 /**
  * Loop that runs at a lower rate and priority for calibration and parameter tasks.
  */
@@ -288,12 +287,12 @@ int commander_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "arm")) {
-		arm();
+		armDisarm(true, mavlink_fd, "command line");
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "disarm")) {
-		disarm();
+	if (!strcmp(argv[1], "2")) {
+		armDisarm(false, mavlink_fd, "command line");
 		exit(0);
 	}
 
@@ -363,30 +362,22 @@ void print_status()
 
 static orb_advert_t status_pub;
 
-int arm()
+transition_result_t armDisarm(bool arm, const int mavlink_fd, const char* armedBy)
 {
-	int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
-
-	if (arming_res == TRANSITION_CHANGED) {
-		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
-		return 0;
-
-	} else {
-		return 1;
-	}
-}
-
-int disarm()
-{
-	int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
-
-	if (arming_res == TRANSITION_CHANGED) {
-		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
-		return 0;
-
-	} else {
-		return 1;
-	}
+    transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+    
+    // Transition the armed state. By passing mavlink_fd to arming_state_transition it will
+    // output appropriate error messages if the state cannot transition.
+    arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
+    if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
+        char buf[256];
+        snprintf(buf, sizeof(buf), "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
+        mavlink_log_info(mavlink_fd, buf);
+    } else if (arming_res == TRANSITION_DENIED) {
+        tune_negative(true);
+    }
+    
+    return arming_res;
 }
 
 bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
@@ -424,37 +415,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			if (hil_ret == OK)
 				ret = true;
 
-			// TODO remove debug code
-			//mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode);
-			/* set arming state */
-			arming_res = TRANSITION_NOT_CHANGED;
-
-			if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
-				if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) {
-					print_reject_arm("NOT ARMING: Press safety switch first.");
-					arming_res = TRANSITION_DENIED;
-
-				} else {
-					arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
-				}
-
-				if (arming_res == TRANSITION_CHANGED) {
-					mavlink_log_info(mavlink_fd, "[cmd] ARMED by command");
-				}
-
-			} else {
-				if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
-					arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-					arming_res = arming_state_transition(status, safety, new_arming_state, armed);
-
-					if (arming_res == TRANSITION_CHANGED) {
-						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
-					}
-
-				} else {
-					arming_res = TRANSITION_NOT_CHANGED;
-				}
-			}
+            // Transition the arming state
+            arming_res = armDisarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
 
 			if (arming_res == TRANSITION_CHANGED)
 				ret = true;
@@ -513,26 +475,17 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
-			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
-
-			if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
-				if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) {
-					print_reject_arm("NOT ARMING: Press safety switch first.");
-					arming_res = TRANSITION_DENIED;
-
-				} else {
-					arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
-				}
-
-				if (arming_res == TRANSITION_CHANGED) {
-					mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd");
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-					ret = true;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
-					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-				}
+            // Follow exactly what the mavlink spec says for values
+            if (cmd->param1 != 0.0f && cmd->param1 != 1.0f) {
+                return VEHICLE_CMD_RESULT_UNSUPPORTED;
+            }
+        
+            transition_result_t arming_res = armDisarm(cmd->param1 == 1.0f, mavlink_fd, "arm/disarm component command");
+            if (arming_res == TRANSITION_DENIED) {
+                mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
+                result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+            } else {
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
 			}
 		}
 		break;
-- 
cgit v1.2.3


From 4b980b508c93f8bb840ababf19e61862dcedbda9 Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Sun, 16 Mar 2014 21:39:06 -0700
Subject: arming_state_transition now outputs error messages

If mavlink fd is passed in method will output reason for arming failure
to mavlink.
---
 src/modules/commander/state_machine_helper.cpp | 21 +++++++++++++--------
 src/modules/commander/state_machine_helper.h   |  2 +-
 2 files changed, 14 insertions(+), 9 deletions(-)

(limited to 'src/modules/commander')

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 31955d3e5..94ffc31d6 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -70,7 +70,7 @@ static bool failsafe_state_changed = true;
 
 transition_result_t
 arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
-			arming_state_t new_arming_state, struct actuator_armed_s *armed)
+			arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd)
 {
 	/*
 	 * Perform an atomic state update
@@ -125,13 +125,18 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 		case ARMING_STATE_ARMED:
 
 			/* allow arming from STANDBY and IN-AIR-RESTORE */
-			if ((status->arming_state == ARMING_STATE_STANDBY
-			     || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
-			    && (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */
-				ret = TRANSITION_CHANGED;
-				armed->armed = true;
-				armed->ready_to_arm = true;
-			}
+            if ((status->arming_state == ARMING_STATE_STANDBY || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)) {
+                if (status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
+                    // If we need to wait for safety switch then output message, but only if we have fd for mavlink connection
+                    if (mavlink_fd) {
+                        mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first.");
+                    }
+                } else {
+                    ret = TRANSITION_CHANGED;
+                    armed->armed = true;
+                    armed->ready_to_arm = true;
+                }
+            }
 
 			break;
 
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index f04879ff9..0ddd4f05a 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -57,7 +57,7 @@ typedef enum {
 } transition_result_t;
 
 transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
-		arming_state_t new_arming_state, struct actuator_armed_s *armed);
+		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0);
 
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
-- 
cgit v1.2.3


From 783a2403969032e8c0119196cb94c092847965d1 Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Mon, 17 Mar 2014 11:01:15 -0700
Subject: Function naming and remove unneeded sprintf

---
 src/modules/commander/commander.cpp | 16 +++++++---------
 1 file changed, 7 insertions(+), 9 deletions(-)

(limited to 'src/modules/commander')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index e010c233f..1c11e2587 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -219,7 +219,7 @@ void print_status();
 
 transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
 
-transition_result_t armDisarm(bool arm, const int mavlink_fd, const char* armedBy);
+transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy);
 
 /**
  * Loop that runs at a lower rate and priority for calibration and parameter tasks.
@@ -287,12 +287,12 @@ int commander_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "arm")) {
-		armDisarm(true, mavlink_fd, "command line");
+		arm_disarm(true, mavlink_fd, "command line");
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "2")) {
-		armDisarm(false, mavlink_fd, "command line");
+		arm_disarm(false, mavlink_fd, "command line");
 		exit(0);
 	}
 
@@ -362,7 +362,7 @@ void print_status()
 
 static orb_advert_t status_pub;
 
-transition_result_t armDisarm(bool arm, const int mavlink_fd, const char* armedBy)
+transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy)
 {
     transition_result_t arming_res = TRANSITION_NOT_CHANGED;
     
@@ -370,9 +370,7 @@ transition_result_t armDisarm(bool arm, const int mavlink_fd, const char* armedB
     // output appropriate error messages if the state cannot transition.
     arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
     if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
-        char buf[256];
-        snprintf(buf, sizeof(buf), "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
-        mavlink_log_info(mavlink_fd, buf);
+        mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
     } else if (arming_res == TRANSITION_DENIED) {
         tune_negative(true);
     }
@@ -416,7 +414,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				ret = true;
 
             // Transition the arming state
-            arming_res = armDisarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
+            arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
 
 			if (arming_res == TRANSITION_CHANGED)
 				ret = true;
@@ -480,7 +478,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
                 return VEHICLE_CMD_RESULT_UNSUPPORTED;
             }
         
-            transition_result_t arming_res = armDisarm(cmd->param1 == 1.0f, mavlink_fd, "arm/disarm component command");
+            transition_result_t arming_res = arm_disarm(cmd->param1 == 1.0f, mavlink_fd, "arm/disarm component command");
             if (arming_res == TRANSITION_DENIED) {
                 mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
                 result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-- 
cgit v1.2.3


From 521539897e59f495adaae481911e3a4ac847625e Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Wed, 26 Mar 2014 14:51:57 -0700
Subject: Simpler state transition code

Also fixed ARMING_STATE_ARMED_ERROR->ARMING_STATE_STANDBY_ERROR
transition.
---
 src/modules/commander/state_machine_helper.cpp | 175 +++++++++++--------------
 src/modules/uORB/topics/vehicle_status.h       |   2 +
 2 files changed, 76 insertions(+), 101 deletions(-)

(limited to 'src/modules/commander')

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 94ffc31d6..8564747fa 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -68,10 +68,44 @@ static bool arming_state_changed = true;
 static bool main_state_changed = true;
 static bool failsafe_state_changed = true;
 
+// This array defines the arming state transitions. The rows are the new state, and the columns
+// are the current state. Using new state and current  state you can index into the array which
+// will be true for a valid transition or false for a invalid transition. In some cases even
+// though the transition is marked as true additional checks must be made. See arming_state_transition
+// code for those checks.
+static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
+    //                                  INIT,   STANDBY,    ARMED,  ARMED_ERROR,    STANDBY_ERROR,  REBOOT,     IN_AIR_RESTORE
+    { /* ARMING_STATE_INIT */           true,   true,       false,  false,          false,          false,      false },
+    { /* ARMING_STATE_STANDBY */        true,   true,       true,   true,           false,          false,      false },
+    { /* ARMING_STATE_ARMED */          false,  true,       true,   false,          false,          false,      true },
+    { /* ARMING_STATE_ARMED_ERROR */    false,  false,      true,   true,           false,          false,      false },
+    { /* ARMING_STATE_STANDBY_ERROR */  true,   true,       false,  true,           true,           false,      false },
+    { /* ARMING_STATE_REBOOT */         true,   true,       false,  false,          true,           true,       true },
+    { /* ARMING_STATE_IN_AIR_RESTORE */ false,  false,      false,  false,          false,          false,      false }, // NYI
+};
+
+// You can index into the array with an arming_state_t in order to get it's textual representation
+static const char* state_names[ARMING_STATE_MAX] = {
+    "ARMING_STATE_INIT",
+    "ARMING_STATE_STANDBY",
+    "ARMING_STATE_ARMED",
+    "ARMING_STATE_ARMED_ERROR",
+    "ARMING_STATE_STANDBY_ERROR",
+    "ARMING_STATE_REBOOT",
+    "ARMING_STATE_IN_AIR_RESTORE",
+};
+
 transition_result_t
-arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
-			arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd)
+arming_state_transition(struct vehicle_status_s *status,            /// current vehicle status
+                        const struct safety_s   *safety,            /// current safety settings
+                        arming_state_t          new_arming_state,   /// arming state requested
+                        struct actuator_armed_s *armed,             /// current armed status
+                        const int               mavlink_fd)         /// mavlink fd for error reporting, 0 for none
 {
+    // Double check that our static arrays are still valid
+    ASSERT(ARMING_STATE_INIT == 0);
+    ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
+    
 	/*
 	 * Perform an atomic state update
 	 */
@@ -82,121 +116,60 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 	/* only check transition if the new state is actually different from the current one */
 	if (new_arming_state == status->arming_state) {
 		ret = TRANSITION_NOT_CHANGED;
-
 	} else {
-
 		/* enforce lockdown in HIL */
 		if (status->hil_state == HIL_STATE_ON) {
 			armed->lockdown = true;
-
 		} else {
 			armed->lockdown = false;
 		}
-
-		switch (new_arming_state) {
-		case ARMING_STATE_INIT:
-
-			/* allow going back from INIT for calibration */
-			if (status->arming_state == ARMING_STATE_STANDBY) {
-				ret = TRANSITION_CHANGED;
-				armed->armed = false;
-				armed->ready_to_arm = false;
-			}
-
-			break;
-
-		case ARMING_STATE_STANDBY:
-
-			/* allow coming from INIT and disarming from ARMED */
-			if (status->arming_state == ARMING_STATE_INIT
-			    || status->arming_state == ARMING_STATE_ARMED
-			    || status->hil_state == HIL_STATE_ON) {
-
-				/* sensors need to be initialized for STANDBY state */
-				if (status->condition_system_sensors_initialized) {
-					ret = TRANSITION_CHANGED;
-					armed->armed = false;
-					armed->ready_to_arm = true;
-				}
-			}
-
-			break;
-
-		case ARMING_STATE_ARMED:
-
-			/* allow arming from STANDBY and IN-AIR-RESTORE */
-            if ((status->arming_state == ARMING_STATE_STANDBY || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)) {
+        
+        // Check that we have a valid state transition
+        bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
+        if (valid_transition) {
+            // We have a good transition. Now perform any secondary validation.
+            if (new_arming_state == ARMING_STATE_ARMED) {
+                // In non-HIL, fail transition if we need safety switch press
                 if (status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
-                    // If we need to wait for safety switch then output message, but only if we have fd for mavlink connection
                     if (mavlink_fd) {
                         mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first.");
                     }
-                } else {
-                    ret = TRANSITION_CHANGED;
-                    armed->armed = true;
-                    armed->ready_to_arm = true;
+                    valid_transition = false;
                 }
+            } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
+                new_arming_state = ARMING_STATE_STANDBY_ERROR;
             }
-
-			break;
-
-		case ARMING_STATE_ARMED_ERROR:
-
-			/* an armed error happens when ARMED obviously */
-			if (status->arming_state == ARMING_STATE_ARMED) {
-				ret = TRANSITION_CHANGED;
-				armed->armed = true;
-				armed->ready_to_arm = false;
-			}
-
-			break;
-
-		case ARMING_STATE_STANDBY_ERROR:
-
-			/* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */
-			if (status->arming_state == ARMING_STATE_STANDBY
-			    || status->arming_state == ARMING_STATE_INIT
-			    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
-				ret = TRANSITION_CHANGED;
-				armed->armed = false;
-				armed->ready_to_arm = false;
-			}
-
-			break;
-
-		case ARMING_STATE_REBOOT:
-
-			/* an armed error happens when ARMED obviously */
-			if (status->arming_state == ARMING_STATE_INIT
-			    || status->arming_state == ARMING_STATE_STANDBY
-			    || status->arming_state == ARMING_STATE_STANDBY_ERROR) {
-				ret = TRANSITION_CHANGED;
-				armed->armed = false;
-				armed->ready_to_arm = false;
-			}
-
-			break;
-
-		case ARMING_STATE_IN_AIR_RESTORE:
-
-			/* XXX implement */
-			break;
-
-		default:
-			break;
-		}
-
-		if (ret == TRANSITION_CHANGED) {
-			status->arming_state = new_arming_state;
-			arming_state_changed = true;
-		}
-	}
-
+        }
+        
+        // HIL can always go to standby
+        if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
+            valid_transition = true;
+        }
+        
+        /* Sensors need to be initialized for STANDBY state */
+        if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
+            valid_transition = false;
+        }
+
+        // Finish up the state transition
+        if (valid_transition) {
+            armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
+            armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
+            ret = TRANSITION_CHANGED;
+            status->arming_state = new_arming_state;
+            arming_state_changed = true;
+        }
+    }
+    
 	/* end of atomic state update */
 	irqrestore(flags);
 
-	if (ret == TRANSITION_DENIED)
-		warnx("arming transition rejected");
+    if (ret == TRANSITION_DENIED) {
+        if (mavlink_fd) {
+            mavlink_log_critical(mavlink_fd, "Invalid arming transition from %s to %s", state_names[status->arming_state], state_names[new_arming_state]);
+        }
+        warnx("arming transition rejected");
+    }
 
 	return ret;
 }
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 5d40554fd..0071d3125 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -69,6 +69,8 @@ typedef enum {
 	MAIN_STATE_MAX
 } main_state_t;
 
+// If you change the order, add or remove arming_state_t states make sure to update the arrays
+// in state_machine_helper.cpp as well.
 typedef enum {
 	ARMING_STATE_INIT = 0,
 	ARMING_STATE_STANDBY,
-- 
cgit v1.2.3


From b9a56fbeb9effad0b30e2085dba4ea92041456be Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Thu, 27 Mar 2014 13:05:51 -0700
Subject: Fixed bug with transition from in air restore to armed

Also added better debug output for invalid transitions
---
 src/modules/commander/state_machine_helper.cpp | 11 +++++++----
 1 file changed, 7 insertions(+), 4 deletions(-)

(limited to 'src/modules/commander')

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 8564747fa..359cc62cd 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -129,8 +129,10 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
         if (valid_transition) {
             // We have a good transition. Now perform any secondary validation.
             if (new_arming_state == ARMING_STATE_ARMED) {
-                // In non-HIL, fail transition if we need safety switch press
-                if (status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
+                // Fail transition if we need safety switch press
+                //      Allow if coming from in air restore
+                //      Allow if HIL_STATE_ON
+                if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
                     if (mavlink_fd) {
                         mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first.");
                     }
@@ -165,10 +167,11 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 	irqrestore(flags);
 
     if (ret == TRANSITION_DENIED) {
+        static const char* errMsg = "Invalid arming transition from %s to %s";
         if (mavlink_fd) {
-            mavlink_log_critical(mavlink_fd, "Invalid arming transition from %s to %s", state_names[status->arming_state], state_names[new_arming_state]);
+            mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
         }
-        warnx("arming transition rejected");
+        warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
     }
 
 	return ret;
-- 
cgit v1.2.3


From dcc11b8cabda82ff8616ee2d14db9a53d6d23ac0 Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Thu, 27 Mar 2014 13:06:09 -0700
Subject: Update to convert to C++ style

---
 src/modules/commander/commander_tests/commander_tests.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

(limited to 'src/modules/commander')

diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp
index 6e72cf0d9..0abb84a82 100644
--- a/src/modules/commander/commander_tests/commander_tests.cpp
+++ b/src/modules/commander/commander_tests/commander_tests.cpp
@@ -48,8 +48,7 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
 
 int commander_tests_main(int argc, char *argv[])
 {
-	state_machine_helper_test();
-	//state_machine_test();
+	stateMachineHelperTest();
 
 	return 0;
 }
-- 
cgit v1.2.3


From f97263f5a094e9112ecdf4a5af99ad39d4a5dda4 Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Thu, 27 Mar 2014 13:07:27 -0700
Subject: Added comprehensive arming_state_transition unit test

Also converted full to C++ style. Also converted to new unit test code.
---
 .../commander_tests/state_machine_helper_test.cpp  | 357 +++++++++++++++------
 .../commander_tests/state_machine_helper_test.h    |   2 +-
 2 files changed, 267 insertions(+), 92 deletions(-)

(limited to 'src/modules/commander')

diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
index 40bedd9f3..8a946543d 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -49,13 +49,12 @@ public:
 	StateMachineHelperTest();
 	virtual ~StateMachineHelperTest();
 
-	virtual const char*	run_tests();
+	virtual void runTests(void);
 
 private:
-	const char*	arming_state_transition_test();
-	const char*	arming_state_transition_arm_disarm_test();
-	const char*	main_state_transition_test();
-	const char*	is_safe_test();
+	bool armingStateTransitionTest();
+	bool mainStateTransitionTest();
+	bool isSafeTest();
 };
 
 StateMachineHelperTest::StateMachineHelperTest() {
@@ -64,61 +63,242 @@ StateMachineHelperTest::StateMachineHelperTest() {
 StateMachineHelperTest::~StateMachineHelperTest() {
 }
 
-const char*
-StateMachineHelperTest::arming_state_transition_test()
+bool StateMachineHelperTest::armingStateTransitionTest(void)
 {
+    // These are the critical values from vehicle_status_s and actuator_armed_s which must be primed
+    // to simulate machine state prior to testing an arming state transition. This structure is also
+    // use to represent the expected machine state after the transition has been requested.
+    typedef struct {
+        arming_state_t  arming_state;   // vehicle_status_s.arming_state
+        bool            armed;          // actuator_armed_s.armed
+        bool            ready_to_arm;   // actuator_armed_s.ready_to_arm
+    } ArmingTransitionVolatileState_t;
+    
+    // This structure represents a test case for arming_state_transition. It contains the machine
+    // state prior to transtion, the requested state to transition to and finally the expected
+    // machine state after transition.
+    typedef struct {
+        const char*                     assertMsg;                              // Text to show when test case fails
+        ArmingTransitionVolatileState_t current_state;                          // Machine state prior to transtion
+        hil_state_t                     hil_state;                              // Current vehicle_status_s.hil_state
+        bool                            condition_system_sensors_initialized;   // Current vehicle_status_s.condition_system_sensors_initialized
+        bool                            safety_switch_available;                // Current safety_s.safety_switch_available
+        bool                            safety_off;                             // Current safety_s.safety_off
+        arming_state_t                  requested_state;                        // Requested arming state to transition to
+        ArmingTransitionVolatileState_t expected_state;                         // Expected machine state after transition
+        transition_result_t             expected_transition_result;             // Expected result from arming_state_transition
+    } ArmingTransitionTest_t;
+    
+    // We use these defines so that our test cases are more readable
+    #define ATT_ARMED true
+    #define ATT_DISARMED false
+    #define ATT_READY_TO_ARM true
+    #define ATT_NOT_READY_TO_ARM false
+    #define ATT_SENSORS_INITIALIZED true
+    #define ATT_SENSORS_NOT_INITIALIZED false
+    #define ATT_SAFETY_AVAILABLE true
+    #define ATT_SAFETY_NOT_AVAILABLE true
+    #define ATT_SAFETY_OFF true
+    #define ATT_SAFETY_ON false
+    
+    // These are test cases for arming_state_transition
+    static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
+        // TRANSITION_NOT_CHANGED tests
+        
+        { "no transition: identical states",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_INIT,
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
+        
+        // TRANSITION_CHANGED tests
+        
+        // Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s
+        
+        { "transition: init to standby",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: init to standby error",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY_ERROR,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: init to reboot",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: standby to init",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_INIT,
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: standby to standby error",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY_ERROR,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: standby to reboot",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: armed to standby",
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: armed to armed error",
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED_ERROR,
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: armed error to standby error",
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY_ERROR,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: standby error to reboot",
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: in air restore to armed",
+            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: in air restore to reboot",
+            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        // hil on tests, standby error to standby not normally allowed
+        
+        { "transition: standby error to standby, hil on",
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        // Safety switch arming tests
+        
+        { "transition: init to standby, no safety switch",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: init to standby, safety switch off",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        // standby error
+        { "transition: armed error to standby error requested standby",
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        // TRANSITION_DENIED tests
+        
+        // Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s
+        
+        { "no transition: init to armed",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: standby to armed error",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED_ERROR,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: armed to init",
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_INIT,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: armed to reboot",
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: armed error to armed",
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: armed error to reboot",
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: standby error to armed",
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: standby error to standby",
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: reboot to armed",
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: in air restore to standby",
+            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        // Sensor tests
+        
+        { "no transition: init to standby - sensors not initialized",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        // Safety switch arming tests
+        
+        { "no transition: init to standby, safety switch on",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+    };
+    
 	struct vehicle_status_s status;
-	struct safety_s safety;
-	arming_state_t new_arming_state;
-	struct actuator_armed_s armed;
-
-	// Identical states.
-	status.arming_state = ARMING_STATE_INIT;
-	new_arming_state = ARMING_STATE_INIT;
-	mu_assert("no transition: identical states",
-		  TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
-
-	// INIT to STANDBY.
-	armed.armed = false;
-	armed.ready_to_arm = false;
-	status.arming_state = ARMING_STATE_INIT;
-	status.condition_system_sensors_initialized = true;
-	new_arming_state = ARMING_STATE_STANDBY;
-	mu_assert("transition: init to standby",
-		  TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
-	mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state);
-	mu_assert("not armed", !armed.armed);
-	mu_assert("ready to arm", armed.ready_to_arm);
-
-	// INIT to STANDBY, sensors not initialized.
-	armed.armed = false;
-	armed.ready_to_arm = false;
-	status.arming_state = ARMING_STATE_INIT;
-	status.condition_system_sensors_initialized = false;
-	new_arming_state = ARMING_STATE_STANDBY;
-	mu_assert("no transition: sensors not initialized",
-		  TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed));
-	mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state);
-	mu_assert("not armed", !armed.armed);
-	mu_assert("not ready to arm", !armed.ready_to_arm);
-
-	return 0;
-}
-
-const char*
-StateMachineHelperTest::arming_state_transition_arm_disarm_test()
-{
-	struct vehicle_status_s status;
-	struct safety_s safety;
-	arming_state_t new_arming_state;
+	struct safety_s         safety;
 	struct actuator_armed_s armed;
-
-	// TODO(sjwilks): ARM then DISARM.
-	return 0;
+    
+    size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
+    for (size_t i=0; i<cArmingTransitionTests; i++) {
+        const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];
+        
+        // Setup initial machine state
+        status.arming_state = test->current_state.arming_state;
+        status.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
+        status.hil_state = test->hil_state;
+        safety.safety_switch_available = test->safety_switch_available;
+        safety.safety_off = test->safety_off;
+        armed.armed = test->current_state.armed;
+        armed.ready_to_arm = test->current_state.ready_to_arm;
+        
+        // Attempt transition
+        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed);
+        
+        // Validate result of transition
+        ut_assert(test->assertMsg, test->expected_transition_result == result);
+        ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state);
+        ut_assert(test->assertMsg, armed.armed == test->expected_state.armed);
+        ut_assert(test->assertMsg, armed.ready_to_arm == test->expected_state.ready_to_arm);
+    }
+
+	return true;
 }
 
-const char*
-StateMachineHelperTest::main_state_transition_test()
+bool StateMachineHelperTest::mainStateTransitionTest(void)
 {
 	struct vehicle_status_s current_state;
 	main_state_t new_main_state;
@@ -126,70 +306,69 @@ StateMachineHelperTest::main_state_transition_test()
 	// Identical states.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	new_main_state = MAIN_STATE_MANUAL;
-	mu_assert("no transition: identical states",
+	ut_assert("no transition: identical states",
 		  TRANSITION_NOT_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);	
+	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);	
 
 	// AUTO to MANUAL.
 	current_state.main_state = MAIN_STATE_AUTO;
 	new_main_state = MAIN_STATE_MANUAL;
-	mu_assert("transition changed: auto to manual",
+	ut_assert("transition changed: auto to manual",
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+	ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
 	// MANUAL to SEATBELT.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_altitude_valid = true;
 	new_main_state = MAIN_STATE_SEATBELT;
-	mu_assert("tranisition: manual to seatbelt", 
+	ut_assert("tranisition: manual to seatbelt", 
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state);
+	ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state);
 
 	// MANUAL to SEATBELT, invalid local altitude.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_altitude_valid = false;
 	new_main_state = MAIN_STATE_SEATBELT;
-	mu_assert("no transition: invalid local altitude",
+	ut_assert("no transition: invalid local altitude",
 		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
 	// MANUAL to EASY.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_position_valid = true;
 	new_main_state = MAIN_STATE_EASY;
-	mu_assert("transition: manual to easy",
+	ut_assert("transition: manual to easy",
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state);
+	ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state);
 
 	// MANUAL to EASY, invalid local position.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_position_valid = false;
 	new_main_state = MAIN_STATE_EASY;
-	mu_assert("no transition: invalid position",
+	ut_assert("no transition: invalid position",
 		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
 	// MANUAL to AUTO.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_global_position_valid = true;
 	new_main_state = MAIN_STATE_AUTO;
-	mu_assert("transition: manual to auto",
+	ut_assert("transition: manual to auto",
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
+	ut_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
 
 	// MANUAL to AUTO, invalid global position.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_global_position_valid = false;
 	new_main_state = MAIN_STATE_AUTO;
-	mu_assert("no transition: invalid global position",
+	ut_assert("no transition: invalid global position",
 		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
-	return 0;
+	return true;
 }
 
-const char*
-StateMachineHelperTest::is_safe_test()
+bool StateMachineHelperTest::isSafeTest(void)
 {
 	struct vehicle_status_s current_state;
 	struct safety_s safety;
@@ -199,49 +378,45 @@ StateMachineHelperTest::is_safe_test()
 	armed.lockdown = false;
 	safety.safety_switch_available = true;
 	safety.safety_off = false;
-	mu_assert("is safe: not armed", is_safe(&current_state, &safety, &armed));
+	ut_assert("is safe: not armed", is_safe(&current_state, &safety, &armed));
 
 	armed.armed = false;
 	armed.lockdown = true;
 	safety.safety_switch_available = true;
 	safety.safety_off = true;
-	mu_assert("is safe: software lockdown", is_safe(&current_state, &safety, &armed));
+	ut_assert("is safe: software lockdown", is_safe(&current_state, &safety, &armed));
 
 	armed.armed = true;
 	armed.lockdown = false;
 	safety.safety_switch_available = true;
 	safety.safety_off = true;
-	mu_assert("not safe: safety off", !is_safe(&current_state, &safety, &armed));
+	ut_assert("not safe: safety off", !is_safe(&current_state, &safety, &armed));
 
 	armed.armed = true;
 	armed.lockdown = false;
 	safety.safety_switch_available = true;
 	safety.safety_off = false;
-	mu_assert("is safe: safety off", is_safe(&current_state, &safety, &armed));
+	ut_assert("is safe: safety off", is_safe(&current_state, &safety, &armed));
 
 	armed.armed = true;
 	armed.lockdown = false;
 	safety.safety_switch_available = false;
 	safety.safety_off = false;
-	mu_assert("not safe: no safety switch", !is_safe(&current_state, &safety, &armed));
+	ut_assert("not safe: no safety switch", !is_safe(&current_state, &safety, &armed));
 
-	return 0;
+	return true;
 }
 
-const char*
-StateMachineHelperTest::run_tests()
+void StateMachineHelperTest::runTests(void)
 {
-	mu_run_test(arming_state_transition_test);
-	mu_run_test(arming_state_transition_arm_disarm_test);
-	mu_run_test(main_state_transition_test);
-	mu_run_test(is_safe_test);
-
-	return 0;
+	ut_run_test(armingStateTransitionTest);
+	ut_run_test(mainStateTransitionTest);
+	ut_run_test(isSafeTest);
 }
 
-void
-state_machine_helper_test()
+void stateMachineHelperTest(void)
 {
 	StateMachineHelperTest* test = new StateMachineHelperTest();
-	test->UnitTest::print_results(test->run_tests());
+    test->runTests();
+	test->printResults();
 }
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h
index 10a68e602..bbf66255e 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.h
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.h
@@ -39,6 +39,6 @@
 #ifndef STATE_MACHINE_HELPER_TEST_H_
 #define STATE_MACHINE_HELPER_TEST_
 
-void state_machine_helper_test();
+void stateMachineHelperTest(void);
 
 #endif /* STATE_MACHINE_HELPER_TEST_H_ */
-- 
cgit v1.2.3


From c1545bd237cdaf9636da55f5b907bdadc39ed130 Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Mon, 7 Apr 2014 18:56:03 -0700
Subject: Fix float equality comparison

Also restructured incorrect return statement
---
 src/modules/commander/commander.cpp | 23 ++++++++++++-----------
 1 file changed, 12 insertions(+), 11 deletions(-)

(limited to 'src/modules/commander')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 1c11e2587..57faafcfd 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -473,18 +473,19 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
-            // Follow exactly what the mavlink spec says for values
-            if (cmd->param1 != 0.0f && cmd->param1 != 1.0f) {
-                return VEHICLE_CMD_RESULT_UNSUPPORTED;
-            }
-        
-            transition_result_t arming_res = arm_disarm(cmd->param1 == 1.0f, mavlink_fd, "arm/disarm component command");
-            if (arming_res == TRANSITION_DENIED) {
-                mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
-                result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+            // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
+            // We use an float epsilon delta to test float equality.
+            if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
+				mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1);
             } else {
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
-			}
+                transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
+                if (arming_res == TRANSITION_DENIED) {
+                    mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
+                    result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+                } else {
+                    result = VEHICLE_CMD_RESULT_ACCEPTED;
+                }
+            }
 		}
 		break;
 
-- 
cgit v1.2.3


From a0c922704457b540b236f1c72ed5630662da9d9e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 22 Apr 2014 01:38:15 +0200
Subject: commander: Also publish battery status in HIL, since we have a fake
 battery available and the system freaks out without knowing its main supply

---
 src/modules/commander/commander.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src/modules/commander')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index ce6de88ef..819dfbe83 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -967,7 +967,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
 
 			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
-			if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
+			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
 				status.battery_voltage = battery.voltage_filtered_v;
 				status.battery_current = battery.current_a;
 				status.condition_battery_voltage_valid = true;
-- 
cgit v1.2.3


From 56592ec77d3863f135c10619b15a0591f957fdbf Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Wed, 23 Apr 2014 19:01:05 +0200
Subject: commander: don't start RTL on failsafe if landed

---
 src/modules/commander/commander.cpp | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

(limited to 'src/modules/commander')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 271209412..ac433b1c7 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1015,7 +1015,7 @@ int commander_thread_main(int argc, char *argv[])
 			status_changed = true;
 		}
 
-		/* update subsystem */
+		/* update position setpoint triplet */
 		orb_check(pos_sp_triplet_sub, &updated);
 
 		if (updated) {
@@ -1273,10 +1273,15 @@ int commander_thread_main(int argc, char *argv[])
 
 				} else {
 					/* failsafe for manual modes */
-					transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
+					transition_result_t res = TRANSITION_DENIED;
+
+					if (!status.condition_landed) {
+						/* vehicle is not landed, try to perform RTL */
+						res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
+					}
 
 					if (res == TRANSITION_DENIED) {
-						/* RTL not allowed (no global position estimate), try LAND */
+						/* RTL not allowed (no global position estimate) or not wanted, try LAND */
 						res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
 
 						if (res == TRANSITION_DENIED) {
-- 
cgit v1.2.3


From 63c50676f9757f18dfca9ec3735ce59a045cea33 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 24 Apr 2014 22:38:19 +0200
Subject: MISSION switch renamed to LOITER

---
 src/modules/commander/commander.cpp               |  4 ++--
 src/modules/sensors/sensor_params.c               |  4 ++--
 src/modules/sensors/sensors.cpp                   | 12 ++++++------
 src/modules/uORB/topics/manual_control_setpoint.h |  2 +-
 src/modules/uORB/topics/rc_channels.h             |  2 +-
 5 files changed, 12 insertions(+), 12 deletions(-)

(limited to 'src/modules/commander')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index ac433b1c7..67b9d16e8 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1231,12 +1231,12 @@ int commander_thread_main(int argc, char *argv[])
 
 			} else {
 				/* MISSION switch */
-				if (sp_man.mission_switch == SWITCH_POS_ON) {
+				if (sp_man.loiter_switch == SWITCH_POS_ON) {
 					/* stick is in LOITER position */
 					status.set_nav_state = NAV_STATE_LOITER;
 					status.set_nav_state_timestamp = hrt_absolute_time();
 
-				} else if (sp_man.mission_switch != SWITCH_POS_NONE) {
+				} else if (sp_man.loiter_switch != SWITCH_POS_NONE) {
 					/* stick is in MISSION position */
 					status.set_nav_state = NAV_STATE_MISSION;
 					status.set_nav_state_timestamp = hrt_absolute_time();
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index a1f2a4ad5..bc49f5c85 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -594,13 +594,13 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
 
 /**
- * Mission switch channel mapping.
+ * Loiter switch channel mapping.
  *
  * @min 0
  * @max 18
  * @group Radio Calibration
  */
-PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
+PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0);
 
 //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
 
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index ba233dfd0..41e2148ac 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -254,7 +254,7 @@ private:
 		int rc_map_mode_sw;
 		int rc_map_return_sw;
 		int rc_map_assisted_sw;
-		int rc_map_mission_sw;
+		int rc_map_loiter_sw;
 
 //		int rc_map_offboard_ctrl_mode_sw;
 
@@ -297,7 +297,7 @@ private:
 		param_t rc_map_mode_sw;
 		param_t rc_map_return_sw;
 		param_t rc_map_assisted_sw;
-		param_t rc_map_mission_sw;
+		param_t rc_map_loiter_sw;
 
 //		param_t rc_map_offboard_ctrl_mode_sw;
 
@@ -508,7 +508,7 @@ Sensors::Sensors() :
 
 	/* optional mode switches, not mapped per default */
 	_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
-	_parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW");
+	_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
 
 //	_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
 
@@ -654,7 +654,7 @@ Sensors::parameters_update()
 		warnx(paramerr);
 	}
 
-	if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) {
+	if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) {
 		warnx(paramerr);
 	}
 
@@ -682,7 +682,7 @@ Sensors::parameters_update()
 	_rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
 	_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
 	_rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1;
-	_rc.function[MISSION] = _parameters.rc_map_mission_sw - 1;
+	_rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
 
 	_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
 
@@ -1416,7 +1416,7 @@ Sensors::rc_poll()
 			/* mode switches */
 			manual.mode_switch = get_rc_switch_position(MODE);
 			manual.assisted_switch = get_rc_switch_position(ASSISTED);
-			manual.mission_switch = get_rc_switch_position(MISSION);
+			manual.loiter_switch = get_rc_switch_position(LOITER);
 			manual.return_switch = get_rc_switch_position(RETURN);
 
 			/* publish manual_control_setpoint topic */
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index 2b3a337b2..a23d89cd2 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -79,7 +79,7 @@ struct manual_control_setpoint_s {
 	switch_pos_t mode_switch;			/**< mode 3 position switch (mandatory): manual, assisted, auto */
 	switch_pos_t return_switch;			/**< land 2 position switch (mandatory): land, no effect */
 	switch_pos_t assisted_switch;			/**< assisted 2 position switch (optional): seatbelt, simple */
-	switch_pos_t mission_switch;		/**< mission 2 position switch (optional): mission, loiter */
+	switch_pos_t loiter_switch;		/**< mission 2 position switch (optional): mission, loiter */
 }; /**< manual control inputs */
 
 /**
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 3246a39dd..c168b2fac 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -65,7 +65,7 @@ enum RC_CHANNELS_FUNCTION {
 	MODE = 4,
 	RETURN = 5,
 	ASSISTED = 6,
-	MISSION = 7,
+	LOITER = 7,
 	OFFBOARD_MODE = 8,
 	FLAPS   = 9,
 	AUX_1   = 10,
-- 
cgit v1.2.3


From 2453b354faa5a6ccdcca6afb94b54967679cc9de Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 25 Apr 2014 22:26:51 +0200
Subject: Failsafe landing without position control fixed

---
 src/modules/commander/commander.cpp                |  7 +++++--
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 12 ++++++++++++
 2 files changed, 17 insertions(+), 2 deletions(-)

(limited to 'src/modules/commander')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 1827f252c..0bf752279 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1747,8 +1747,11 @@ set_control_mode()
 		control_mode.flag_control_auto_enabled = true;
 		control_mode.flag_control_rates_enabled = true;
 		control_mode.flag_control_attitude_enabled = true;
-		control_mode.flag_control_position_enabled = true;
-		control_mode.flag_control_velocity_enabled = true;
+
+		/* in failsafe LAND mode position may be not available */
+		control_mode.flag_control_position_enabled = status.condition_local_position_valid;
+		control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
+
 		control_mode.flag_control_altitude_enabled = true;
 		control_mode.flag_control_climb_rate_enabled = true;
 	}
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 6b797f222..5194ef697 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1001,6 +1001,18 @@ MulticopterPositionControl::task_main()
 						_att_sp.roll_body = euler(0);
 						_att_sp.pitch_body = euler(1);
 						/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
+
+					} else if (!_control_mode.flag_control_manual_enabled) {
+						/* autonomous altitude control without position control (failsafe landing),
+						 * force level attitude, don't change yaw */
+						R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
+
+						/* copy rotation matrix to attitude setpoint topic */
+						memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+						_att_sp.R_valid = true;
+
+						_att_sp.roll_body = 0.0f;
+						_att_sp.pitch_body = 0.0f;
 					}
 
 					_att_sp.thrust = thrust_abs;
-- 
cgit v1.2.3


From e6d332aa7cd038b00643a70d99e82d60b7ffbcf0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 27 Apr 2014 15:50:53 +0200
Subject: Make commander less pedantic about position status

---
 src/modules/commander/commander.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src/modules/commander')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 352711734..53ed34f46 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -117,7 +117,7 @@ extern struct system_load_s system_load;
 #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
-#define POSITION_TIMEOUT 30000 /**< consider the local or global position estimate invalid after 30ms */
+#define POSITION_TIMEOUT 100000 /**< consider the local or global position estimate invalid after 100ms */
 #define RC_TIMEOUT 500000
 #define DIFFPRESS_TIMEOUT 2000000
 
-- 
cgit v1.2.3