Introduced new actuator_safety topic

src/drivers/ardrone_interface/ardrone_interface.c

@@ -55,6 +55,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_safety.h>
 
 #include <systemlib/systemlib.h>
 
@@ -247,13 +248,13 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	memset(&state, 0, sizeof(state));
 	struct actuator_controls_s actuator_controls;
 	memset(&actuator_controls, 0, sizeof(actuator_controls));
-	struct actuator_armed_s armed;
+	struct actuator_safety_s safety;
-	armed.armed = false;
+	safety.armed = false;
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
-	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
 
 	printf("[ardrone_interface] Motors initialized - ready.\n");
 	fflush(stdout);
@@ -322,18 +323,14 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 		} else {
 			/* MAIN OPERATION MODE */
 
-			/* get a local copy of the vehicle state */
-			orb_copy(ORB_ID(vehicle_status), state_sub, &state);
 			/* get a local copy of the actuator controls */
 			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
-			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+			orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
 			
 			/* for now only spin if armed and immediately shut down
 			 * if in failsafe
 			 */
-			//XXX fix this
+			if (safety.armed && !safety.lockdown) {
-			//if (armed.armed && !armed.lockdown) {
-			if (state.flag_fmu_armed) {
 				ardrone_mixing_and_output(ardrone_write, &actuator_controls);
 
 			} else {

src/drivers/blinkm/blinkm.cpp

@@ -116,6 +116,7 @@
 #include <poll.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_safety.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
 static const float MAX_CELL_VOLTAGE	= 4.3f;
@@ -376,6 +377,7 @@ BlinkM::led()
 
 	static int vehicle_status_sub_fd;
 	static int vehicle_gps_position_sub_fd;
+	static int actuator_safety_sub_fd;
 
 	static int num_of_cells = 0;
 	static int detected_cells_runcount = 0;
@@ -386,6 +388,7 @@ BlinkM::led()
 	static int led_interval = 1000;
 
 	static int no_data_vehicle_status = 0;
+	static int no_data_actuator_safety = 0;
 	static int no_data_vehicle_gps_position = 0;
 
 	static bool topic_initialized = false;
@@ -398,6 +401,9 @@ BlinkM::led()
 		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
 		orb_set_interval(vehicle_status_sub_fd, 1000);
 
+		actuator_safety_sub_fd = orb_subscribe(ORB_ID(actuator_safety));
+		orb_set_interval(actuator_safety_sub_fd, 1000);
+
 		vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
 		orb_set_interval(vehicle_gps_position_sub_fd, 1000);
 
@@ -452,12 +458,14 @@ BlinkM::led()
 	if (led_thread_runcount == 15) {
 		/* obtained data for the first file descriptor */
 		struct vehicle_status_s vehicle_status_raw;
+		struct actuator_safety_s actuator_safety;
 		struct vehicle_gps_position_s vehicle_gps_position_raw;
 
 		memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
 		memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
 
 		bool new_data_vehicle_status;
+		bool new_data_actuator_safety;
 		bool new_data_vehicle_gps_position;
 
 		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
@@ -471,6 +479,17 @@ BlinkM::led()
 				no_data_vehicle_status = 3;
 		}
 
+		orb_check(actuator_safety_sub_fd, &new_data_actuator_safety);
+
+		if (new_data_actuator_safety) {
+			orb_copy(ORB_ID(actuator_safety), actuator_safety_sub_fd, &actuator_safety);
+			no_data_actuator_safety = 0;
+		} else {
+			no_data_actuator_safety++;
+			if(no_data_vehicle_status >= 3)
+				no_data_vehicle_status = 3;
+		}
+
 		orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
 
 		if (new_data_vehicle_gps_position) {
@@ -530,7 +549,7 @@ BlinkM::led()
 				} else {
 					/* no battery warnings here */
 
-					if(vehicle_status_raw.flag_fmu_armed == false) {
+					if(actuator_safety.armed == false) {
 						/* system not armed */
 						led_color_1 = LED_RED;
 						led_color_2 = LED_RED;

src/drivers/hil/hil.cpp

@@ -75,6 +75,7 @@
 #include <systemlib/mixer/mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_safety.h>
 #include <uORB/topics/actuator_outputs.h>
 
 #include <systemlib/err.h>
@@ -108,7 +109,7 @@ private:
 	int 		_current_update_rate;
 	int		_task;
 	int		_t_actuators;
-	int		_t_armed;
+	int		_t_safety;
 	orb_advert_t	_t_outputs;
 	unsigned	_num_outputs;
 	bool		_primary_pwm_device;
@@ -161,7 +162,7 @@ HIL::HIL() :
 	_current_update_rate(0),
 	_task(-1),
 	_t_actuators(-1),
-	_t_armed(-1),
+	_t_safety(-1),
 	_t_outputs(0),
 	_num_outputs(0),
 	_primary_pwm_device(false),
@@ -321,8 +322,8 @@ HIL::task_main()
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
+	_t_safety = orb_subscribe(ORB_ID(actuator_safety));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+	orb_set_interval(_t_safety, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
@@ -334,7 +335,7 @@ HIL::task_main()
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
+	fds[1].fd = _t_safety;
 	fds[1].events = POLLIN;
 
 	unsigned num_outputs;
@@ -426,15 +427,15 @@ HIL::task_main()
 
 		/* how about an arming update? */
 		if (fds[1].revents & POLLIN) {
-			actuator_armed_s aa;
+			actuator_safety_s aa;
 
 			/* get new value */
-			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+			orb_copy(ORB_ID(actuator_safety), _t_safety, &aa);
 		}
 	}
 
 	::close(_t_actuators);
-	::close(_t_armed);
+	::close(_t_safety);
 
 	/* make sure servos are off */
 	// up_pwm_servo_deinit();

src/drivers/mkblctrl/mkblctrl.cpp

@@ -74,6 +74,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_safety.h>
 
 #include <systemlib/err.h>
 
@@ -132,7 +133,7 @@ private:
 	int 		_current_update_rate;
 	int		_task;
 	int		_t_actuators;
-	int		_t_armed;
+	int		_t_actuator_safety;
 	unsigned int		_motor;
 	int    _px4mode;
 	int    _frametype;
@@ -240,7 +241,7 @@ MK::MK(int bus) :
 	_update_rate(50),
 	_task(-1),
 	_t_actuators(-1),
-	_t_armed(-1),
+	_t_actuator_safety(-1),
 	_t_outputs(0),
 	_t_actuators_effective(0),
 	_num_outputs(0),
@@ -496,8 +497,8 @@ MK::task_main()
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
+	_t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+	orb_set_interval(_t_actuator_safety, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
@@ -519,7 +520,7 @@ MK::task_main()
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
+	fds[1].fd = _t_actuator_safety;
 	fds[1].events = POLLIN;
 
 	log("starting");
@@ -625,13 +626,13 @@ MK::task_main()
 
 		/* how about an arming update? */
 		if (fds[1].revents & POLLIN) {
-			actuator_armed_s aa;
+			actuator_safety_s as;
 
 			/* get new value */
-			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+			orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as);
 
 			/* update PWM servo armed status if armed and not locked down */
-			mk_servo_arm(aa.armed && !aa.lockdown);
+			mk_servo_arm(as.armed && !as.lockdown);
 		}
 
 
@@ -639,7 +640,7 @@ MK::task_main()
 
 	::close(_t_actuators);
 	::close(_t_actuators_effective);
-	::close(_t_armed);
+	::close(_t_actuator_safety);
 
 
 	/* make sure servos are off */

src/drivers/px4fmu/fmu.cpp

@@ -69,6 +69,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_safety.h>
 
 #include <systemlib/err.h>
 #include <systemlib/ppm_decode.h>
@@ -104,7 +105,7 @@ private:
 	unsigned	_current_update_rate;
 	int		_task;
 	int		_t_actuators;
-	int		_t_armed;
+	int		_t_actuator_safety;
 	orb_advert_t	_t_outputs;
 	orb_advert_t	_t_actuators_effective;
 	unsigned	_num_outputs;
@@ -174,7 +175,7 @@ PX4FMU::PX4FMU() :
 	_current_update_rate(0),
 	_task(-1),
 	_t_actuators(-1),
-	_t_armed(-1),
+	_t_actuator_safety(-1),
 	_t_outputs(0),
 	_t_actuators_effective(0),
 	_num_outputs(0),
@@ -376,8 +377,8 @@ PX4FMU::task_main()
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
+	_t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+	orb_set_interval(_t_actuator_safety, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
@@ -396,7 +397,7 @@ PX4FMU::task_main()
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
+	fds[1].fd = _t_actuator_safety;
 	fds[1].events = POLLIN;
 
 	unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
@@ -499,13 +500,13 @@ PX4FMU::task_main()
 
 		/* how about an arming update? */
 		if (fds[1].revents & POLLIN) {
-			actuator_armed_s aa;
+			actuator_safety_s as;
 
 			/* get new value */
-			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+			orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as);
 
 			/* update PWM servo armed status if armed and not locked down */
-			bool set_armed = aa.armed && !aa.lockdown;
+			bool set_armed = as.armed && !as.lockdown;
 			if (set_armed != _armed) {
 				_armed = set_armed;
 				up_pwm_servo_arm(set_armed);
@@ -535,7 +536,7 @@ PX4FMU::task_main()
 
 	::close(_t_actuators);
 	::close(_t_actuators_effective);
-	::close(_t_armed);
+	::close(_t_actuator_safety);
 
 	/* make sure servos are off */
 	up_pwm_servo_deinit();
@@ -1021,12 +1022,12 @@ fake(int argc, char *argv[])
 	if (handle < 0)
 		errx(1, "advertise failed");
 
-	actuator_armed_s aa;
+	actuator_safety_s as;
 
-	aa.armed = true;
+	as.armed = true;
-	aa.lockdown = false;
+	as.lockdown = false;
 
-	handle = orb_advertise(ORB_ID(actuator_armed), &aa);
+	handle = orb_advertise(ORB_ID(actuator_safety), &as);
 
 	if (handle < 0)
 		errx(1, "advertise failed 2");

src/drivers/px4io/px4io.cpp

@@ -75,6 +75,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_safety.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/rc_channels.h>
@@ -152,7 +153,7 @@ private:
 
 	/* subscribed topics */
 	int			_t_actuators;	///< actuator controls topic
-	int			_t_armed;	///< system armed control topic
+	int			_t_actuator_safety;	///< system armed control topic
 	int 			_t_vstatus;	///< system / vehicle status
 	int			_t_param;	///< parameter update topic
 
@@ -327,7 +328,7 @@ PX4IO::PX4IO() :
 	_status(0),
 	_alarms(0),
 	_t_actuators(-1),
-	_t_armed(-1),
+	_t_actuator_safety(-1),
 	_t_vstatus(-1),
 	_t_param(-1),
 	_to_input_rc(0),
@@ -433,20 +434,20 @@ PX4IO::init()
 		 * remains untouched (so manual override is still available).
 		 */
 
-		int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+		int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
 		/* fill with initial values, clear updated flag */
-		vehicle_status_s status;
+		struct actuator_safety_s armed;
 		uint64_t try_start_time = hrt_absolute_time();
 		bool updated = false;
 		
-		/* keep checking for an update, ensure we got a recent state,
+		/* keep checking for an update, ensure we got a arming information,
 		   not something that was published a long time ago. */
 		do {
-			orb_check(vstatus_sub, &updated);
+			orb_check(safety_sub, &updated);
 
 			if (updated) {
 				/* got data, copy and exit loop */
-				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+				orb_copy(ORB_ID(actuator_safety), safety_sub, &armed);
 				break;
 			}
 
@@ -472,10 +473,10 @@ PX4IO::init()
 		cmd.param6 = 0;
 		cmd.param7 = 0;
 		cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM;
-		cmd.target_system = status.system_id;
+		// cmd.target_system = status.system_id;
-		cmd.target_component = status.component_id;
+		// cmd.target_component = status.component_id;
-		cmd.source_system = status.system_id;
+		// cmd.source_system = status.system_id;
-		cmd.source_component = status.component_id;
+		// cmd.source_component = status.component_id;
 		/* ask to confirm command */
 		cmd.confirmation =  1;
 
@@ -484,10 +485,10 @@ PX4IO::init()
 
 		/* spin here until IO's state has propagated into the system */
 		do {
-			orb_check(vstatus_sub, &updated);
+			orb_check(safety_sub, &updated);
 
 			if (updated) {
-				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+				orb_copy(ORB_ID(actuator_safety), safety_sub, &armed);
 			}
 
 			/* wait 10 ms */
@@ -500,7 +501,7 @@ PX4IO::init()
 			}
 
 		/* keep waiting for state change for 10 s */
-		} while (!status.flag_fmu_armed);
+		} while (!armed.armed);
 
 	/* regular boot, no in-air restart, init IO */
 	} else {
@@ -564,8 +565,8 @@ PX4IO::task_main()
 				     ORB_ID(actuator_controls_1));
 	orb_set_interval(_t_actuators, 20);		/* default to 50Hz */
 
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
+	_t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+	orb_set_interval(_t_actuator_safety, 200);		/* 5Hz update rate */
 
 	_t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
 	orb_set_interval(_t_vstatus, 200);		/* 5Hz update rate max. */
@@ -577,7 +578,7 @@ PX4IO::task_main()
 	pollfd fds[4];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
+	fds[1].fd = _t_actuator_safety;
 	fds[1].events = POLLIN;
 	fds[2].fd = _t_vstatus;
 	fds[2].events = POLLIN;
@@ -713,16 +714,16 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
 int
 PX4IO::io_set_arming_state()
 {
-	actuator_armed_s	armed;		///< system armed state
+	actuator_safety_s	safety;		///< system armed state
 	vehicle_status_s	vstatus;	///< overall system state
 
-	orb_copy(ORB_ID(actuator_armed), _t_armed, &armed);
+	orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety);
 	orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus);
 
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
-	if (armed.armed && !armed.lockdown) {
+	if (safety.armed && !safety.lockdown) {
 		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;

src/modules/commander/commander.c

@@ -73,8 +73,10 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_safety.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/differential_pressure.h>
 #include <mavlink/mavlink_log.h>
@@ -137,10 +139,6 @@ static bool thread_should_exit = false;		/**< daemon exit flag */
 static bool thread_running = false;		/**< daemon status flag */
 static int daemon_task;				/**< Handle of daemon task / thread */
 
-/* Main state machine */
-static struct vehicle_status_s current_status;
-static orb_advert_t stat_pub;
-
 /* timout until lowlevel failsafe */
 static unsigned int failsafe_lowlevel_timeout_ms;
 
@@ -187,7 +185,7 @@ void do_rc_calibration(void);
 void do_airspeed_calibration(void);
 
 
-void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
+void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety);
 
 int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
 
@@ -304,10 +302,11 @@ void do_rc_calibration()
 {
 	mavlink_log_info(mavlink_fd, "trim calibration starting");
 
-	if (current_status.offboard_control_signal_lost) {
+	/* XXX fix this */
-		mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
+	// if (current_status.offboard_control_signal_lost) {
-		return;
+	// 	mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
-	}
+	// 	return;
+	// }
 
 	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
 	struct manual_control_setpoint_s sp;
@@ -718,7 +717,7 @@ void do_airspeed_calibration()
 	close(diff_pres_sub);
 }
 
-void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd)
+void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety)
 {
 	/* result of the command */
 	uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
@@ -738,13 +737,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, ARMING_STATE_ARMED, mavlink_fd)) {
+				if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) {
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
 				} else {
 					result = VEHICLE_CMD_RESULT_DENIED;
 				}
 			} else {
-				if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) {
+				if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) {
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
 				} else {
 					result = VEHICLE_CMD_RESULT_DENIED;
@@ -757,14 +756,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, ARMING_STATE_ARMED, mavlink_fd)) {
+				if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, 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, ARMING_STATE_STANDBY, mavlink_fd)) {
+				if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) {
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
 				} else {
 					result = VEHICLE_CMD_RESULT_DENIED;
@@ -777,7 +776,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, ARMING_STATE_REBOOT, mavlink_fd)) {
+					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, safety_pub, safety, mavlink_fd)) {
 						/* reboot is executed later, after positive tune is sent */
 						result = VEHICLE_CMD_RESULT_ACCEPTED;
 						tune_positive();
@@ -826,7 +825,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 				if(low_prio_task == LOW_PRIO_TASK_NONE) {
 
 					/* try to go to INIT/PREFLIGHT arming state */
-					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
 						result = VEHICLE_CMD_RESULT_ACCEPTED;
 						/* now set the task for the low prio thread */
 						low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION;
@@ -845,7 +844,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 				if(low_prio_task == LOW_PRIO_TASK_NONE) {
 
 					/* try to go to INIT/PREFLIGHT arming state */
-					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
 						result = VEHICLE_CMD_RESULT_ACCEPTED;
 						/* now set the task for the low prio thread */
 						low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION;
@@ -865,7 +864,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 			// 	if(low_prio_task == LOW_PRIO_TASK_NONE) {
 
 			// 		/* try to go to INIT/PREFLIGHT arming state */
-			// 		if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+			// 		if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
 			// 			result = VEHICLE_CMD_RESULT_ACCEPTED;
 			// 			/* now set the task for the low prio thread */
 			// 			low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION;
@@ -886,7 +885,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 			// 	if(low_prio_task == LOW_PRIO_TASK_NONE) {
 
 			// 		/* try to go to INIT/PREFLIGHT arming state */
-			// 		if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+			// 		if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
 			// 			result = VEHICLE_CMD_RESULT_ACCEPTED;
 			// 			/* now set the task for the low prio thread */
 			// 			low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION;
@@ -906,7 +905,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 				if(low_prio_task == LOW_PRIO_TASK_NONE) {
 
 					/* try to go to INIT/PREFLIGHT arming state */
-					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
 						result = VEHICLE_CMD_RESULT_ACCEPTED;
 						/* now set the task for the low prio thread */
 						low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION;
@@ -925,7 +924,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 				if(low_prio_task == LOW_PRIO_TASK_NONE) {
 
 					/* try to go to INIT/PREFLIGHT arming state */
-					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) {
 						result = VEHICLE_CMD_RESULT_ACCEPTED;
 						/* now set the task for the low prio thread */
 						low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION;
@@ -1189,17 +1188,30 @@ int commander_thread_main(int argc, char *argv[])
 	if (mavlink_fd < 0) {
 		warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.");
 	}
-
+	
+	/* Main state machine */
+	struct vehicle_status_s current_status;
+	orb_advert_t status_pub;
 	/* make sure we are in preflight state */
 	memset(&current_status, 0, sizeof(current_status));
 
+	/* safety topic */
+	struct actuator_safety_s safety;
+	orb_advert_t safety_pub;
+	/* Initialize safety with all false */
+	memset(&safety, 0, sizeof(safety));
+	
+
+	/* flags for control apps */
+	struct vehicle_control_mode_s control_mode;
+	orb_advert_t control_mode_pub;
+
+	/* Initialize all flags to false */
+	memset(&control_mode, 0, sizeof(control_mode));
 
 	current_status.navigation_state = NAVIGATION_STATE_INIT;
 	current_status.arming_state = ARMING_STATE_INIT;
 	current_status.hil_state = HIL_STATE_OFF;
-	current_status.flag_hil_enabled = false;
-	current_status.flag_fmu_armed = false;
-	current_status.flag_io_armed = false; // XXX read this from somewhere
 
 	/* neither manual nor offboard control commands have been received */
 	current_status.offboard_control_signal_found_once = false;
@@ -1222,16 +1234,18 @@ int commander_thread_main(int argc, char *argv[])
 	current_status.condition_system_sensors_initialized = true;
 
 	/* advertise to ORB */
-	stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
+	status_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
 	/* publish current state machine */
-	state_machine_publish(stat_pub, &current_status, mavlink_fd);
+	state_machine_publish(status_pub, &current_status, mavlink_fd);
+
+	safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety);
 
 	/* home position */
 	orb_advert_t home_pub = -1;
 	struct home_position_s home;
 	memset(&home, 0, sizeof(home));
 
-	if (stat_pub < 0) {
+	if (status_pub < 0) {
 		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
 		warnx("exiting.");
 		exit(ERROR);
@@ -1333,6 +1347,7 @@ int commander_thread_main(int argc, char *argv[])
 	memset(&battery, 0, sizeof(battery));
 	battery.voltage_v = 0.0f;
 
+
 	// uint8_t vehicle_state_previous = current_status.state_machine;
 	float voltage_previous = 0.0f;
 
@@ -1386,9 +1401,11 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
 
 			/* handle it */
-			handle_command(stat_pub, &current_status, &cmd);
+			handle_command(status_pub, &current_status, &cmd, safety_pub, &safety);
 		}
 
+		
+
 		/* update parameters */
 		orb_check(param_changed_sub, &new_data);
 
@@ -1397,9 +1414,8 @@ int commander_thread_main(int argc, char *argv[])
 			/* parameters changed */
 			orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
 
-
 			/* update parameters */
-			if (!current_status.flag_fmu_armed) {
+			if (!safety.armed) {
 				if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
 					warnx("failed setting new system type");
 				}
@@ -1417,7 +1433,6 @@ int commander_thread_main(int argc, char *argv[])
 				/* check and update system / component ID */
 				param_get(_param_system_id, &(current_status.system_id));
 				param_get(_param_component_id, &(current_status.component_id));
-
 			}
 		}
 
@@ -1564,7 +1579,7 @@ int commander_thread_main(int argc, char *argv[])
 				mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!");
 				current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
 				// XXX implement this
-				// state_machine_emergency(stat_pub, &current_status, mavlink_fd);
+				// state_machine_emergency(status_pub, &current_status, mavlink_fd);
 			}
 
 			critical_voltage_counter++;
@@ -1579,7 +1594,7 @@ 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 check for sensors
-			arming_state_transition(stat_pub, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
+			arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
 		} else {
 			// XXX: Add emergency stuff if sensors are lost
 		}
@@ -1694,7 +1709,7 @@ int commander_thread_main(int argc, char *argv[])
 				&& (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk
 				&& !home_position_set
 				&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
-				&& !current_status.flag_fmu_armed) {
+				&& !safety.armed) {
 				warnx("setting home position");
 
 				/* copy position data to uORB home message, store it locally as well */
@@ -1734,24 +1749,22 @@ int commander_thread_main(int argc, char *argv[])
 
 					warnx("mode sw not finite");
 					/* no valid stick position, go to default */
+					current_status.mode_switch = MODE_SWITCH_MANUAL;
 
 				} else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) {
 
 					/* bottom stick position, go to manual mode */
 					current_status.mode_switch = MODE_SWITCH_MANUAL;
-					printf("mode switch: manual\n");
 
 				} else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) {
 
 					/* top stick position, set auto/mission for all vehicle types */
 					current_status.mode_switch = MODE_SWITCH_AUTO;
-					printf("mode switch: auto\n");
 
 				} else {
 
 					/* center stick position, set seatbelt/simple control */
 					current_status.mode_switch = MODE_SWITCH_SEATBELT;
-					printf("mode switch: seatbelt\n");
 				}
 
 				// warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
@@ -1811,7 +1824,7 @@ int commander_thread_main(int argc, char *argv[])
 						/* just manual, XXX this might depend on the return switch */
 						if (current_status.mode_switch == MODE_SWITCH_MANUAL) {
 							
-							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
+							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
 								// These is not supposed to happen
 								warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
 							}
@@ -1819,9 +1832,9 @@ int commander_thread_main(int argc, char *argv[])
 						/* Try seatbelt or fallback to manual */
 						} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) {
 			
-							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) {
+							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) {
 								// fallback to MANUAL_STANDBY
-								if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
+								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
 								// These is not supposed to happen
 								warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
 								}
