dummy commander and mixer: armed status

3 files changed, 36 insertions, 8 deletions

diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp
index 1972d8cfa..d8ff739b9 100644
--- a/src/platforms/ros/nodes/commander/commander.cpp
+++ b/src/platforms/ros/nodes/commander/commander.cpp
@@ -42,7 +42,6 @@
 
 #include <px4/manual_control_setpoint.h>
 #include <px4/vehicle_control_mode.h>
-#include <px4/actuator_armed.h>
 #include <px4/vehicle_status.h>
 #include <platforms/px4_middleware.h>
 
@@ -53,14 +52,14 @@ Commander::Commander() :
 	_actuator_armed_pub(_n.advertise<px4::actuator_armed>("actuator_armed", 10)),
 	_vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10)),
 	_parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)),
-	_msg_parameter_update()
+	_msg_parameter_update(),
+	_msg_actuator_armed()
 {
 }
 
 void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr& msg)
 {
 	px4::vehicle_control_mode msg_vehicle_control_mode;
-	px4::actuator_armed msg_actuator_armed;
 	px4::vehicle_status msg_vehicle_status;
 
 	/* fill vehicle control mode */
@@ -71,9 +70,19 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 	msg_vehicle_control_mode.flag_control_attitude_enabled = true;
 
 	/* fill actuator armed */
-	//XXX hardcoded
-	msg_actuator_armed.timestamp = px4::get_time_micros();
-	msg_actuator_armed.armed = true;
+	float arm_th = 0.95;
+	_msg_actuator_armed.timestamp = px4::get_time_micros();
+	if (_msg_actuator_armed.armed) {
+		/* Check for disarm */
+		if (msg->r < -arm_th && msg->z < (1-arm_th)) {
+			_msg_actuator_armed.armed = false;
+		}
+	} else {
+		/* Check for arm */
+		if (msg->r > arm_th && msg->z < (1-arm_th)) {
+			_msg_actuator_armed.armed = true;
+		}
+	}
 
 	/* fill vehicle status */
 	//XXX hardcoded
@@ -83,9 +92,10 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 	msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED;
 	msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF;
 	msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR;
+	msg_vehicle_status.is_rotary_wing = true;
 
 	_vehicle_control_mode_pub.publish(msg_vehicle_control_mode);
-	_actuator_armed_pub.publish(msg_actuator_armed);
+	_actuator_armed_pub.publish(_msg_actuator_armed);
 	_vehicle_status_pub.publish(msg_vehicle_status);
 
 	/* Fill parameter update */
diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h
index cd9be5135..f3c2f6f1a 100644
--- a/src/platforms/ros/nodes/commander/commander.h
+++ b/src/platforms/ros/nodes/commander/commander.h
@@ -41,6 +41,7 @@
 #include "ros/ros.h"
 #include <px4/manual_control_setpoint.h>
 #include <px4/parameter_update.h>
+#include <px4/actuator_armed.h>
 
 class Commander {
 public:
@@ -62,5 +63,6 @@ protected:
 	ros::Publisher _parameter_update_pub;
 
 	px4::parameter_update _msg_parameter_update;
+	px4::actuator_armed _msg_actuator_armed;
 
 };
diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
index e8c65905f..4dc428a27 100644
--- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -54,6 +54,7 @@ public:
 	};
 
 	void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg);
+	void actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg);
 
 private:
 
@@ -73,6 +74,9 @@ private:
 		float control[6];
 	}outputs;
 
+	bool _armed;
+	ros::Subscriber _sub_actuator_armed;
+
 	void mix();
 };
 
@@ -113,6 +117,7 @@ MultirotorMixer::MultirotorMixer():
 	if (!_n.hasParam("motor_scaling_radps")) {
 		_n.setParam("motor_scaling_radps", 1500.0);
 	}
+	_sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback,this);
 }
 
 void MultirotorMixer::mix() {
@@ -184,7 +189,13 @@ void MultirotorMixer::mix() {
 	}
 
 	// mix
-	mix();
+	if (_armed) {
+		mix();
+	} else {
+		for (unsigned i = 0; i < _rotor_count; i++) {
+			outputs.control[i] = 0.0f;
+		}
+	}
 
 	// publish message
 	mav_msgs::MotorSpeed rotor_vel_msg;
@@ -204,3 +215,8 @@ void MultirotorMixer::mix() {
 
   return 0;
 }
+
+void MultirotorMixer::actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg)
+{
+	_armed = msg.armed;
+}