Checkpoint: Added arming state check

6 files changed, 36 insertions, 8 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 8716caef7..1bfdd5660 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -2054,9 +2054,13 @@ int commander_thread_main(int argc, char *argv[])
 			state_changed = false;
 		}
 
+		/* make changes in state machine if needed */
+		update_state_machine(stat_pub, &current_status, mavlink_fd);
+
+
+
 		/* Store old modes to detect and act on state transitions */
 		voltage_previous = current_status.voltage_battery;
-
 		fflush(stdout);
 		counter++;
 		usleep(COMMANDER_MONITORING_INTERVAL);
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index e1ec73110..d89be781a 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -53,6 +53,28 @@
 #include "state_machine_helper.h"
 
 
+void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+{
+	/* check arming first */
+	if (current_status->flag_system_armed && current_status->flag_system_emergency) {
+		do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT);
+	} else if(current_status->flag_system_armed && !current_status->flag_system_emergency) {
+		do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ARMED);
+	} else if(!current_status->flag_system_armed && current_status->flag_system_emergency) {
+		do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR);
+	} else if(!current_status->flag_system_armed && !current_status->flag_system_emergency) {
+		do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY);
+	} else if (current_status->flag_system_sensors_ok) {
+		do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY);
+	} else if (!current_status->flag_system_sensors_ok && current_status->flag_system_armed) {
+		do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT);
+	} else if (!current_status->flag_system_sensors_ok && !current_status->flag_system_armed) {
+		do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR);
+	}
+
+	/* now determine the navigation state */
+}
+
 /**
  * Transition from one navigation state to another
  */
diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h
index bf9296caf..ed18bfbd2 100644
--- a/apps/commander/state_machine_helper.h
+++ b/apps/commander/state_machine_helper.h
@@ -47,6 +47,8 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 
+void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+
 int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state);
 
 int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state);
diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c
index 9f23ebbba..df7f66116 100644
--- a/apps/sensors/sensor_params.c
+++ b/apps/sensors/sensor_params.c
@@ -158,13 +158,11 @@ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
 PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
 PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
 
-PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5);
-PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6);
+PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
+PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6);
+PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
 
-PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
+//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
 
 PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
 
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 28579bc70..ce404ee7e 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -438,7 +438,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
 
 	/* optional mode switches, not mapped per default */
-	_parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSION_SW");
+	_parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW");
 
 //	_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
 
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index f9c4a5fff..ba9b9793b 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -170,6 +170,8 @@ struct vehicle_status_s
 
 	bool flag_system_armed;				/**< true is motors / actuators are armed */
 	bool flag_system_emergency;
+	bool flag_system_sensors_ok;
+
 	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
 	bool flag_control_offboard_enabled;		/**< true if offboard control input is on */
 	bool flag_hil_enabled;				/**< true if hardware in the loop simulation is enabled */