@@ -1830,11 +1843,11 @@ int commander_thread_main(int argc, char *argv[])
 						/* Try auto or fallback to seatbelt or even manual */
 						} else if (current_status.mode_switch == MODE_SWITCH_AUTO) {
 							
-							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) {
+							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) {
 								// first fallback to SEATBELT_STANDY
-								if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) {
+								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) {
 									// or fallback to MANUAL_STANDBY
-									if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
+									if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) {
 									// These is not supposed to happen
 									warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
 									}
@@ -1850,7 +1863,7 @@ int commander_thread_main(int argc, char *argv[])
 						/* Always accept manual mode */
 						if (current_status.mode_switch == MODE_SWITCH_MANUAL) {
 
-							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
+							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
 								// These is not supposed to happen
 								warnx("ERROR: Navigation state MANUAL rejected");
 							}
@@ -1859,9 +1872,9 @@ int commander_thread_main(int argc, char *argv[])
 						} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT
 								&& current_status.return_switch == RETURN_SWITCH_NONE) {
 			
-							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
+							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
 								// fallback to MANUAL_STANDBY
-								if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
+								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
 								// These is not supposed to happen
 								warnx("ERROR: Navigation state MANUAL rejected");
 								}
@@ -1871,9 +1884,9 @@ int commander_thread_main(int argc, char *argv[])
 						} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT
 								&& current_status.return_switch == RETURN_SWITCH_RETURN) {
 			
-							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) {
+							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) {
 								// fallback to MANUAL_STANDBY
-								if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
+								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
 								// These is not supposed to happen
 								warnx("ERROR: Navigation state MANUAL rejected");
 								}
