diff options
| -rw-r--r-- | launch/gazebo_ardrone_empty_world.launch | 2 | ||||
| -rw-r--r-- | launch/gazebo_ardrone_house_world.launch | 2 | ||||
| -rw-r--r-- | launch/gazebo_iris_empty_world.launch | 2 | ||||
| -rw-r--r-- | launch/gazebo_iris_outdoor_world.launch | 2 | ||||
| -rw-r--r-- | launch/multicopter.launch | 1 | ||||
| -rw-r--r-- | launch/multicopter_w.launch | 9 | ||||
| -rw-r--r-- | launch/multicopter_x.launch | 9 | ||||
| -rw-r--r-- | src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 20 |
8 files changed, 42 insertions, 5 deletions
diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch index 8091e3ff2..00ef7ab45 100644 --- a/launch/gazebo_ardrone_empty_world.launch +++ b/launch/gazebo_ardrone_empty_world.launch @@ -1,6 +1,6 @@ <launch> <include file="$(find mav_gazebo)/launch/ardrone_empty_world_with_joy.launch" /> -<include file="$(find px4)/launch/multicopter.launch" /> +<include file="$(find px4)/launch/multicopter_x.launch" /> </launch> diff --git a/launch/gazebo_ardrone_house_world.launch b/launch/gazebo_ardrone_house_world.launch index 7b6be75ac..f9ac1cac5 100644 --- a/launch/gazebo_ardrone_house_world.launch +++ b/launch/gazebo_ardrone_house_world.launch @@ -1,6 +1,6 @@ <launch> <include file="$(find mav_gazebo)/launch/ardrone_house_world_with_joy.launch" /> -<include file="$(find px4)/launch/multicopter.launch" /> +<include file="$(find px4)/launch/multicopter_x.launch" /> </launch> diff --git a/launch/gazebo_iris_empty_world.launch b/launch/gazebo_iris_empty_world.launch index 20574e00d..22e032978 100644 --- a/launch/gazebo_iris_empty_world.launch +++ b/launch/gazebo_iris_empty_world.launch @@ -1,6 +1,6 @@ <launch> <include file="$(find mav_gazebo)/launch/iris_empty_world_with_joy.launch" /> -<include file="$(find px4)/launch/multicopter.launch" /> +<include file="$(find px4)/launch/multicopter_w.launch" /> </launch> diff --git a/launch/gazebo_iris_outdoor_world.launch b/launch/gazebo_iris_outdoor_world.launch index e7fd0b395..88960b5ec 100644 --- a/launch/gazebo_iris_outdoor_world.launch +++ b/launch/gazebo_iris_outdoor_world.launch @@ -1,6 +1,6 @@ <launch> <include file="$(find mav_gazebo)/launch/iris_outdoor_world_with_joy.launch" /> -<include file="$(find px4)/launch/multicopter.launch" /> +<include file="$(find px4)/launch/multicopter_x.launch" /> </launch> diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 0a4b8c26d..79c3c36d9 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -19,6 +19,7 @@ <param name="MC_YAWRATE_P" type="double" value="0.5" /> <param name="MC_MAN_R_MAX" type="double" value="10.0" /> <param name="MC_MAN_P_MAX" type="double" value="10.0" /> + <param name="mixer" type="string" value="x" /> </group> </launch> diff --git a/launch/multicopter_w.launch b/launch/multicopter_w.launch new file mode 100644 index 000000000..f124082aa --- /dev/null +++ b/launch/multicopter_w.launch @@ -0,0 +1,9 @@ +<launch> + +<include file="$(find px4)/launch/multicopter.launch" /> + +<group ns="px4_multicopter"> + <param name="mixer" type="string" value="w" /> +</group> + +</launch> diff --git a/launch/multicopter_x.launch b/launch/multicopter_x.launch new file mode 100644 index 000000000..6355b87be --- /dev/null +++ b/launch/multicopter_x.launch @@ -0,0 +1,9 @@ +<launch> + +<include file="$(find px4)/launch/multicopter.launch" /> + +<group ns="px4_multicopter"> + <param name="mixer" type="string" value="x" /> +</group> + +</launch> 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 <px4.h> #include <lib/mathlib/mathlib.h> #include <mav_msgs/MotorSpeed.h> +#include <string> 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<mav_msgs::MotorSpeed>("/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(); |