diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch index 8091e3ff2b..00ef7ab459 100644 --- a/launch/gazebo_ardrone_empty_world.launch +++ b/launch/gazebo_ardrone_empty_world.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/gazebo_ardrone_house_world.launch b/launch/gazebo_ardrone_house_world.launch index 7b6be75acb..f9ac1cac52 100644 --- a/launch/gazebo_ardrone_house_world.launch +++ b/launch/gazebo_ardrone_house_world.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/gazebo_iris_empty_world.launch b/launch/gazebo_iris_empty_world.launch index 20574e00d0..22e0329783 100644 --- a/launch/gazebo_iris_empty_world.launch +++ b/launch/gazebo_iris_empty_world.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/gazebo_iris_outdoor_world.launch b/launch/gazebo_iris_outdoor_world.launch index e7fd0b3953..88960b5ec5 100644 --- a/launch/gazebo_iris_outdoor_world.launch +++ b/launch/gazebo_iris_outdoor_world.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 0a4b8c26df..79c3c36d97 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -19,6 +19,7 @@ + diff --git a/launch/multicopter_w.launch b/launch/multicopter_w.launch new file mode 100644 index 0000000000..f124082aa9 --- /dev/null +++ b/launch/multicopter_w.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/launch/multicopter_x.launch b/launch/multicopter_x.launch new file mode 100644 index 0000000000..6355b87be9 --- /dev/null +++ b/launch/multicopter_x.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index e66558fe2e..9954692bc1 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();