@@ -1885,19 +1898,19 @@ int commander_thread_main(int argc, char *argv[])
 								&& current_status.mission_switch == MISSION_SWITCH_NONE) {
 			
 							/* we might come from the disarmed state AUTO_STANDBY */
-							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) {
+							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) {
-								if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
+								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
 									// fallback to MANUAL_STANDBY
-									if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
+									if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
 									// These is not supposed to happen
 									warnx("ERROR: Navigation state MANUAL rejected");
 									}
 								}
 							/* or from some other armed state like SEATBELT or MANUAL */
-							} else if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) {
+							} else if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) {
-								if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
+								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
 									// fallback to MANUAL_STANDBY
-									if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
+									if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
 									// These is not supposed to happen
 									warnx("ERROR: Navigation state MANUAL rejected");
 									}
@@ -1909,10 +1922,10 @@ int commander_thread_main(int argc, char *argv[])
 								&& current_status.return_switch == RETURN_SWITCH_NONE
 								&& current_status.mission_switch == MISSION_SWITCH_MISSION) {
 							
-							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) {
+							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) {
-								if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
+								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) {
 									// fallback to MANUAL_STANDBY
-									if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
+									if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
 									// These is not supposed to happen
 									warnx("ERROR: Navigation state MANUAL rejected");
 									}
