Renamed actuator_safety back to actuator_armed, compiling but untested

6 files changed, 110 insertions, 95 deletions

diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index 187820372..b88f61ce8 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -53,9 +53,10 @@
 #include <sys/prctl.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/safety.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_safety.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
 
 #include <systemlib/systemlib.h>
 
@@ -244,17 +245,15 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	int led_counter = 0;
 
 	/* declare and safely initialize all structs */
-	struct vehicle_status_s state;
-	memset(&state, 0, sizeof(state));
 	struct actuator_controls_s actuator_controls;
 	memset(&actuator_controls, 0, sizeof(actuator_controls));
-	struct actuator_safety_s safety;
-	safety.armed = false;
+	struct actuator_armed_s armed;
+	//XXX is this necessairy?
+	armed.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 safety_sub = orb_subscribe(ORB_ID(actuator_safety));
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
 	printf("[ardrone_interface] Motors initialized - ready.\n");
 	fflush(stdout);
@@ -325,12 +324,12 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 
 			/* 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_safety), safety_sub, &safety);
+			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
 			
 			/* for now only spin if armed and immediately shut down
 			 * if in failsafe
 			 */
-			if (safety.armed && !safety.lockdown) {
+			if (armed.armed && !armed.lockdown) {
 				ardrone_mixing_and_output(ardrone_write, &actuator_controls);
 
 			} else {
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index a11f88477..e83a87aa9 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -116,7 +116,8 @@
 #include <poll.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_safety.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
 static const float MAX_CELL_VOLTAGE	= 4.3f;
@@ -376,8 +377,9 @@ BlinkM::led()
 {
 
 	static int vehicle_status_sub_fd;
+	static int vehicle_control_mode_sub_fd;
 	static int vehicle_gps_position_sub_fd;
-	static int actuator_safety_sub_fd;
+	static int actuator_armed_sub_fd;
 
 	static int num_of_cells = 0;
 	static int detected_cells_runcount = 0;
@@ -388,7 +390,8 @@ 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_control_mode = 0;
+	static int no_data_actuator_armed = 0;
 	static int no_data_vehicle_gps_position = 0;
 
 	static bool topic_initialized = false;
@@ -401,8 +404,11 @@ 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_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
+		orb_set_interval(vehicle_control_mode_sub_fd, 1000);
+
+		actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
+		orb_set_interval(actuator_armed_sub_fd, 1000);
 
 		vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
 		orb_set_interval(vehicle_gps_position_sub_fd, 1000);
@@ -458,14 +464,16 @@ 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_control_mode_s vehicle_control_mode;
+		struct actuator_armed_s actuator_armed;
 		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_control_mode;
+		bool new_data_actuator_armed;
 		bool new_data_vehicle_gps_position;
 
 		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
@@ -479,15 +487,26 @@ BlinkM::led()
 				no_data_vehicle_status = 3;
 		}
 
-		orb_check(actuator_safety_sub_fd, &new_data_actuator_safety);
+		orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode);
 
-		if (new_data_actuator_safety) {
-			orb_copy(ORB_ID(actuator_safety), actuator_safety_sub_fd, &actuator_safety);
-			no_data_actuator_safety = 0;
+		if (new_data_vehicle_control_mode) {
+			orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode);
+			no_data_vehicle_control_mode = 0;
 		} else {
-			no_data_actuator_safety++;
-			if(no_data_vehicle_status >= 3)
-				no_data_vehicle_status = 3;
+			no_data_vehicle_control_mode++;
+			if(no_data_vehicle_control_mode >= 3)
+				no_data_vehicle_control_mode = 3;
+		}
+
+		orb_check(actuator_armed_sub_fd, &new_data_actuator_armed);
+
+		if (new_data_actuator_armed) {
+			orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed);
+			no_data_actuator_armed = 0;
+		} else {
+			no_data_actuator_armed++;
+			if(no_data_actuator_armed >= 3)
+				no_data_actuator_armed = 3;
 		}
 
 		orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
