diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-02 16:11:16 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-02 16:11:28 +0100 |
commit | 530c38b6fa3bc0294d66801a0d7a36e8888e903f (patch) | |
tree | b3c49a3b87e137c9d2e40208178f235cca7d6eeb /src/platforms/ros/nodes/mc_mixer | |
parent | 3932013777c232c8c91e34144f54f6fd2f635175 (diff) | |
download | px4-firmware-530c38b6fa3bc0294d66801a0d7a36e8888e903f.tar.gz px4-firmware-530c38b6fa3bc0294d66801a0d7a36e8888e903f.tar.bz2 px4-firmware-530c38b6fa3bc0294d66801a0d7a36e8888e903f.zip |
dummy commander and mixer: armed status
Diffstat (limited to 'src/platforms/ros/nodes/mc_mixer')
-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; +} |