@@ -1924,10 +1937,10 @@ int commander_thread_main(int argc, char *argv[])
 								&& current_status.return_switch == RETURN_SWITCH_RETURN
 								&& (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) {
 			
-							if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) {
+							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) {
-								if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) {
+								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) {
 									// fallback to MANUAL_STANDBY
-									if (navigation_state_transition(stat_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
+									if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) {
 									// These is not supposed to happen
 									warnx("ERROR: Navigation state MANUAL rejected");
 									}
@@ -1975,13 +1988,22 @@ 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) ||
+				// printf("checking\n");
-					 (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
+				if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
-					 (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
-					) &&
-					(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, ARMING_STATE_STANDBY, mavlink_fd);
+						
+
+						printf("System Type: %d\n", current_status.system_type);
+
+						if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
+						 (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
+						 (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
+						) {
+							arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
+							
+						} else {
+							mavlink_log_critical(mavlink_fd, "STICK DISARM not allowed");
+						}
 						stick_off_counter = 0;
 
 					} else {
@@ -1993,7 +2015,7 @@ 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, ARMING_STATE_ARMED, mavlink_fd);
+						arming_state_transition(status_pub, &current_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd);
 						stick_on_counter = 0;
 
 					} else {
@@ -2095,13 +2117,13 @@ int commander_thread_main(int argc, char *argv[])
 
 				// XXX check if this is correct
 				/* arm / disarm on request */
-				if (sp_offboard.armed && !current_status.flag_fmu_armed) {
+				if (sp_offboard.armed && !safety.armed) {
 
-					arming_state_transition(stat_pub, &current_status, ARMING_STATE_ARMED, mavlink_fd);
+					arming_state_transition(status_pub, &current_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd);
 
-				} else if (!sp_offboard.armed && current_status.flag_fmu_armed) {
+				} else if (!sp_offboard.armed && safety.armed) {
 
-					arming_state_transition(stat_pub, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
+					arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
 				}
 
 			} else {
@@ -2143,13 +2165,13 @@ int commander_thread_main(int argc, char *argv[])
 		//     current_status.flag_preflight_mag_calibration == false &&
 		//     current_status.flag_preflight_accel_calibration == false) {
 		// 	/* All ok, no calibration going on, go to standby */
-		// 	do_state_update(stat_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
+		// 	do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
 		// }
 
 		/* publish at least with 1 Hz */
 		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) {
 
-			orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
+			orb_publish(ORB_ID(vehicle_status), status_pub, &current_status);
 			state_changed = false;
 		}
 

src/modules/commander/state_machine_helper.c

@@ -54,7 +54,7 @@
 
 #include "state_machine_helper.h"
 
-int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) {
+int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) {
 	
 	int ret = ERROR;
 
@@ -69,7 +69,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
 				/* allow going back from INIT for calibration */
 				if (current_state->arming_state == ARMING_STATE_STANDBY) {
 					ret = OK;
-					current_state->flag_fmu_armed = false;
+					safety->armed = false;
 				}
 				break;
 			case ARMING_STATE_STANDBY:
@@ -81,7 +81,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
 					/* sensors need to be initialized for STANDBY state */
 					if (current_state->condition_system_sensors_initialized) {
 						ret = OK;
-						current_state->flag_fmu_armed = false;
+						safety->armed = false;
 					} else {
 						mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized");
 					}
@@ -95,7 +95,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
 
 					/* XXX conditions for arming? */
 					ret = OK;
-					current_state->flag_fmu_armed = true;
+					safety->armed = true;
 				}
 				break;
 			case ARMING_STATE_ARMED_ERROR:
@@ -105,7 +105,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
 					
 					/* XXX conditions for an error state? */
 					ret = OK;
-					current_state->flag_fmu_armed = true;
+					safety->armed = true;
 				}
 				break;
 			case ARMING_STATE_STANDBY_ERROR:
@@ -114,7 +114,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
 				 || current_state->arming_state == ARMING_STATE_INIT
 				 || current_state->arming_state == ARMING_STATE_ARMED_ERROR) {
 					ret = OK;
-					current_state->flag_fmu_armed = false;
+					safety->armed = false;
 				}
 				break;
 			case ARMING_STATE_REBOOT:
