diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-05 13:09:32 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-05 13:09:32 +0100 |
commit | 17e544ebf3c65338db891edc3615e6fd942f6ab4 (patch) | |
tree | edc66bb7a738e59e05dfc937540b0830d97400a1 /src/platforms/ros/nodes/mc_mixer | |
parent | ef485177acb123483fe42d49b3ab23671ee3aa31 (diff) | |
download | px4-firmware-17e544ebf3c65338db891edc3615e6fd942f6ab4.tar.gz px4-firmware-17e544ebf3c65338db891edc3615e6fd942f6ab4.tar.bz2 px4-firmware-17e544ebf3c65338db891edc3615e6fd942f6ab4.zip |
dummy mixer: add offset param
Diffstat (limited to 'src/platforms/ros/nodes/mc_mixer')
-rw-r--r-- | src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 25 |
1 files changed, 15 insertions, 10 deletions
diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 4dc428a27..5db180052 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -115,7 +115,10 @@ MultirotorMixer::MultirotorMixer(): _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback,this); _pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands",10); if (!_n.hasParam("motor_scaling_radps")) { - _n.setParam("motor_scaling_radps", 1500.0); + _n.setParam("motor_scaling_radps", 150.0); + } + if (!_n.hasParam("motor_offset_radps")) { + _n.setParam("motor_offset_radps", 600.0); } _sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback,this); } @@ -189,20 +192,22 @@ void MultirotorMixer::mix() { } // mix - if (_armed) { - mix(); - } else { - for (unsigned i = 0; i < _rotor_count; i++) { - outputs.control[i] = 0.0f; - } - } + mix(); // publish message mav_msgs::MotorSpeed rotor_vel_msg; double scaling; + double offset; _n.getParamCached("motor_scaling_radps", scaling); - for (int i = 0; i < _rotor_count; i++) { - rotor_vel_msg.motor_speed.push_back(outputs.control[i] * scaling); + _n.getParamCached("motor_offset_radps", offset); + if (_armed) { + for (int i = 0; i < _rotor_count; i++) { + rotor_vel_msg.motor_speed.push_back(outputs.control[i] * scaling + offset); + } + } else { + for (int i = 0; i < _rotor_count; i++) { + rotor_vel_msg.motor_speed.push_back(0.0); + } } _pub.publish(rotor_vel_msg); } |