From 6b0d0aa2a5d2e9623ab2850396818679672715e9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 14 Jan 2015 11:27:32 +0100 Subject: ros: make mixer name a param --- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index e66558fe2..9954692bc 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -41,6 +41,7 @@ #include #include #include +#include class MultirotorMixer { @@ -118,7 +119,7 @@ const MultirotorMixer::Rotor *_config_index[4] = { MultirotorMixer::MultirotorMixer(): _n(), _rotor_count(4), - _rotors(_config_index[3]) //XXX hardcoded + _rotors(_config_index[0]) { _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this); _pub = _n.advertise("/mixed_motor_commands", 10); @@ -131,6 +132,10 @@ MultirotorMixer::MultirotorMixer(): _n.setParam("motor_offset_radps", 600.0); } + if (!_n.hasParam("mixer")) { + _n.setParam("mixer", "x"); + } + _sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback, this); } @@ -206,6 +211,19 @@ void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_contro inputs.control[i] = msg.control[i]; } + // switch mixer if necessary + std::string mixer_name; + _n.getParamCached("mixer", mixer_name); + if (mixer_name == "x") { + _rotors = _config_index[0]; + } else if (mixer_name == "+") { + _rotors = _config_index[1]; + } else if (mixer_name == "e") { + _rotors = _config_index[2]; + } else if (mixer_name == "w") { + _rotors = _config_index[3]; + } + // mix mix(); -- cgit v1.2.3