@@ -125,7 +125,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
 				 || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) {
 
 					ret = OK;
-					current_state->flag_fmu_armed = false;
+					safety->armed = false;
 
 				}
 				break;
@@ -139,7 +139,12 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
 
 		if (ret == OK) {
 			current_state->arming_state = new_arming_state;
-			state_machine_publish(status_pub, current_state, mavlink_fd);
+			current_state->counter++;
+			current_state->timestamp = hrt_absolute_time();
+			orb_publish(ORB_ID(vehicle_status), status_pub, current_state);
+
+			safety->timestamp = hrt_absolute_time();
+			orb_publish(ORB_ID(actuator_safety), safety_pub, safety);
 		}
 	}
 
@@ -460,7 +465,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 
 		if (ret == OK) {
 			current_state->navigation_state = new_navigation_state;
-			state_machine_publish(status_pub, current_state, mavlink_fd);
+			current_state->counter++;
+			current_state->timestamp = hrt_absolute_time();
+			orb_publish(ORB_ID(vehicle_status), status_pub, current_state);
 		}
 	}
 

src/modules/commander/state_machine_helper.h

@@ -46,6 +46,7 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_safety.h>
 
 void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
 
@@ -53,7 +54,7 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st
 
 void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
 
-int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd);
+int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd);
 int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd);
 
 int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state);

