ACRO mode implemented

9 files changed, 145 insertions, 36 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c39833713..89294fc76 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -619,9 +619,10 @@ int commander_thread_main(int argc, char *argv[])
 
 	char *main_states_str[MAIN_STATE_MAX];
 	main_states_str[0] = "MANUAL";
-	main_states_str[1] = "SEATBELT";
-	main_states_str[2] = "EASY";
-	main_states_str[3] = "AUTO";
+	main_states_str[1] = "ACRO";
+	main_states_str[2] = "SEATBELT";
+	main_states_str[3] = "EASY";
+	main_states_str[4] = "AUTO";
 
 	char *arming_states_str[ARMING_STATE_MAX];
 	arming_states_str[0] = "INIT";
@@ -1510,6 +1511,17 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
 	} else {
 		status->mission_switch = MISSION_SWITCH_MISSION;
 	}
+
+	/* acro switch  */
+	if (!isfinite(sp_man->acro_switch)) {
+		status->acro_switch = ACRO_SWITCH_NONE;
+
+	} else if (sp_man->acro_switch > STICK_ON_OFF_LIMIT) {
+		status->acro_switch = ACRO_SWITCH_ACRO;
+
+	} else {
+		status->acro_switch = ACRO_SWITCH_NORMAL;
+	}
 }
 
 transition_result_t
@@ -1520,7 +1532,11 @@ set_main_state_rc(struct vehicle_status_s *status)
 
 	switch (status->mode_switch) {
 	case MODE_SWITCH_MANUAL:
-		res = main_state_transition(status, MAIN_STATE_MANUAL);
+		if (status->acro_switch == ACRO_SWITCH_ACRO) {
+			res = main_state_transition(status, MAIN_STATE_ACRO);
+		} else {
+			res = main_state_transition(status, MAIN_STATE_MANUAL);
+		}
 		// TRANSITION_DENIED is not possible here
 		break;
 
@@ -1600,6 +1616,17 @@ set_control_mode()
 			control_mode.flag_control_velocity_enabled = false;
 			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;
+
 		case MAIN_STATE_SEATBELT:
 			control_mode.flag_control_manual_enabled = true;
 			control_mode.flag_control_auto_enabled = false;
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 43d0e023e..fd966a068 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -230,6 +230,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_SEATBELT:
 
 		/* need at minimum altitude estimate */
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 a0accb855..5f862652a 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -161,7 +161,12 @@ private:
 		param_t yaw_rate_i;
 		param_t yaw_rate_d;
 		param_t yaw_ff;
+		param_t roll_scale_acro;
+		param_t pitch_scale_acro;
+		param_t yaw_scale_acro;
 
+		param_t rc_scale_roll;
+		param_t rc_scale_pitch;
 		param_t rc_scale_yaw;
 	}		_params_handles;		/**< handles for interesting parameters */
 
@@ -171,8 +176,9 @@ private:
 		math::Vector<3> rate_i;				/**< I gain for angular rate error */
 		math::Vector<3> rate_d;				/**< D gain for angular rate error */
 		float yaw_ff;						/**< yaw control feed-forward */
+		math::Vector<3> scale_acro;			/**< scale for ACRO mode */
 
