diff options
author | Roman Bapst <romanbapst@yahoo.de> | 2014-12-30 11:41:28 +0100 |
---|---|---|
committer | Roman Bapst <romanbapst@yahoo.de> | 2014-12-30 11:41:28 +0100 |
commit | bfc398442663a943ff1e0fa21ec50b9960abb5a7 (patch) | |
tree | 67e3c6288ce5f5364b5b2d5918147301bc24d07b /src/platforms/ros/nodes | |
parent | fa1f09b850f754ff3e0da7f4c5e56e3ee58fce11 (diff) | |
download | px4-firmware-bfc398442663a943ff1e0fa21ec50b9960abb5a7.tar.gz px4-firmware-bfc398442663a943ff1e0fa21ec50b9960abb5a7.tar.bz2 px4-firmware-bfc398442663a943ff1e0fa21ec50b9960abb5a7.zip |
cleanup
Diffstat (limited to 'src/platforms/ros/nodes')
-rw-r--r-- | src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 38 |
1 files changed, 15 insertions, 23 deletions
diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 79eaf19e5..7092a8017 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -41,20 +41,18 @@ #include <px4.h> #include <lib/mathlib/mathlib.h> #include <mav_msgs/MotorSpeed.h> - + class MultirotorMixer { public: MultirotorMixer(); struct Rotor { - float roll_scale; - float pitch_scale; - float yaw_scale; - - }; + float roll_scale; + float pitch_scale; + float yaw_scale; + }; - void mix(); void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg); private: @@ -62,7 +60,7 @@ private: ros::NodeHandle _n; ros::Subscriber _sub; ros::Publisher _pub; - + const Rotor *_rotors; unsigned _rotor_count; @@ -75,21 +73,18 @@ private: float control[6]; }outputs; + void mix(); - }; - - const MultirotorMixer::Rotor _config_x[] = { - { 0.000000, -1.000000, -1.00 }, - { -0.000000, 1.000000, -1.00 }, + { 0.000000, -1.000000, -1.00 }, + { -0.000000, 1.000000, -1.00 }, { 1.000000, 0.000000, 1.00 }, - { -1.000000, 0.000000, 1.00 }, + { -1.000000, 0.000000, 1.00 }, }; const MultirotorMixer::Rotor *_config_index = { &_config_x[0] - }; MultirotorMixer::MultirotorMixer(): @@ -97,10 +92,9 @@ MultirotorMixer::MultirotorMixer(): _rotors(_config_index) { _sub = _n.subscribe("actuator_controls_0", 1000, &MultirotorMixer::actuatorControlsCallback,this); - _pub = _n.advertise<mav_msgs::MotorSpeed>("mixed_motor_commands",10); + _pub = _n.advertise<mav_msgs::MotorSpeed>("mixed_motor_commands",10); } - void MultirotorMixer::mix() { float roll = math::constrain(inputs.control[0], -1.0f, 1.0f); float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f); @@ -164,28 +158,26 @@ void MultirotorMixer::mix() { void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg) { - // read message + // read message for(int i = 0;i < msg.NUM_ACTUATOR_CONTROLS;i++) { inputs.control[i] = msg.control[i]; } - + // mix mix(); // publish message mav_msgs::MotorSpeed rotor_vel_msg; for (int i = 0; i < _rotor_count; i++) { - rotor_vel_msg.motor_speed.push_back(outputs.control[i]); + rotor_vel_msg.motor_speed.push_back(outputs.control[i]); } _pub.publish(rotor_vel_msg); - } int main(int argc, char **argv) { ros::init(argc, argv, "mc_mixer"); - MultirotorMixer mixer; - + MultirotorMixer mixer; ros::spin(); return 0; |