diff options
Diffstat (limited to 'src/platforms/ros/nodes')
-rw-r--r-- | src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp (renamed from src/platforms/ros/nodes/mc_mixer.cpp) | 61 |
1 files changed, 44 insertions, 17 deletions
diff --git a/src/platforms/ros/nodes/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index a131440a8..79eaf19e5 100644 --- a/src/platforms/ros/nodes/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -41,38 +41,67 @@ #include <px4.h> #include <lib/mathlib/mathlib.h> #include <mav_msgs/MotorSpeed.h> + + class MultirotorMixer { +public: - #define _rotor_count 4 + MultirotorMixer(); - struct Rotor { - float roll_scale; - float pitch_scale; - float yaw_scale; -}; + struct Rotor { + float roll_scale; + float pitch_scale; + float yaw_scale; + + }; + + void mix(); + void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg); + +private: + + ros::NodeHandle _n; + ros::Subscriber _sub; + ros::Publisher _pub; + + const Rotor *_rotors; + unsigned _rotor_count; - struct { - float control[8]; + struct { + float control[6]; }inputs; struct { - float control[8]; + float control[6]; }outputs; -const Rotor _config_x[] = { + +}; + + + +const MultirotorMixer::Rotor _config_x[] = { { 0.000000, -1.000000, -1.00 }, { -0.000000, 1.000000, -1.00 }, { 1.000000, 0.000000, 1.00 }, { -1.000000, 0.000000, 1.00 }, }; - const Rotor *_rotors = { &_config_x[0] + const MultirotorMixer::Rotor *_config_index = { &_config_x[0] }; +MultirotorMixer::MultirotorMixer(): + _rotor_count(4), + _rotors(_config_index) +{ + _sub = _n.subscribe("actuator_controls_0", 1000, &MultirotorMixer::actuatorControlsCallback,this); + _pub = _n.advertise<mav_msgs::MotorSpeed>("mixed_motor_commands",10); +} + -void mix() { +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); float yaw = math::constrain(inputs.control[2], -1.0f, 1.0f); @@ -133,7 +162,7 @@ void mix() { } } - void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg) + void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg) { // read message for(int i = 0;i < msg.NUM_ACTUATOR_CONTROLS;i++) { @@ -148,16 +177,14 @@ void mix() { for (int i = 0; i < _rotor_count; i++) { rotor_vel_msg.motor_speed.push_back(outputs.control[i]); } - pub.publish(rotor_vel_msg); + _pub.publish(rotor_vel_msg); } int main(int argc, char **argv) { ros::init(argc, argv, "mc_mixer"); - ros::NodeHandle n; - ros::Subscriber sub = n.subscribe("actuator_controls_0", 1000, actuatorControlsCallback); - ros::Publisher pub = n.advertise<mav_msgs::MotorSpeed>("mixed_motor_commands",10); + MultirotorMixer mixer; ros::spin(); |