-		float rc_scale_yaw;
+		math::Vector<3> rc_scale;			/**< scale for MANUAL mode */
 	}		_params;
 
 	/**
@@ -275,6 +281,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params.rate_p.zero();
 	_params.rate_i.zero();
 	_params.rate_d.zero();
+	_params.scale_acro.zero();
+	_params.rc_scale.zero();
 
 	_R_sp.identity();
 	_R.identity();
@@ -286,21 +294,26 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 
 	I.identity();
 
-	_params_handles.roll_p			= 	param_find("MC_ROLL_P");
-	_params_handles.roll_rate_p		= 	param_find("MC_ROLLRATE_P");
-	_params_handles.roll_rate_i		= 	param_find("MC_ROLLRATE_I");
-	_params_handles.roll_rate_d		= 	param_find("MC_ROLLRATE_D");
-	_params_handles.pitch_p			= 	param_find("MC_PITCH_P");
-	_params_handles.pitch_rate_p	= 	param_find("MC_PITCHRATE_P");
-	_params_handles.pitch_rate_i	= 	param_find("MC_PITCHRATE_I");
-	_params_handles.pitch_rate_d	= 	param_find("MC_PITCHRATE_D");
-	_params_handles.yaw_p			=	param_find("MC_YAW_P");
-	_params_handles.yaw_rate_p		= 	param_find("MC_YAWRATE_P");
-	_params_handles.yaw_rate_i		= 	param_find("MC_YAWRATE_I");
-	_params_handles.yaw_rate_d		= 	param_find("MC_YAWRATE_D");
-	_params_handles.yaw_ff			= 	param_find("MC_YAW_FF");
-
-	_params_handles.rc_scale_yaw	= 	param_find("RC_SCALE_YAW");
+	_params_handles.roll_p				= 	param_find("MC_ROLL_P");
+	_params_handles.roll_rate_p			= 	param_find("MC_ROLLRATE_P");
+	_params_handles.roll_rate_i			= 	param_find("MC_ROLLRATE_I");
+	_params_handles.roll_rate_d			= 	param_find("MC_ROLLRATE_D");
+	_params_handles.pitch_p				= 	param_find("MC_PITCH_P");
+	_params_handles.pitch_rate_p		= 	param_find("MC_PITCHRATE_P");
+	_params_handles.pitch_rate_i		= 	param_find("MC_PITCHRATE_I");
+	_params_handles.pitch_rate_d		= 	param_find("MC_PITCHRATE_D");
+	_params_handles.yaw_p				=	param_find("MC_YAW_P");
+	_params_handles.yaw_rate_p			= 	param_find("MC_YAWRATE_P");
+	_params_handles.yaw_rate_i			= 	param_find("MC_YAWRATE_I");
+	_params_handles.yaw_rate_d			= 	param_find("MC_YAWRATE_D");
+	_params_handles.yaw_ff				= 	param_find("MC_YAW_FF");
+	_params_handles.roll_scale_acro		= 	param_find("MC_ROLL_S_ACRO");
+	_params_handles.pitch_scale_acro	= 	param_find("MC_PITCH_S_ACRO");
+	_params_handles.yaw_scale_acro		= 	param_find("MC_YAW_S_ACRO");
+
+	_params_handles.rc_scale_roll		= 	param_find("RC_SCALE_ROLL");
+	_params_handles.rc_scale_pitch		= 	param_find("RC_SCALE_PITCH");
+	_params_handles.rc_scale_yaw		= 	param_find("RC_SCALE_YAW");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -344,6 +357,10 @@ MulticopterAttitudeControl::parameters_update()
 	_params.rate_i(0) = v;
 	param_get(_params_handles.roll_rate_d, &v);
 	_params.rate_d(0) = v;
+	param_get(_params_handles.roll_scale_acro, &v);
+	_params.scale_acro(0) = v;
+	param_get(_params_handles.rc_scale_roll, &v);
+	_params.rc_scale(0) = v;
 
 	/* pitch */
 	param_get(_params_handles.pitch_p, &v);
@@ -354,6 +371,10 @@ MulticopterAttitudeControl::parameters_update()
 	_params.rate_i(1) = v;
 	param_get(_params_handles.pitch_rate_d, &v);
 	_params.rate_d(1) = v;
+	param_get(_params_handles.pitch_scale_acro, &v);
+	_params.scale_acro(1) = v;
+	param_get(_params_handles.rc_scale_pitch, &v);
+	_params.rc_scale(1) = v;
 
 	/* yaw */
 	param_get(_params_handles.yaw_p, &v);