@@ -549,7 +568,7 @@ BlinkM::led()
 				} else {
 					/* no battery warnings here */
 
-					if(actuator_safety.armed == false) {
+					if(actuator_armed.armed == false) {
 						/* system not armed */
 						led_color_1 = LED_RED;
 						led_color_2 = LED_RED;
@@ -573,25 +592,21 @@ BlinkM::led()
 						led_color_8 = LED_OFF;
 						led_blink = LED_BLINK;
 
-						/* handle 4th led - flightmode indicator */
-#warning fix this
-//						switch((int)vehicle_status_raw.flight_mode) {
-//							case VEHICLE_FLIGHT_MODE_MANUAL:
-//								led_color_4 = LED_AMBER;
-//								break;
-//
-//							case VEHICLE_FLIGHT_MODE_STAB:
-//								led_color_4 = LED_YELLOW;
-//								break;
-//
-//							case VEHICLE_FLIGHT_MODE_HOLD:
-//								led_color_4 = LED_BLUE;
-//								break;
-//
-//							case VEHICLE_FLIGHT_MODE_AUTO:
-//								led_color_4 = LED_GREEN;
-//								break;
-//						}
+						if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
+
+							//XXX please check
+							if (vehicle_control_mode.flag_control_position_enabled)
+								led_color_4 = LED_GREEN;
+							else if (vehicle_control_mode.flag_control_velocity_enabled)
+								led_color_4 = LED_BLUE;
+							else if (vehicle_control_mode.flag_control_attitude_enabled)
+								led_color_4 = LED_YELLOW;
+							else if (vehicle_control_mode.flag_control_manual_enabled)
+								led_color_4 = LED_AMBER;
+							else 
+								led_color_4 = LED_OFF;
+								
+						}
 
 						if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
 							/* handling used sat�s */
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index 15fbf8c88..3e30e3292 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -75,7 +75,7 @@
 #include <systemlib/mixer/mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_safety.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
 
 #include <systemlib/err.h>
@@ -109,7 +109,7 @@ private:
 	int 		_current_update_rate;
 	int		_task;
 	int		_t_actuators;
-	int		_t_safety;
+	int		_t_armed;
 	orb_advert_t	_t_outputs;
 	unsigned	_num_outputs;
 	bool		_primary_pwm_device;
@@ -162,7 +162,7 @@ HIL::HIL() :
 	_current_update_rate(0),
 	_task(-1),
 	_t_actuators(-1),
-	_t_safety(-1),
+	_t_armed(-1),
 	_t_outputs(0),
 	_num_outputs(0),
 	_primary_pwm_device(false),
@@ -322,8 +322,8 @@ HIL::task_main()
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
-	_t_safety = orb_subscribe(ORB_ID(actuator_safety));
-	orb_set_interval(_t_safety, 200);		/* 5Hz update rate */
+	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
@@ -335,7 +335,7 @@ HIL::task_main()
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_safety;
+	fds[1].fd = _t_armed;
 	fds[1].events = POLLIN;
 
 	unsigned num_outputs;
@@ -427,15 +427,15 @@ HIL::task_main()
 
 		/* how about an arming update? */
 		if (fds[1].revents & POLLIN) {
-			actuator_safety_s aa;
+			actuator_armed_s aa;
 
 			/* get new value */
-			orb_copy(ORB_ID(actuator_safety), _t_safety, &aa);
+			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
 		}
 	}
 
 	::close(_t_actuators);
-	::close(_t_safety);
+	::close(_t_armed);
 
 	/* make sure servos are off */
 	// up_pwm_servo_deinit();
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 3fc1054e6..06930db38 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -74,7 +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 <uORB/topics/actuator_armed.h>
 #include <uORB/topics/esc_status.h>
 
 #include <systemlib/err.h>
@@ -135,7 +135,7 @@ private:
 	int 		_current_update_rate;
 	int		_task;
 	int		_t_actuators;
-	int		_t_actuator_safety;
+	int		_t_actuator_armed;
 	unsigned int		_motor;
 	int    _px4mode;
 	int    _frametype;
@@ -248,7 +248,7 @@ MK::MK(int bus) :
 	_update_rate(50),
 	_task(-1),
 	_t_actuators(-1),
-	_t_actuator_safety(-1),
+	_t_actuator_armed(-1),
 	_t_outputs(0),
 	_t_actuators_effective(0),
 	_t_esc_status(0),
@@ -513,8 +513,8 @@ MK::task_main()
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
-	_t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety));
-	orb_set_interval(_t_actuator_safety, 200);		/* 5Hz update rate */
+	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_actuator_armed, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
@@ -540,7 +540,7 @@ MK::task_main()
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_actuator_safety;
+	fds[1].fd = _t_actuator_armed;
 	fds[1].events = POLLIN;
 
 	log("starting");
