Merge pull request #979 from PX4/acro2

ACRO mode

11 files changed, 160 insertions, 15 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 0b6f863ff..c26200048 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -453,6 +453,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 					/* AUTO */
 					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
+					/* ACRO */
+					main_res = main_state_transition(status, MAIN_STATE_ACRO);
 				}
 
 			} else {
@@ -652,6 +656,7 @@ int commander_thread_main(int argc, char *argv[])
 	main_states_str[1] = "ALTCTL";
 	main_states_str[2] = "POSCTL";
 	main_states_str[3] = "AUTO";
+	main_states_str[4] = "ACRO";
 
 	char *arming_states_str[ARMING_STATE_MAX];
 	arming_states_str[0] = "INIT";
@@ -1206,7 +1211,7 @@ int commander_thread_main(int argc, char *argv[])
 			 * do it only for rotary wings */
 			if (status.is_rotary_wing &&
 			    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
-			    (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
+			    (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) &&
 			    sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
@@ -1619,7 +1624,12 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 		break;
 
 	case SWITCH_POS_OFF:		// MANUAL
-		res = main_state_transition(status, MAIN_STATE_MANUAL);
+		if (sp_man->acro_switch == SWITCH_POS_ON) {
+			res = main_state_transition(status, MAIN_STATE_ACRO);
+
+		} else {
+			res = main_state_transition(status, MAIN_STATE_MANUAL);
+		}
 		// TRANSITION_DENIED is not possible here
 		break;
 
@@ -1731,6 +1741,17 @@ set_control_mode()
 			navigator_enabled = true;
 			break;
 
+		case MAIN_STATE_ACRO:
+			control_mode.flag_control_manual_enabled = true;
+			control_mode.flag_control_auto_enabled = false;
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
+
 		default:
 			break;
 		}
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
index a83c81850..e0f8dc95d 100644
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@@ -15,6 +15,7 @@ enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_ALTCTL,
 	PX4_CUSTOM_MAIN_MODE_POSCTL,
 	PX4_CUSTOM_MAIN_MODE_AUTO,
+	PX4_CUSTOM_MAIN_MODE_ACRO,
 };
 
 enum PX4_CUSTOM_SUB_MODE_AUTO {
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index abcf72717..f1a353d5b 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -222,6 +222,10 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 		ret = TRANSITION_CHANGED;
 		break;
 
+	case MAIN_STATE_ACRO:
+		ret = TRANSITION_CHANGED;
+		break;
+
 	case MAIN_STATE_ALTCTL:
 
 		/* need at minimum altitude estimate */
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 79dd88657..2837c16b8 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -136,6 +136,11 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+
+		} else if (status->main_state == MAIN_STATE_ACRO) {
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
 		}
 
 	} else {
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index b23166a5e..20e016da3 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -162,6 +162,9 @@ private:
 		param_t man_roll_max;
 		param_t man_pitch_max;
 		param_t man_yaw_max;
+		param_t acro_roll_max;
+		param_t acro_pitch_max;
+		param_t acro_yaw_max;
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	struct {
@@ -175,6 +178,7 @@ private:
 		float man_roll_max;
 		float man_pitch_max;
 		float man_yaw_max;
+		math::Vector<3> acro_rate_max;		/**< max attitude rates in acro mode */
 	}		_params;
 
 	/**
@@ -284,6 +288,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params.man_roll_max = 0.0f;
 	_params.man_pitch_max = 0.0f;
 	_params.man_yaw_max = 0.0f;
+	_params.acro_rate_max.zero();
 
 	_rates_prev.zero();
 	_rates_sp.zero();
@@ -310,6 +315,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.man_roll_max	= 	param_find("MC_MAN_R_MAX");
 	_params_handles.man_pitch_max	= 	param_find("MC_MAN_P_MAX");
 	_params_handles.man_yaw_max		= 	param_find("MC_MAN_Y_MAX");
+	_params_handles.acro_roll_max	= 	param_find("MC_ACRO_R_MAX");
+	_params_handles.acro_pitch_max	= 	param_find("MC_ACRO_P_MAX");
+	_params_handles.acro_yaw_max		= 	param_find("MC_ACRO_Y_MAX");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -386,6 +394,14 @@ MulticopterAttitudeControl::parameters_update()
 	_params.man_pitch_max = math::radians(_params.man_pitch_max);
 	_params.man_yaw_max = math::radians(_params.man_yaw_max);
 
+	/* acro control scale */
+	param_get(_params_handles.acro_roll_max, &v);
+	_params.acro_rate_max(0) = math::radians(v);
+	param_get(_params_handles.acro_pitch_max, &v);
+	_params.acro_rate_max(1) = math::radians(v);
+	param_get(_params_handles.acro_yaw_max, &v);
+	_params.acro_rate_max(2) = math::radians(v);
+
 	return OK;
 }
 
