Checkpoint: Arming/Disarming works

apps/commander/commander.c

@@ -1649,6 +1649,13 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* End battery voltage check */
 
+		/* If in INIT state, try to proceed to STANDBY state */
+		if (current_status.arming_state == ARMING_STATE_INIT) {
+			do_arming_state_update(stat_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+		} else {
+			// XXX: Add emergency stuff if sensors are lost
+		}
+
 
 		/*
 		 * Check for valid position information.
@@ -1894,12 +1901,11 @@ int commander_thread_main(int argc, char *argv[])
 				 * Check if left stick is in lower left position --> switch to standby state.
 				 * Do this only for multirotors, not for fixed wing aircraft.
 				 */
-				if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-				     (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-				     (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
-				    ) &&
-				    ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) &&
-				    (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
+//				if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
+//				     (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
+//				     (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
+//				    ) &&
+				if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
 					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 						do_arming_state_update(stat_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
 						stick_off_counter = 0;

apps/commander/state_machine_helper.c

@@ -304,8 +304,11 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat
 	bool valid_transition = false;
 	int ret = ERROR;
 
+	warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state);
+
 	if (current_status->arming_state == new_state) {
 		warnx("Arming state not changed");
+		valid_transition = true;
 
 	} else {
 
@@ -333,6 +336,12 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat
 					} else {
 						mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init.");
 					}
+
+				} else if (current_status->arming_state == ARMING_STATE_ARMED) {
+
+					current_status->flag_system_armed = false;
+					mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state");
+					valid_transition = true;
 				}
 				break;
 

apps/mavlink_onboard/mavlink.c

@@ -290,7 +290,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct
 	}
 
 	/* set arming state */
-	if (armed->armed) {
+	if (v_status->flag_system_armed) {
 		*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 	} else {
 		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;