src/modules/gpio_led/gpio_led.c

@@ -51,6 +51,7 @@
 #include <systemlib/err.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_safety.h>
 #include <poll.h>
 #include <drivers/drv_gpio.h>
 
@@ -60,6 +61,8 @@ struct gpio_led_s {
 	int pin;
 	struct vehicle_status_s status;
 	int vehicle_status_sub;
+	struct actuator_safety_s safety;
+	int actuator_safety_sub;
 	bool led_state;
 	int counter;
 };
@@ -168,11 +171,15 @@ void gpio_led_cycle(FAR void *arg)
 	if (status_updated)
 		orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
 
+	orb_check(priv->vehicle_status_sub, &status_updated);
+
+	if (status_updated)
+		orb_copy(ORB_ID(actuator_safety), priv->actuator_safety_sub, &priv->safety);
+
 	/* select pattern for current status */
 	int pattern = 0;
 
-	/* XXX fmu armed correct? */
+	if (priv->safety.armed) {
-	if (priv->status.flag_fmu_armed) {
 		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
 			pattern = 0x3f;	// ****** solid (armed)
 
@@ -181,11 +188,10 @@ void gpio_led_cycle(FAR void *arg)
 		}
 
 	} else {
-		if (priv->status.arming_state == ARMING_STATE_STANDBY) {
+		if (priv->safety.ready_to_arm) {
 			pattern = 0x00;	// ______ off (disarmed, preflight check)
 
-		} else if (priv->status.arming_state == ARMING_STATE_STANDBY &&
+		} else if (priv->safety.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
-			   priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
 			pattern = 0x38;	// ***___ slow blink (disarmed, ready)
 
 		} else {

src/modules/mavlink/orb_listener.c

@@ -71,7 +71,7 @@ struct vehicle_local_position_s local_pos;
 struct vehicle_status_s v_status;
 struct rc_channels_s rc;
 struct rc_input_values rc_raw;
-struct actuator_armed_s armed;
+struct actuator_safety_s safety;
 
 struct mavlink_subscriptions mavlink_subs;
 
@@ -109,7 +109,7 @@ static void	l_global_position_setpoint(const struct listener *l);
 static void	l_local_position_setpoint(const struct listener *l);
 static void	l_attitude_setpoint(const struct listener *l);
 static void	l_actuator_outputs(const struct listener *l);
-static void	l_actuator_armed(const struct listener *l);
+static void	l_actuator_safety(const struct listener *l);
 static void	l_manual_control_setpoint(const struct listener *l);
 static void	l_vehicle_attitude_controls(const struct listener *l);
 static void	l_debug_key_value(const struct listener *l);
@@ -133,7 +133,7 @@ static const struct listener listeners[] = {
 	{l_actuator_outputs,		&mavlink_subs.act_1_sub,	1},
 	{l_actuator_outputs,		&mavlink_subs.act_2_sub,	2},
 	{l_actuator_outputs,		&mavlink_subs.act_3_sub,	3},
-	{l_actuator_armed,		&mavlink_subs.armed_sub,	0},
+	{l_actuator_safety,		&mavlink_subs.safety_sub,	0},
 	{l_manual_control_setpoint,	&mavlink_subs.man_control_sp_sub, 0},
 	{l_vehicle_attitude_controls,	&mavlink_subs.actuators_sub,	0},
 	{l_debug_key_value,		&mavlink_subs.debug_key_value,	0},
@@ -269,7 +269,7 @@ l_vehicle_status(const struct listener *l)
 {
 	/* immediately communicate state changes back to user */
 	orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
-	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
+	orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety);
 
 	/* enable or disable HIL */
 	set_hil_on_off(v_status.flag_hil_enabled);
@@ -466,7 +466,7 @@ l_actuator_outputs(const struct listener *l)
 						  act_outputs.output[7]);
 
 		/* only send in HIL mode */
-		if (mavlink_hil_enabled && armed.armed) {
+		if (mavlink_hil_enabled && safety.armed) {
 
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
@@ -538,9 +538,9 @@ l_actuator_outputs(const struct listener *l)
 }
 
 void
-l_actuator_armed(const struct listener *l)
+l_actuator_safety(const struct listener *l)
 {
-	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
+	orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety);
 }
 
 void
@@ -745,8 +745,8 @@ uorb_receive_start(void)
 	orb_set_interval(mavlink_subs.act_3_sub, 100);	/* 10Hz updates */
 
 	/* --- ACTUATOR ARMED VALUE --- */
-	mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	mavlink_subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety));
-	orb_set_interval(mavlink_subs.armed_sub, 100);	/* 10Hz updates */
+	orb_set_interval(mavlink_subs.safety_sub, 100);	/* 10Hz updates */
 
 	/* --- MAPPED MANUAL CONTROL INPUTS --- */
 	mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));

src/modules/mavlink/orb_topics.h

@@ -58,6 +58,7 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_safety.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/debug_key_value.h>
 #include <drivers/drv_rc_input.h>
