diff options
Diffstat (limited to 'src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp')
-rw-r--r-- | src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 18 |
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; +} |