@@ -651,13 +651,13 @@ MK::task_main()
 
 		/* how about an arming update? */
 		if (fds[1].revents & POLLIN) {
-			actuator_safety_s as;
+			actuator_armed_s aa;
 
 			/* get new value */
-			orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as);
+			orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
 
 			/* update PWM servo armed status if armed and not locked down */
-			mk_servo_arm(as.armed && !as.lockdown);
+			mk_servo_arm(aa.armed && !aa.lockdown);
 		}
 
 
@@ -700,7 +700,7 @@ MK::task_main()
 	//::close(_t_esc_status);
 	::close(_t_actuators);
 	::close(_t_actuators_effective);
-	::close(_t_actuator_safety);
+	::close(_t_actuator_armed);
 
 
 	/* make sure servos are off */
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index d0499d2fd..41c8c8bb7 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -69,7 +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 <uORB/topics/actuator_armed.h>
 
 #include <systemlib/err.h>
 #include <systemlib/ppm_decode.h>
@@ -105,7 +105,7 @@ private:
 	unsigned	_current_update_rate;
 	int		_task;
 	int		_t_actuators;
-	int		_t_actuator_safety;
+	int		_t_actuator_armed;
 	orb_advert_t	_t_outputs;
 	orb_advert_t	_t_actuators_effective;
 	unsigned	_num_outputs;
@@ -175,7 +175,7 @@ PX4FMU::PX4FMU() :
 	_current_update_rate(0),
 	_task(-1),
 	_t_actuators(-1),
-	_t_actuator_safety(-1),
+	_t_actuator_armed(-1),
 	_t_outputs(0),
 	_t_actuators_effective(0),
 	_num_outputs(0),
@@ -377,8 +377,8 @@ PX4FMU::task_main()
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
-	_t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety));
-	orb_set_interval(_t_actuator_safety, 200);		/* 5Hz update rate */
+	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_actuator_armed, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
@@ -397,7 +397,7 @@ PX4FMU::task_main()
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_actuator_safety;
+	fds[1].fd = _t_actuator_armed;
 	fds[1].events = POLLIN;
 
 	unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
@@ -500,13 +500,13 @@ PX4FMU::task_main()
 
 		/* how about an arming update? */
 		if (fds[1].revents & POLLIN) {
-			actuator_safety_s as;
+			actuator_armed_s aa;
 
 			/* get new value */
-			orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as);
+			orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
 
 			/* update PWM servo armed status if armed and not locked down */
