manual input dummy node: extend to support switching between modes

also fixing thrust input

2 files changed, 84 insertions, 23 deletions

diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp
index 688df50e0..7e1ae7a17 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.cpp
+++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp
@@ -40,13 +40,14 @@
 
 #include "manual_input.h"
 
-#include <px4/manual_control_setpoint.h>
 #include <platforms/px4_middleware.h>
 
 ManualInput::ManualInput() :
 	_n(),
 	_joy_sub(_n.subscribe("joy", 1, &ManualInput::JoyCallback, this)),
-	_man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 1))
+	_man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 1)),
+	_buttons_state(),
+	_msg_mc_sp()
 {
 	double dz_default = 0.2;
 	/* Load parameters, default values work for Microsoft XBox Controller */
@@ -63,40 +64,46 @@ ManualInput::ManualInput() :
 	_n.param("map_z", _param_axes_map[2], 2);
 	_n.param("scale_z", _param_axes_scale[2], -0.5);
 	_n.param("off_z", _param_axes_offset[2], -1.0);
-	_n.param("dz_z", _param_axes_dz[2], dz_default);
+	_n.param("dz_z", _param_axes_dz[2], 0.0);
 
 	_n.param("map_r", _param_axes_map[3], 0);
 	_n.param("scale_r", _param_axes_scale[3], -1.0);
 	_n.param("off_r", _param_axes_offset[3], 0.0);
 	_n.param("dz_r", _param_axes_dz[3], dz_default);
 
+	_n.param("map_manual", _param_buttons_map[0], 0);
+	_n.param("map_altctl", _param_buttons_map[1], 1);
+	_n.param("map_posctl", _param_buttons_map[2], 2);
+	_n.param("map_auto_mission", _param_buttons_map[3], 3);
+	_n.param("map_auto_loiter", _param_buttons_map[4], 4);
+	_n.param("map_auto_rtl", _param_buttons_map[5], 4);
+
+	/* Default to manual */
+	_msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+	_msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+	_msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+	_msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+	_msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+
 }
 
 void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr &msg)
 {
-	px4::manual_control_setpoint msg_out;
 
 	/* Fill px4 manual control setpoint topic with contents from ros joystick */
 	/* Map sticks to x, y, z, r */
-	MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], _param_axes_dz[0], msg_out.x);
-	MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], msg_out.y);
-	MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], msg_out.z);
-	MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], msg_out.r);
-	//ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r);
+	MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], _param_axes_dz[0], _msg_mc_sp.x);
+	MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], _msg_mc_sp.y);
+	MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], _msg_mc_sp.z);
+	MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], _msg_mc_sp.r);
+	//ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_mc_sp.x, msg_out.y, msg_out.z, msg_out.r);
 
 	/* Map buttons to switches */
-	//XXX todo
-	/* for now just publish switches in position for manual flight */
-	msg_out.mode_switch = 0;
-	msg_out.return_switch = 0;
-	msg_out.posctl_switch = 0;
-	msg_out.loiter_switch = 0;
-	msg_out.acro_switch = 0;
-	msg_out.offboard_switch = 0;
-
-	msg_out.timestamp = px4::get_time_micros();
-
-	_man_ctrl_sp_pub.publish(msg_out);
+	MapButtons(msg, _msg_mc_sp);
+
+	_msg_mc_sp.timestamp = px4::get_time_micros();
+
+	_man_ctrl_sp_pub.publish(_msg_mc_sp);
 }
 
 void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset,
@@ -106,6 +113,41 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, do
 
 	if (val + offset > deadzone || val + offset < -deadzone) {
 		out = (float)((val + offset)) * scale;
+	} else {
+		out = 0.0f;
+	}
+}
+
+void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) {
+	msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE;
+	msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_NONE;
+
+	if (_buttons_state[MAIN_STATE_MANUAL] != msg->buttons[_param_buttons_map[MAIN_STATE_MANUAL]] == true) {
+		msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		return;
+
+	} else if (_buttons_state[MAIN_STATE_ALTCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_ALTCTL]] == true) {
+		msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE;
+		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		return;
+
+	} else if (_buttons_state[MAIN_STATE_POSCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_POSCTL]] == true) {
+		msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE;
+		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON;
+		msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
+		return;
+
+	} else {
+		ROS_WARN("requested mode via joystick that is not implemented");
 	}
 }
 
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h
index 93e0abe64..bf704f675 100644
--- a/src/platforms/ros/nodes/manual_input/manual_input.h
+++ b/src/platforms/ros/nodes/manual_input/manual_input.h
@@ -39,6 +39,7 @@
 */
 
 #include "ros/ros.h"
+#include <px4/manual_control_setpoint.h>
 #include <sensor_msgs/Joy.h>
 
 class ManualInput
@@ -55,20 +56,38 @@ protected:
 	void JoyCallback(const sensor_msgs::JoyConstPtr &msg);
 
 	/**
-	 * Helper function to map and scale joystick input
+	 * Helper function to map and scale joystick axis
 	 */
 	void MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, double deadzone,
 		     float &out);
+	/**
+	 * Helper function to map joystick buttons
+	 */
+	void MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp);
 
 	ros::NodeHandle _n;
 	ros::Subscriber _joy_sub;
 	ros::Publisher _man_ctrl_sp_pub;
 
 	/* Parameters */
+	enum MAIN_STATE {
+		MAIN_STATE_MANUAL = 0,
+		MAIN_STATE_ALTCTL,
+		MAIN_STATE_POSCTL,
+		MAIN_STATE_AUTO_MISSION,
+		MAIN_STATE_AUTO_LOITER,
+		MAIN_STATE_AUTO_RTL,
+		MAIN_STATE_MAX
+	};
+
+	int _param_buttons_map[MAIN_STATE_MAX];	    /**< joystick button mapping, order according to MAIN_STATE */
+	bool _buttons_state[MAIN_STATE_MAX];	    /**< joystick button state of last iteration,
+						      order according to MAIN_STATE */
+
 	int _param_axes_map[4];
 	double _param_axes_scale[4];
 	double _param_axes_offset[4];
 	double _param_axes_dz[4];
 
-
+	px4::manual_control_setpoint _msg_mc_sp;
 };