Checkpoint: Navigation states and arming seem to work

3 files changed, 43 insertions, 27 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 38d2ffc96..2784e77db 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -812,13 +812,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 			}
 
 			if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) {
-				if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) {
+				if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) {
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
 				} else {
 					result = VEHICLE_CMD_RESULT_DENIED;
 				}
 			} else {
-				if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) {
+				if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) {
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
 				} else {
 					result = VEHICLE_CMD_RESULT_DENIED;
@@ -831,14 +831,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 
 			/* request to arm */
 			if ((int)cmd->param1 == 1) {
-				if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) {
+				if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) {
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
 				} else {
 					result = VEHICLE_CMD_RESULT_DENIED;
 				}
 			/* request to disarm */
 			} else if ((int)cmd->param1 == 0) {
-				if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) {
+				if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) {
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
 				} else {
 					result = VEHICLE_CMD_RESULT_DENIED;
@@ -851,7 +851,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 
 			/* request for an autopilot reboot */
 			if ((int)cmd->param1 == 1) {
-					if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) {
+					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, mavlink_fd)) {
 						/* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
 						result = VEHICLE_CMD_RESULT_ACCEPTED;
 
@@ -893,7 +893,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 				/* gyro calibration */
 				if ((int)(cmd->param1) == 1) {
 
-					if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) {
+					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
 
 						result = VEHICLE_CMD_RESULT_ACCEPTED;
 
@@ -903,7 +903,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 						mavlink_log_info(mavlink_fd, "finished gyro cal");
 						tune_confirm();
 						// XXX if this fails, go to ERROR
-						arming_state_transition(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+						arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
 
 					} else {
 						result = VEHICLE_CMD_RESULT_DENIED;
@@ -915,7 +915,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 				/* magnetometer calibration */
 				if ((int)(cmd->param2) == 1) {
 
-					if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) {
+					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
 
 						result = VEHICLE_CMD_RESULT_ACCEPTED;
 
@@ -925,7 +925,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 						mavlink_log_info(mavlink_fd, "finished mag cal");
 						tune_confirm();
 						// XXX if this fails, go to ERROR
-						arming_state_transition(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+						arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
 
 					} else {
 						result = VEHICLE_CMD_RESULT_DENIED;
@@ -986,7 +986,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 				/* accel calibration */
 				if ((int)(cmd->param5) == 1) {
 
-					if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) {
+					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
 
 						result = VEHICLE_CMD_RESULT_ACCEPTED;
 
@@ -996,7 +996,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 						mavlink_log_info(mavlink_fd, "finished acc cal");
 						tune_confirm();
 						// XXX what if this fails
-						arming_state_transition(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+						arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
 
 					} else {
 						result = VEHICLE_CMD_RESULT_DENIED;
@@ -1647,8 +1647,9 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (current_status.arming_state == ARMING_STATE_INIT) {
-			//XXX what if this fails
-			arming_state_transition(stat_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+			// XXX fix for now
+			current_status.flag_system_sensors_initialized = true;
+			arming_state_transition(stat_pub, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
 		} else {
 			// XXX: Add emergency stuff if sensors are lost
 		}
@@ -1790,11 +1791,10 @@ int commander_thread_main(int argc, char *argv[])
 				* Check if manual control modes have to be switched
 				*/
 				if (!isfinite(sp_man.mode_switch)) {
-				warnx("mode sw not finite");
 
+					warnx("mode sw not finite");
 					/* no valid stick position, go to default */
 
-
 				} else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) {
 
 					/* bottom stick position, go to manual mode */
@@ -1806,6 +1806,7 @@ int commander_thread_main(int argc, char *argv[])
 					current_status.mode_switch = MODE_SWITCH_AUTO;
 
 				} else {
+
 					/* center stick position, set seatbelt/simple control */
 					current_status.mode_switch = MODE_SWITCH_SEATBELT;
 				}
@@ -1869,7 +1870,7 @@ int commander_thread_main(int argc, char *argv[])
 							
 							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
 								// These is not supposed to happen
-								warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
+								warnx("ERROR: Navigation state MANUAL_STANDBY rejected 1");
 							}
 			
 						/* Try seatbelt or fallback to manual */
@@ -1879,7 +1880,7 @@ int commander_thread_main(int argc, char *argv[])
 								// fallback to MANUAL_STANDBY
 								if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
 								// These is not supposed to happen
-								warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
+								warnx("ERROR: Navigation state MANUAL_STANDBY rejected 2");
 								}
 							}
 			
@@ -1892,7 +1893,7 @@ int commander_thread_main(int argc, char *argv[])
 									// or fallback to MANUAL_STANDBY
 									if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
 									// These is not supposed to happen
-									warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
+									warnx("ERROR: Navigation state MANUAL_STANDBY rejected 3");
 									}
 								}
 							}
@@ -2040,7 +2041,8 @@ int commander_thread_main(int argc, char *argv[])
 				//				    ) &&
 				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) {
-						arming_state_transition(stat_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+						printf("Try disarm\n");
+						arming_state_transition(stat_pub, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
 						stick_off_counter = 0;
 
 					} else {
@@ -2052,7 +2054,8 @@ int commander_thread_main(int argc, char *argv[])
 				/* check if left stick is in lower right position --> arm */
 				if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) {
 					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						arming_state_transition(stat_pub, &current_status, mavlink_fd, ARMING_STATE_ARMED);
+						printf("Try arm\n");
+						arming_state_transition(stat_pub, &current_status, ARMING_STATE_ARMED, mavlink_fd);
 						stick_on_counter = 0;
 
 					} else {
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index 6c15bd725..ae7f2a1c1 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -255,10 +255,13 @@
 //}
 
 int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) {
+	
 	int ret = ERROR;
 
 	/* only check transition if the new state is actually different from the current one */
-	if (new_arming_state != current_state->arming_state) {
+	if (new_arming_state == current_state->arming_state) {
+		ret = OK;
+	} else {
 
 		switch (new_arming_state) {
 			case ARMING_STATE_INIT:
@@ -313,7 +316,13 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
 			default:
 				break;
 		}
+
+		if (ret == OK) {
+			current_state->arming_state = new_arming_state;
+			state_machine_publish(status_pub, current_state, mavlink_fd);
+		}
 	}
+
 	return ret;
 }
 
@@ -328,7 +337,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 	int ret = ERROR;
 
 	/* only check transition if the new state is actually different from the current one */
-	if (new_navigation_state != current_state->navigation_state) {
+	if (new_navigation_state == current_state->navigation_state) {
+		ret = OK;
+	} else {
 
 		switch (new_navigation_state) {
 			case NAVIGATION_STATE_INIT:
@@ -561,13 +572,15 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 			default:
 				break;
 		}
-	}
 
-	if (ret == OK) {
-		current_state->navigation_state = new_navigation_state;
-		state_machine_publish(status_pub, current_state, mavlink_fd);
+		if (ret == OK) {
+			current_state->navigation_state = new_navigation_state;
+			state_machine_publish(status_pub, current_state, mavlink_fd);
+		}
 	}
 
+	
+
 	return ret;
 }
 
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index ce404ee7e..b53de8c46 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -432,7 +432,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
 
 	/* mandatory mode switches, mapped to channel 5 and 6 per default */
-	_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_OVER_SW");
+	_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
 	_parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW");
 
 	_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");