@@ -782,11 +798,36 @@ MulticopterAttitudeControl::task_main()
 
 			} else {
 				/* attitude controller disabled, poll rates setpoint topic */
-				vehicle_rates_setpoint_poll();
-				_rates_sp(0) = _v_rates_sp.roll;
-				_rates_sp(1) = _v_rates_sp.pitch;
-				_rates_sp(2) = _v_rates_sp.yaw;
-				_thrust_sp = _v_rates_sp.thrust;
+				if (_v_control_mode.flag_control_manual_enabled) {
+					/* manual rates control - ACRO mode */
+					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max);
+					_thrust_sp = _manual_control_sp.z;
+
+					/* reset yaw setpoint after ACRO */
+					_reset_yaw_sp = true;
+
+					/* publish attitude rates setpoint */
+					_v_rates_sp.roll = _rates_sp(0);
+					_v_rates_sp.pitch = _rates_sp(1);
+					_v_rates_sp.yaw = _rates_sp(2);
+					_v_rates_sp.thrust = _thrust_sp;
+					_v_rates_sp.timestamp = hrt_absolute_time();
+
+					if (_v_rates_sp_pub > 0) {
+						orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
+
+					} else {
+						_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
+					}
+
+				} else {
+					/* attitude controller disabled, poll rates setpoint topic */
+					vehicle_rates_setpoint_poll();
+					_rates_sp(0) = _v_rates_sp.roll;
+					_rates_sp(1) = _v_rates_sp.pitch;
+					_rates_sp(2) = _v_rates_sp.yaw;
+					_thrust_sp = _v_rates_sp.thrust;
+				}
 			}
 
 			if (_v_control_mode.flag_control_rates_enabled) {
diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c
index ad38a0a03..819847b40 100644
--- a/src/modules/mc_att_control/mc_att_control_params.c
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -215,3 +215,32 @@ PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f);
  * @group Multicopter Attitude Control
  */
 PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f);
+
+/**
+ * Max acro roll rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
+
+/**
+ * Max acro pitch rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
+
+/**
+ * Max acro yaw rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f);
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index c021e1fde..999cf8bb3 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -626,6 +626,15 @@ PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0);
 
 /**
+ * Acro switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0);
+
+/**
  * Flaps channel mapping.
  *
  * @min 0
@@ -756,3 +765,19 @@ PARAM_DEFINE_FLOAT(RC_RETURN_TH, 0.5f);
  *
  */
 PARAM_DEFINE_FLOAT(RC_LOITER_TH, 0.5f);
+
+/**
+ * Threshold for selecting acro mode
+ *
+ * min:-1
+ * max:+1
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index c5954d02a..b268b1b36 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -263,6 +263,7 @@ private:
 		int rc_map_return_sw;
 		int rc_map_posctl_sw;
 		int rc_map_loiter_sw;
+		int rc_map_acro_sw;
 
 		int rc_map_flaps;
 
@@ -278,11 +279,13 @@ private:
 		float rc_posctl_th;
 		float rc_return_th;
 		float rc_loiter_th;
+		float rc_acro_th;
 		bool rc_assist_inv;
 		bool rc_auto_inv;
 		bool rc_posctl_inv;
 		bool rc_return_inv;
 		bool rc_loiter_inv;
+		bool rc_acro_inv;
 
 		float battery_voltage_scaling;
 		float battery_current_scaling;
@@ -315,6 +318,7 @@ private:
 		param_t rc_map_return_sw;
 		param_t rc_map_posctl_sw;
 		param_t rc_map_loiter_sw;
+		param_t rc_map_acro_sw;
 
 		param_t rc_map_flaps;
 
@@ -330,6 +334,7 @@ private:
 		param_t rc_posctl_th;
 		param_t rc_return_th;
 		param_t rc_loiter_th;
+		param_t rc_acro_th;
 
 		param_t battery_voltage_scaling;
 		param_t battery_current_scaling;
@@ -530,6 +535,7 @@ Sensors::Sensors() :
 	/* optional mode switches, not mapped per default */
 	_parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW");
 	_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
