use optimal recovery strategy for tailsitters

cmake/configs/nuttx_px4fmu-v1_default.cmake

@@ -113,6 +113,7 @@ set(config_module_list
 	lib/launchdetection
 	lib/terrain_estimation
 	lib/runway_takeoff
+	lib/tailsitter_recovery
 	platforms/nuttx
 
 	# had to add for cmake, not sure why wasn't in original config

cmake/configs/nuttx_px4fmu-v2_default.cmake

@@ -124,6 +124,7 @@ set(config_module_list
 	lib/launchdetection
 	lib/terrain_estimation
 	lib/runway_takeoff
+	lib/tailsitter_recovery
 	platforms/nuttx
 
 	# had to add for cmake, not sure why wasn't in original config

cmake/configs/posix_sitl_default.cmake

@@ -59,6 +59,7 @@ set(config_module_list
 	lib/launchdetection
 	lib/terrain_estimation
 	lib/runway_takeoff
+	lib/tailsitter_recovery
 	examples/px4_simple_app
 	)
 

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -88,6 +88,7 @@
 #include <systemlib/circuit_breaker.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/geo/geo.h>
+#include <lib/tailsitter_recovery/tailsitter_recovery.h>
 
 /**
  * Multicopter attitude control app start / stop handling function
@@ -194,6 +195,8 @@ private:
 		param_t acro_yaw_max;
 		param_t rattitude_thres;
 
+		param_t vtol_type;
+
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	struct {
@@ -211,8 +214,11 @@ private:
 
 		math::Vector<3> acro_rate_max;		/**< max attitude rates in acro mode */
 		float rattitude_thres;
+		int vtol_type;						/**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */
 	}		_params;
 
+	TailsitterRecovery *_ts_opt_recovery;	/**< Computes optimal rates for tailsitter recovery */
+
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -316,7 +322,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 
 	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")),
-	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency"))
+	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
+	_ts_opt_recovery(nullptr)
 
 {
 	memset(&_ctrl_state, 0, sizeof(_ctrl_state));
@@ -376,9 +383,18 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.acro_pitch_max	= 	param_find("MC_ACRO_P_MAX");
 	_params_handles.acro_yaw_max	= 	param_find("MC_ACRO_Y_MAX");
 	_params_handles.rattitude_thres = 	param_find("MC_RATT_TH");
+	_params_handles.vtol_type 		= 	param_find("VT_TYPE");
 
 	/* fetch initial parameter values */
 	parameters_update();
+	vehicle_status_poll();
+
+	if (_vehicle_status.is_vtol && _params.vtol_type == 0) {
+		// the vehicle is a tailsitter, use optimal recovery control strategy
+		_ts_opt_recovery = new TailsitterRecovery();
+	}
+
+
 }
 
 MulticopterAttitudeControl::~MulticopterAttitudeControl()
@@ -402,6 +418,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl()
 		} while (_control_task != -1);
 	}
 
+	delete _ts_opt_recovery;
 	mc_att_control::g_control = nullptr;
 }
 
@@ -467,6 +484,8 @@ MulticopterAttitudeControl::parameters_update()
 	/* stick deflection needed in rattitude mode to control rates not angles */
 	param_get(_params_handles.rattitude_thres, &_params.rattitude_thres);
 
+	param_get(_params_handles.vtol_type, &_params.vtol_type);
+
 	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
 
 	return OK;
@@ -558,11 +577,13 @@ MulticopterAttitudeControl::vehicle_status_poll()
 
 	if (vehicle_status_updated) {
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+
 		/* set correct uORB ID, depending on if vehicle is VTOL or not */
 		if (!_rates_sp_id) {
 			if (_vehicle_status.is_vtol) {
 				_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
 				_actuators_id = ORB_ID(actuator_controls_virtual_mc);
+
 			} else {
 				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
 				_actuators_id = ORB_ID(actuator_controls_0);
@@ -699,7 +720,8 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
 
 	/* angular rates error */
 	math::Vector<3> rates_err = _rates_sp - rates;
-	_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt;
+	_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
+		       _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt;
 	_rates_sp_prev = _rates_sp;
 	_rates_prev = rates;
 
@@ -756,8 +778,9 @@ MulticopterAttitudeControl::task_main()
 		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		/* timed out - periodic check for _task_should_exit */
-		if (pret == 0)
+		if (pret == 0) {
 			continue;
+		}
 
 		/* this is undesirable but not much we can do - might want to flag unhappy status */
 		if (pret < 0) {
@@ -806,8 +829,23 @@ MulticopterAttitudeControl::task_main()
 
 			if (_v_control_mode.flag_control_attitude_enabled) {
 
+				if (_ts_opt_recovery == nullptr) {
+					// the  tailsitter recovery instance has not been created, thus, the vehicle
+					// is not a tailsitter, do normal attitude control
 					control_attitude(dt);
 
+				} else {
+					math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
+					math::Quaternion q_sp(&_v_att_sp.q_d[0]);
+					_ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff);
+					_ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp);
+
+					/* limit rates */
+					for (int i = 0; i < 3; i++) {
+						_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
+					}
+				}
+
 				/* publish attitude rates setpoint */
 				_v_rates_sp.roll = _rates_sp(0);
 				_v_rates_sp.pitch = _rates_sp(1);
@@ -821,12 +859,15 @@ MulticopterAttitudeControl::task_main()
 				} else if (_rates_sp_id) {
 					_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
 				}
+
 				//}
+
 			} else {
 				/* attitude controller disabled, poll rates setpoint topic */
 				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);
+					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
+								    _manual_control_sp.r).emult(_params.acro_rate_max);
 					_thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER);
 
 					/* publish attitude rates setpoint */
@@ -883,6 +924,7 @@ MulticopterAttitudeControl::task_main()
 				/* publish controller status */
 				if (_controller_status_pub != nullptr) {
 					orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status);
+
 				} else {
 					_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
 				}