Introduced new actuator_safety topic

1 files changed, 21 insertions, 20 deletions

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 962a91c7f..28a3eb917 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/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_component = status.component_id;
-		cmd.source_system = status.system_id;
-		cmd.source_component = status.component_id;
+		// cmd.target_system = status.system_id;
+		// cmd.target_component = status.component_id;
+		// cmd.source_system = status.system_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));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+	_t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety));
+	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;