dummy commander and mixer: armed status

1 files changed, 17 insertions, 1 deletions

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;
+}