@@ -72,7 +73,7 @@ struct mavlink_subscriptions {
 	int act_3_sub;
 	int gps_sub;
 	int man_control_sp_sub;
-	int armed_sub;
+	int safety_sub;
 	int actuators_sub;
 	int local_pos_sub;
 	int spa_sub;

src/modules/mavlink_onboard/mavlink.c

@@ -274,7 +274,7 @@ void mavlink_update_system(void)
 }
 
 void
-get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
+get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety,
 	uint8_t *mavlink_state, uint8_t *mavlink_mode)
 {
 	/* reset MAVLink mode bitfield */
@@ -290,7 +290,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct
 	}
 
 	/* set arming state */
-	if (v_status->flag_fmu_armed) {
+	if (safety->armed) {
 		*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 	} else {
 		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
@@ -369,7 +369,7 @@ int mavlink_thread_main(int argc, char *argv[])
 	baudrate = 57600;
 
 	struct vehicle_status_s v_status;
-	struct actuator_armed_s armed;
+	struct actuator_safety_s safety;
 
 	/* work around some stupidity in task_create's argv handling */
 	argc -= 2;
@@ -437,7 +437,7 @@ int mavlink_thread_main(int argc, char *argv[])
 			/* translate the current system state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
 			uint8_t mavlink_mode = 0;
-			get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
+			get_mavlink_mode_and_state(&v_status, &safety, &mavlink_state, &mavlink_mode);
 
 			/* send heartbeat */
 			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state);

src/modules/mavlink_onboard/orb_topics.h

@@ -55,6 +55,7 @@
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_safety.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/debug_key_value.h>
 #include <drivers/drv_rc_input.h>
@@ -69,7 +70,7 @@ struct mavlink_subscriptions {
 	int act_3_sub;
 	int gps_sub;
 	int man_control_sp_sub;
-	int armed_sub;
+	int safety_sub;
 	int actuators_sub;
 	int local_pos_sub;
 	int spa_sub;

src/modules/mavlink_onboard/util.h

@@ -50,5 +50,5 @@ extern volatile bool thread_should_exit;
 /**
  * Translate the custom state into standard mavlink modes and state.
  */
-extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
+extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety,
 	uint8_t *mavlink_state, uint8_t *mavlink_mode);

src/modules/multirotor_att_control/multirotor_att_control_main.c

@@ -58,6 +58,7 @@
 #include <uORB/uORB.h>
 #include <drivers/drv_gyro.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_safety.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
@@ -84,13 +85,16 @@ static bool motor_test_mode = false;
 
 static orb_advert_t actuator_pub;
 
-static struct vehicle_status_s state;
+
 
 static int
 mc_thread_main(int argc, char *argv[])
 {
 	/* declare and safely initialize all structs */
+	struct vehicle_status_s state;
 	memset(&state, 0, sizeof(state));
+	struct actuator_safety_s safety;
+	memset(&safety, 0, sizeof(safety));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 	struct vehicle_attitude_setpoint_s att_sp;
@@ -113,6 +117,7 @@ mc_thread_main(int argc, char *argv[])
 	int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
 	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 
@@ -150,7 +155,7 @@ mc_thread_main(int argc, char *argv[])
 	/* store last control mode to detect mode switches */
 	bool flag_control_manual_enabled = false;
 	bool flag_control_attitude_enabled = false;
-	bool flag_fmu_armed = false;
+	bool flag_armed = false;
 	bool flag_control_position_enabled = false;
 	bool flag_control_velocity_enabled = false;
 
@@ -200,6 +205,12 @@ mc_thread_main(int argc, char *argv[])
 					orb_copy(ORB_ID(vehicle_status), state_sub, &state);
 				}
 
+				orb_check(safety_sub, &updated);
+
+				if (updated) {
+					orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
+				}
+
 				/* get a local copy of manual setpoint */
 				orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
 				/* get a local copy of attitude */
@@ -248,7 +259,7 @@ mc_thread_main(int argc, char *argv[])
 						/* initialize to current yaw if switching to manual or att control */
 						if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
 						    state.flag_control_manual_enabled != flag_control_manual_enabled ||
-						    state.flag_fmu_armed != flag_fmu_armed) {
+						    safety.armed != flag_armed) {
 							att_sp.yaw_body = att.yaw;
 						}
 
@@ -292,7 +303,7 @@ mc_thread_main(int argc, char *argv[])
 							att_sp.pitch_body = manual.pitch;
 
 							/* set attitude if arming */
-							if (!flag_control_attitude_enabled && state.flag_fmu_armed) {
+							if (!flag_control_attitude_enabled && safety.armed) {
 								att_sp.yaw_body = att.yaw;
 							}
 
@@ -399,7 +410,7 @@ mc_thread_main(int argc, char *argv[])
 				flag_control_manual_enabled = state.flag_control_manual_enabled;
 				flag_control_position_enabled = state.flag_control_position_enabled;
 				flag_control_velocity_enabled = state.flag_control_velocity_enabled;
-				flag_fmu_armed = state.flag_fmu_armed;
+				flag_armed = safety.armed;
 
 				perf_end(mc_loop_perf);
 			} /* end of poll call for attitude updates */

src/modules/sdlog2/sdlog2.c

@@ -60,6 +60,7 @@
 #include <drivers/drv_hrt.h>
 
 #include <uORB/uORB.h>
+#include <uORB/topics/actuator_safety.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -190,7 +191,7 @@ static int file_copy(const char *file_old, const char *file_new);
 
 static void handle_command(struct vehicle_command_s *cmd);
 
-static void handle_status(struct vehicle_status_s *cmd);
+static void handle_status(struct actuator_safety_s *safety);
 
 /**
  * Create folder for current logging session. Store folder name in 'log_folder'.
@@ -623,6 +624,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	/* warning! using union here to save memory, elements should be used separately! */
 	union {
+		struct actuator_safety_s safety;
 		struct vehicle_command_s cmd;
 		struct sensor_combined_s sensor;
 		struct vehicle_attitude_s att;
@@ -643,6 +645,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	memset(&buf, 0, sizeof(buf));
 
 	struct {
+		int safety_sub;
 		int cmd_sub;
 		int status_sub;
 		int sensor_sub;
@@ -690,6 +693,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- SAFETY STATUS --- */
+	subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety));
+	fds[fdsc_count].fd = subs.safety_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* --- VEHICLE STATUS --- */
 	subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	fds[fdsc_count].fd = subs.status_sub;