-			bool set_armed = as.armed && !as.lockdown;
+			bool set_armed = aa.armed && !aa.lockdown;
 			if (set_armed != _armed) {
 				_armed = set_armed;
 				up_pwm_servo_arm(set_armed);
@@ -536,7 +536,7 @@ PX4FMU::task_main()
 
 	::close(_t_actuators);
 	::close(_t_actuators_effective);
-	::close(_t_actuator_safety);
+	::close(_t_actuator_armed);
 
 	/* make sure servos are off */
 	up_pwm_servo_deinit();
@@ -1022,12 +1022,12 @@ fake(int argc, char *argv[])
 	if (handle < 0)
 		errx(1, "advertise failed");
 
-	actuator_safety_s as;
+	actuator_armed_s aa;
 
-	as.armed = true;
-	as.lockdown = false;
+	aa.armed = true;
+	aa.lockdown = false;
 
-	handle = orb_advertise(ORB_ID(actuator_safety), &as);
+	handle = orb_advertise(ORB_ID(actuator_armed), &aa);
 
 	if (handle < 0)
 		errx(1, "advertise failed 2");
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 94f231821..ea732eccd 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -75,7 +75,8 @@
 #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/actuator_armed.h>
+#include <uORB/topics/safety.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/rc_channels.h>
@@ -178,7 +179,7 @@ private:
 
 	/* subscribed topics */
 	int			_t_actuators;	///< actuator controls topic
-	int			_t_actuator_safety;	///< system armed control topic
+	int			_t_actuator_armed;	///< system armed control topic
 	int 			_t_vstatus;	///< system / vehicle status
 	int			_t_param;	///< parameter update topic
 
@@ -360,7 +361,7 @@ PX4IO::PX4IO() :
 	_status(0),
 	_alarms(0),
 	_t_actuators(-1),
-	_t_actuator_safety(-1),
+	_t_actuator_armed(-1),
 	_t_vstatus(-1),
 	_t_param(-1),
 	_to_input_rc(0),
@@ -505,9 +506,9 @@ PX4IO::init()
 		 * remains untouched (so manual override is still available).
 		 */
 
-		int safety_sub = orb_subscribe(ORB_ID(actuator_safety));
+		int safety_sub = orb_subscribe(ORB_ID(actuator_armed));
 		/* fill with initial values, clear updated flag */
-		struct actuator_safety_s safety;
+		struct actuator_armed_s safety;
 		uint64_t try_start_time = hrt_absolute_time();
 		bool updated = false;
 		
@@ -518,7 +519,7 @@ PX4IO::init()
 
 			if (updated) {
 				/* got data, copy and exit loop */
-				orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
+				orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
 				break;
 			}
 
@@ -559,7 +560,7 @@ PX4IO::init()
 			orb_check(safety_sub, &updated);
 
 			if (updated) {
-				orb_copy(ORB_ID(actuator_safety), safety_sub, &safety);
+				orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
 			}
 
 			/* wait 50 ms */
@@ -643,8 +644,8 @@ PX4IO::task_main()
 				     ORB_ID(actuator_controls_1));
 	orb_set_interval(_t_actuators, 20);		/* default to 50Hz */
 
-	_t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety));
-	orb_set_interval(_t_actuator_safety, 200);		/* 5Hz update rate */
+	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_t_actuator_armed, 200);		/* 5Hz update rate */
 
 	_t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
 	orb_set_interval(_t_vstatus, 200);		/* 5Hz update rate max. */
@@ -656,7 +657,7 @@ PX4IO::task_main()
 	pollfd fds[4];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
-	fds[1].fd = _t_actuator_safety;
+	fds[1].fd = _t_actuator_armed;
 	fds[1].events = POLLIN;
 	fds[2].fd = _t_vstatus;
 	fds[2].events = POLLIN;
@@ -832,21 +833,21 @@ PX4IO::set_idle_values(const uint16_t *vals, unsigned len)
 int
 PX4IO::io_set_arming_state()
 {
-	actuator_safety_s	safety;		///< system armed state
+	actuator_armed_s	armed;		///< system armed state
 	vehicle_status_s	vstatus;	///< overall system state
 
-	orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety);
+	orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
 	orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus);
 
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
-	if (safety.armed && !safety.lockdown) {
+	if (armed.armed && !armed.lockdown) {
 		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	}
-	if (safety.ready_to_arm) {
+	if (armed.ready_to_arm) {
 		set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
@@ -1004,10 +1005,10 @@ PX4IO::io_handle_status(uint16_t status)
 	/**
 	 * Get and handle the safety status
 	 */
-	struct actuator_safety_s safety;
+	struct safety_s safety;
 	safety.timestamp = hrt_absolute_time();
 
-	orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety);
+	orb_copy(ORB_ID(safety), _to_safety, &safety);
 
 	if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
 		safety.safety_off = true;
@@ -1019,9 +1020,9 @@ PX4IO::io_handle_status(uint16_t status)
 
 	/* lazily publish the safety status */
 	if (_to_safety > 0) {
-		orb_publish(ORB_ID(actuator_safety), _to_safety, &safety);
+		orb_publish(ORB_ID(safety), _to_safety, &safety);
 	} else {
-		_to_safety = orb_advertise(ORB_ID(actuator_safety), &safety);
+		_to_safety = orb_advertise(ORB_ID(safety), &safety);
 	}
 
 	return ret;