@@ -364,10 +385,11 @@ MulticopterAttitudeControl::parameters_update()
 	_params.rate_i(2) = v;
 	param_get(_params_handles.yaw_rate_d, &v);
 	_params.rate_d(2) = v;
-
 	param_get(_params_handles.yaw_ff, &_params.yaw_ff);
-
-	param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw);
+	param_get(_params_handles.yaw_scale_acro, &v);
+	_params.scale_acro(2) = v;
+	param_get(_params_handles.rc_scale_yaw, &v);
+	_params.rc_scale(2) = v;
 
 	return OK;
 }
@@ -463,6 +485,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
 
 	if (_v_control_mode.flag_control_manual_enabled) {
 		/* manual input, set or modify attitude setpoint */
+		vehicle_manual_poll();
 
 		if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) {
 			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
@@ -488,16 +511,16 @@ MulticopterAttitudeControl::control_attitude(float dt)
 			//	reset_yaw_sp = true;
 			//}
 		} else {
-			float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw;
-			if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
+			float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale(2);
+			if (_params.rc_scale(2) > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
 				/* move yaw setpoint */
-				yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw;
+				yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale(2);
 				if (_manual_control_sp.yaw > 0.0f) {
 					yaw_sp_move_rate -= YAW_DEADZONE;
 				} else {
 					yaw_sp_move_rate += YAW_DEADZONE;
 				}
-				yaw_sp_move_rate *= _params.rc_scale_yaw;
+				yaw_sp_move_rate *= _params.rc_scale(2);
 				_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
 				_v_att_sp.R_valid = false;
 				publish_att_sp = true;
@@ -743,7 +766,6 @@ MulticopterAttitudeControl::task_main()
 			parameter_update_poll();
 			vehicle_control_mode_poll();
 			arming_status_poll();
-			vehicle_manual_poll();
 
 			if (_v_control_mode.flag_control_attitude_enabled) {
 				control_attitude(dt);
@@ -764,9 +786,37 @@ MulticopterAttitudeControl::task_main()
 
 			} else {
 				/* attitude controller disabled */
-				// TODO poll 'attitude_rates_setpoint' topic
-				_rates_sp.zero();
-				_thrust_sp = 0.0f;
+				if (_v_control_mode.flag_control_manual_enabled) {
+					/* manual rates control, ACRO mode */
+					vehicle_manual_poll();
+
+					_rates_sp(0) = _manual_control_sp.roll;
+					_rates_sp(1) = _manual_control_sp.pitch;
+					_rates_sp(2) = _manual_control_sp.yaw;
+
+					/* rescale controls for ACRO mode */
+					_rates_sp = _rates_sp.edivide(_params.rc_scale).emult(_params.scale_acro);
+					_thrust_sp = _manual_control_sp.throttle;
+
+					/* 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 {
+					// TODO poll 'attitude_rates_setpoint' topic
+					_rates_sp.zero();
+					_thrust_sp = 0.0f;
+				}
 			}
 
 			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 27a45b6bb..f3a4022c8 100644
--- a/src/modules/mc_att_control/mc_att_control_params.c
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -54,3 +54,6 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
 PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
 PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
 PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
+PARAM_DEFINE_FLOAT(MC_ROLL_S_ACRO, 5.0f);
+PARAM_DEFINE_FLOAT(MC_PITCH_S_ACRO, 5.0f);
+PARAM_DEFINE_FLOAT(MC_YAW_S_ACRO, 3.0f);
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 30659fd3a..e72b48a88 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -449,6 +449,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
+PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0);
 
 //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
 
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index bd665b592..50ddec8a9 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -246,6 +246,7 @@ private:
 		int rc_map_return_sw;
 		int rc_map_assisted_sw;
 		int rc_map_mission_sw;
+		int rc_map_acro_sw;
 
 //		int rc_map_offboard_ctrl_mode_sw;
 
@@ -296,6 +297,7 @@ private:
 		param_t rc_map_return_sw;
 		param_t rc_map_assisted_sw;
 		param_t rc_map_mission_sw;
+		param_t rc_map_acro_sw;
 
 //		param_t rc_map_offboard_ctrl_mode_sw;
 
@@ -515,6 +517,7 @@ Sensors::Sensors() :
 	/* optional mode switches, not mapped per default */
 	_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
 	_parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW");
+	_parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
 
 //	_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
 
@@ -669,6 +672,10 @@ Sensors::parameters_update()
 		warnx("Failed getting mission sw chan index");
 	}
 
+	if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) {
+		warnx("Failed getting acro sw chan index");
+	}
+
 	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
 		warnx("Failed getting flaps chan index");
 	}
@@ -700,6 +707,7 @@ Sensors::parameters_update()
 	_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
 	_rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1;
 	_rc.function[MISSION] = _parameters.rc_map_mission_sw - 1;
+	_rc.function[ACRO] = _parameters.rc_map_acro_sw - 1;
 
 	_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
 
@@ -1290,6 +1298,7 @@ Sensors::rc_poll()
 		manual_control.return_switch = NAN;
 		manual_control.assisted_switch = NAN;
 		manual_control.mission_switch = NAN;
+		manual_control.acro_switch = NAN;
 //		manual_control.auto_offboard_input_switch = NAN;
 
 		manual_control.flaps = NAN;
@@ -1428,6 +1437,11 @@ Sensors::rc_poll()
 			manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
 		}
 