@@ -826,6 +835,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			int ifds = 0;
 			int handled_topics = 0;
 
+
 			/* --- VEHICLE COMMAND - LOG MANAGEMENT --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
@@ -838,7 +848,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
 
 				if (log_when_armed) {
-					handle_status(&buf_status);
+					handle_status(&buf.safety);
 				}
 
 				handled_topics++;
@@ -870,7 +880,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_STAT.flight_mode = 0;
 				log_msg.body.log_STAT.manual_control_mode = 0;
 				log_msg.body.log_STAT.manual_sas_mode = 0;
-				log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_fmu_armed; /* XXX fmu armed correct? */
+				log_msg.body.log_STAT.armed = (unsigned char) buf.safety.armed; /* XXX fmu armed correct? */
 				log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery;
 				log_msg.body.log_STAT.battery_current = buf_status.current_battery;
 				log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
@@ -1168,11 +1178,10 @@ void handle_command(struct vehicle_command_s *cmd)
 	}
 }
 
-void handle_status(struct vehicle_status_s *status)
+void handle_status(struct actuator_safety_s *safety)
 {
-	/* XXX fmu armed correct? */
+	if (safety->armed != flag_system_armed) {
-	if (status->flag_fmu_armed != flag_system_armed) {
+		flag_system_armed = safety->armed;
-		flag_system_armed = status->flag_fmu_armed;
 
 		if (flag_system_armed) {
 			sdlog2_start_log();

src/modules/uORB/objects_common.cpp

@@ -137,7 +137,9 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
-ORB_DEFINE(actuator_armed, struct actuator_armed_s);
+
+#include "topics/actuator_safety.h"
+ORB_DEFINE(actuator_safety, struct actuator_safety_s);
 
 /* actuator controls, as set by actuators / mixers after limiting */
 #include "topics/actuator_controls_effective.h"

src/modules/uORB/topics/actuator_controls.h

@@ -52,6 +52,9 @@
 #define NUM_ACTUATOR_CONTROLS		8
 #define NUM_ACTUATOR_CONTROL_GROUPS	4	/**< for sanity checking */
 
+/* control sets with pre-defined applications */
+#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS	ORB_ID(actuator_controls_0)
+
 struct actuator_controls_s {
 	uint64_t timestamp;
 	float	control[NUM_ACTUATOR_CONTROLS];
@@ -63,16 +66,4 @@ ORB_DECLARE(actuator_controls_1);
 ORB_DECLARE(actuator_controls_2);
 ORB_DECLARE(actuator_controls_3);
 
-/* control sets with pre-defined applications */
-#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS	ORB_ID(actuator_controls_0)
-
-/** global 'actuator output is live' control. */
-struct actuator_armed_s {
-	bool	armed;		/**< Set to true if system is armed */
-	bool	ready_to_arm;	/**< Set to true if system is ready to be armed */
-	bool	lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
-};
-
-ORB_DECLARE(actuator_armed);
-
 #endif

src/modules/uORB/topics/actuator_safety.h

@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file actuator_controls.h
+ *
+ * Actuator control topics - mixer inputs.
+ *
+ * Values published to these topics are the outputs of the vehicle control
+ * system, and are expected to be mixed and used to drive the actuators
+ * (servos, speed controls, etc.) that operate the vehicle.
+ *
+ * Each topic can be published by a single controller
+ */
+
+#ifndef TOPIC_ACTUATOR_SAFETY_H
+#define TOPIC_ACTUATOR_SAFETY_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/** global 'actuator output is live' control. */
+struct actuator_safety_s {
+
+	uint64_t	timestamp;
+
+	bool	safety_off;	/**< Set to true if safety is off */
+	bool	armed;		/**< Set to true if system is armed */
+	bool	ready_to_arm;	/**< Set to true if system is ready to be armed */
+	bool	lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
+};
+
+ORB_DECLARE(actuator_safety);
+
+#endif

src/modules/uORB/topics/vehicle_control_mode.h

@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_control_mode.h
+ * Definition of the vehicle_control_mode uORB topic.
+ * 
+ * All control apps should depend their actions based on the flags set here.
+ */
+
+#ifndef VEHICLE_CONTROL_MODE
+#define VEHICLE_CONTROL_MODE
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics @{
+ */
+
+
+/**
+ * state machine / state of vehicle.
+ *
+ * Encodes the complete system state and is set by the commander app.
+ */
+struct vehicle_control_mode_s
+{
+	/* use of a counter and timestamp recommended (but not necessary) */
+
+	uint16_t counter;   /**< incremented by the writing thread everytime new data is stored */
+	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
+
+	bool flag_external_manual_override_ok;	/**< external override non-fatal for system. Only true for fixed wing */
+
+	bool flag_hil_enabled;				/**< true if hardware in the loop simulation is enabled */
+	bool flag_armed;				/**< true is motors / actuators are armed  */
+	bool flag_safety_off;				/**< true if safety is off */
+	bool flag_system_emergency;
+	bool flag_preflight_calibration;
+
+	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_auto_enabled;
+	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
+	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
+	bool flag_control_velocity_enabled;		/**< true if speed (implies direction) is controlled */
+	bool flag_control_position_enabled;		/**< true if position is controlled */
+
+	bool failsave_lowlevel;				/**< Set to true if low-level failsafe mode is enabled */
+	bool failsave_highlevel;
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_status);
+
+#endif

src/modules/uORB/topics/vehicle_status.h

@@ -199,8 +199,8 @@ struct vehicle_status_s
 	bool flag_external_manual_override_ok;	/**< external override non-fatal for system. Only true for fixed wing */
 
 	bool flag_hil_enabled;				/**< true if hardware in the loop simulation is enabled */
-	bool flag_fmu_armed;				/**< true is motors / actuators of FMU are armed  */
+	bool flag_armed;				/**< true is motors / actuators are armed  */
-	bool flag_io_armed;				/**< true is motors / actuators of IO are armed */
+	bool flag_safety_off;				/**< true if safety is off */
 	bool flag_system_emergency;
 	bool flag_preflight_calibration;
 
@@ -245,7 +245,6 @@ struct vehicle_status_s
 	uint16_t errors_count2;
 	uint16_t errors_count3;
 	uint16_t errors_count4;
-
 };
 
 /**