+	_parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
 
 	_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
 	_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@@ -544,6 +550,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH");
 	_parameter_handles.rc_return_th = param_find("RC_RETURN_TH");
 	_parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");
+	_parameter_handles.rc_acro_th = param_find("RC_ACRO_TH");
 
 	/* gyro offsets */
 	_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
@@ -687,6 +694,10 @@ Sensors::parameters_update()
 		warnx("%s", paramerr);
 	}
 
+	if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) {
+		warnx("%s", paramerr);
+	}
+
 	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
 		warnx("%s", paramerr);
 	}
@@ -712,6 +723,9 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_loiter_th, &(_parameters.rc_loiter_th));
 	_parameters.rc_loiter_inv = (_parameters.rc_loiter_th < 0);
 	_parameters.rc_loiter_th = fabs(_parameters.rc_loiter_th);
+	param_get(_parameter_handles.rc_acro_th, &(_parameters.rc_acro_th));
+	_parameters.rc_acro_inv = (_parameters.rc_acro_th < 0);
+	_parameters.rc_acro_th = fabs(_parameters.rc_acro_th);
 
 	/* update RC function mappings */
 	_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -723,6 +737,7 @@ Sensors::parameters_update()
 	_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
 	_rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1;
 	_rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
+	_rc.function[ACRO] = _parameters.rc_map_acro_sw - 1;
 
 	_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
 
@@ -1508,6 +1523,7 @@ Sensors::rc_poll()
 			manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
 			manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
 			manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
+			manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
 
 			/* publish manual_control_setpoint topic */
 			if (_manual_control_pub > 0) {
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index 19a29635b..910b8a623 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -97,6 +97,7 @@ struct manual_control_setpoint_s {
 	switch_pos_t return_switch;			/**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
 	switch_pos_t posctl_switch;			/**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
 	switch_pos_t loiter_switch;			/**< loiter 2 position switch (optional): _MISSION_, LOITER */
+	switch_pos_t acro_switch;			/**< acro 2 position switch (optional): _MANUAL_, ACRO */
 }; /**< manual control inputs */
 
 /**
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index b60402452..370c54442 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -45,12 +45,12 @@
 /**
  * The number of RC channel inputs supported.
  * Current (Q4/2013) radios support up to 18 channels,
- * leaving at a sane value of 15.
+ * leaving at a sane value of 16.
  * This number can be greater then number of RC channels,
  * because single RC channel can be mapped to multiple
  * functions, e.g. for various mode switches.
  */
-#define RC_CHANNELS_MAPPED_MAX   15
+#define RC_CHANNELS_MAPPED_MAX   16
 
 /**
  * This defines the mapping of the RC functions.
@@ -67,12 +67,13 @@ enum RC_CHANNELS_FUNCTION {
 	POSCTL = 6,
 	LOITER = 7,
 	OFFBOARD_MODE = 8,
-	FLAPS   = 9,
-	AUX_1   = 10,
-	AUX_2   = 11,
-	AUX_3   = 12,
-	AUX_4   = 13,
-	AUX_5   = 14,
+	ACRO    = 9,
+	FLAPS   = 10,
+	AUX_1   = 11,
+	AUX_2   = 12,
+	AUX_3   = 13,
+	AUX_4   = 14,
+	AUX_5   = 15,
 	RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
 };
 
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index f56355246..ea20a317a 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -66,6 +66,7 @@ typedef enum {
 	MAIN_STATE_ALTCTL,
 	MAIN_STATE_POSCTL,
 	MAIN_STATE_AUTO,
+	MAIN_STATE_ACRO,
 	MAIN_STATE_MAX
 } main_state_t;