+		/* acro switch input */
+		if (_rc.function[ACRO] >= 0) {
+			manual_control.acro_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ACRO]].scaled);
+		}
+
 		/* return switch input */
 		if (_rc.function[RETURN] >= 0) {
 			manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index eac6d6e98..190dc01c8 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -60,6 +60,7 @@ struct manual_control_setpoint_s {
 	float return_switch;			/**< land 2 position switch (mandatory): land, no effect */
 	float assisted_switch;			/**< assisted 2 position switch (optional): seatbelt, simple */
 	float mission_switch;		/**< mission 2 position switch (optional): mission, loiter */
+	float acro_switch;			/**< acro 2 position switch (optional): normal, acro */
 
 	/**
 	 * Any of the channels below may not be available and be set to NaN
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 086b2ef15..beb7008ab 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -68,11 +68,12 @@ enum RC_CHANNELS_FUNCTION
   ASSISTED = 6,
   MISSION = 7,
   OFFBOARD_MODE = 8,
-  FLAPS   = 9,
-  AUX_1   = 10,
-  AUX_2   = 11,
-  AUX_3   = 12,
-  AUX_4   = 13,
+  ACRO	= 9,
+  FLAPS   = 10,
+  AUX_1   = 11,
+  AUX_2   = 12,
+  AUX_3   = 13,
+  AUX_4   = 14,
   AUX_5   = 14,
   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 1b3639e30..5cb0bd8a2 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -63,6 +63,7 @@
 /* main state machine */
 typedef enum {
 	MAIN_STATE_MANUAL = 0,
+	MAIN_STATE_ACRO,
 	MAIN_STATE_SEATBELT,
 	MAIN_STATE_EASY,
 	MAIN_STATE_AUTO,
@@ -116,6 +117,12 @@ typedef enum {
 	MISSION_SWITCH_MISSION
 } mission_switch_pos_t;
 
+typedef enum {
+	ACRO_SWITCH_NONE = 0,
+	ACRO_SWITCH_NORMAL,
+	ACRO_SWITCH_ACRO
+} acro_switch_pos_t;
+
 enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
 	VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
@@ -192,6 +199,7 @@ struct vehicle_status_s
 	return_switch_pos_t return_switch;
 	assisted_switch_pos_t assisted_switch;
 	mission_switch_pos_t mission_switch;
+	acro_switch_pos_t acro_switch;
 
 	bool condition_battery_voltage_valid;
 	bool condition_system_in_air_restore;	/**< true if we can restore in mid air */