VTOL transitions: use FW attitude loop (PX4#11911)

RomanBapst · dagar · commit 816aa0ffb6ba · 2019-05-08T10:33:13.000-04:00
* VTOL trans: changed that now in transition for both MC and FW the corresponding (correct) attitude controller is running
* attitude setpoint for stabilized mode is generated by tailsitter.cpp
* reset yaw setpoint during transition to avoid big yaw errors after
transition back to hover
* VT_TYPE parameter: added "reboot required" metadata
diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py
@@ -247,6 +247,7 @@ def __init__(self, module_whitelist=[], topic_blacklist=[]):
     ('mc_pos_control', r'mc_pos_control_main\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
 
     ('mc_att_control', r'mc_att_control_main\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
+    ('mc_att_control', r'mc_att_control_main\.cpp$', r'\_attitude_sp_id=([^,)]+)', r'^_attitude_sp_id$'),
 
     ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
     ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
@@ -33,6 +33,8 @@
 
 #include "FixedwingAttitudeControl.hpp"
 
+#include <vtol_att_control/vtol_type.h>
+
 using namespace time_literals;
 
 /**
@@ -120,9 +122,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.bat_scale_en = param_find("FW_BAT_SCALE_EN");
 	_parameter_handles.airspeed_mode = param_find("FW_ARSP_MODE");
 
-	// initialize to invalid VTOL type
-	_parameters.vtol_type = -1;
-
 	/* fetch initial parameter values */
 	parameters_update();
 
@@ -242,10 +241,6 @@ FixedwingAttitudeControl::parameters_update()
 
 	param_get(_parameter_handles.rattitude_thres, &_parameters.rattitude_thres);
 
-	if (_vehicle_status.is_vtol) {
-		param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);
-	}
-
 	param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en);
 
 	param_get(_parameter_handles.airspeed_mode, &tmp);
@@ -292,13 +287,25 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
 	if (updated) {
 		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
 	}
+
+	if (_vehicle_status.is_vtol) {
+		const bool is_hovering = _vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode;
+		const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter;
+
+		if (is_hovering || is_tailsitter_transition) {
+			_vcontrol_mode.flag_control_attitude_enabled = false;
+			_vcontrol_mode.flag_control_manual_enabled = false;
+		}
+	}
 }
 
 void
 FixedwingAttitudeControl::vehicle_manual_poll()
 {
+	const bool is_tailsitter_transition = _is_tailsitter && _vehicle_status.in_transition_mode;
+	const bool is_fixed_wing = !_vehicle_status.is_rotary_wing;
 
-	if (_vcontrol_mode.flag_control_manual_enabled && !_vehicle_status.is_rotary_wing) {
+	if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) {
 
 		// Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values
 		if (orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual) == PX4_OK) {
@@ -394,7 +401,7 @@ FixedwingAttitudeControl::vehicle_rates_setpoint_poll()
 	if (updated) {
 		orb_copy(ORB_ID(vehicle_rates_setpoint), _rates_sp_sub, &_rates_sp);
 
-		if (_parameters.vtol_type == vtol_type::TAILSITTER) {
+		if (_is_tailsitter) {
 			float tmp = _rates_sp.roll;
 			_rates_sp.roll = -_rates_sp.yaw;
 			_rates_sp.yaw = tmp;
@@ -424,25 +431,17 @@ FixedwingAttitudeControl::vehicle_status_poll()
 	if (vehicle_status_updated) {
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
 
-		// if VTOL and not in fixed wing mode we should only control body-rates which are published
-		// by the multicoper attitude controller. Therefore, modify the control mode to achieve rate
-		// control only
-		if (_vehicle_status.is_vtol) {
-			if (_vehicle_status.is_rotary_wing) {
-				_vcontrol_mode.flag_control_attitude_enabled = false;
-				_vcontrol_mode.flag_control_manual_enabled = false;
-			}
-		}
-
 		/* set correct uORB ID, depending on if vehicle is VTOL or not */
 		if (!_actuators_id) {
 			if (_vehicle_status.is_vtol) {
 				_actuators_id = ORB_ID(actuator_controls_virtual_fw);
 				_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
 
-				_parameter_handles.vtol_type = param_find("VT_TYPE");
+				int32_t vt_type = -1;
 
-				parameters_update();
+				if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {
+					_is_tailsitter = (vt_type == vtol_type::TAILSITTER);
+				}
 
 			} else {
 				_actuators_id = ORB_ID(actuator_controls_0);
@@ -566,7 +565,7 @@ void FixedwingAttitudeControl::run()
 			/* get current rotation matrix and euler angles from control state quaternions */
 			matrix::Dcmf R = matrix::Quatf(_att.q);
 
-			if (_vehicle_status.is_vtol && _parameters.vtol_type == vtol_type::TAILSITTER) {
+			if (_is_tailsitter) {
 				/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
 				 *
 				 * Since the VTOL airframe is initialized as a multicopter we need to
@@ -609,7 +608,7 @@ void FixedwingAttitudeControl::run()
 				_att.yawspeed = helper;
 			}
 
-			matrix::Eulerf euler_angles(R);
+			const matrix::Eulerf euler_angles(R);
 
 			vehicle_attitude_setpoint_poll();
 			vehicle_control_mode_poll();
@@ -623,7 +622,7 @@ void FixedwingAttitudeControl::run()
 			_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;
 
 			/* lock integrator until control is started */
-			bool lock_integrator = !(_vcontrol_mode.flag_control_rates_enabled && !_vehicle_status.is_rotary_wing);
+			bool lock_integrator = !_vcontrol_mode.flag_control_rates_enabled || _vehicle_status.is_rotary_wing;
 
 			/* Simple handling of failsafe: deploy parachute if failsafe is on */
 			if (_vcontrol_mode.flag_control_termination_enabled) {
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
@@ -60,7 +60,6 @@
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
-#include <vtol_att_control/vtol_type.h>
 
 using matrix::Eulerf;
 using matrix::Quatf;
@@ -140,6 +139,8 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
 
 	bool _flag_control_attitude_enabled_last{false};
 
+	bool _is_tailsitter{false};
+
 	struct {
 		float p_tc;
 		float p_p;
@@ -204,8 +205,6 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
 
 		float rattitude_thres;
 
-		int32_t vtol_type;			/**< VTOL type: 0 = tailsitter, 1 = tiltrotor */
-
 		int32_t bat_scale_en;			/**< Battery scaling enabled */
 		bool airspeed_disabled;
 
@@ -274,8 +273,6 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
 
 		param_t rattitude_thres;
 
-		param_t vtol_type;
-
 		param_t bat_scale_en;
 		param_t airspeed_mode;
 
diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp
@@ -57,6 +57,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/landing_gear.h>
+#include <vtol_att_control/vtol_type.h>
 
 #include <AttitudeControl.hpp>
 
@@ -174,6 +175,7 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 	orb_advert_t	_landing_gear_pub{nullptr};
 
 	orb_id_t _actuators_id{nullptr};	/**< pointer to correct actuator controls0 uORB metadata structure */
+	orb_id_t _attitude_sp_id{nullptr};	/**< pointer to correct attitude setpoint uORB metadata structure */
 
 	bool		_actuators_0_circuit_breaker_enabled{false};	/**< circuit breaker to suppress output */
 
@@ -277,6 +279,8 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 		(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode
 	)
 
+	bool _is_tailsitter{false};
+
 	matrix::Vector3f _rate_p;		/**< P gain for angular rate error */
 	matrix::Vector3f _rate_i;		/**< I gain for angular rate error */
 	matrix::Vector3f _rate_int_lim;		/**< integrator state limit for rate loop */
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -252,9 +252,16 @@ MulticopterAttitudeControl::vehicle_status_poll()
 		if (_actuators_id == nullptr) {
 			if (_vehicle_status.is_vtol) {
 				_actuators_id = ORB_ID(actuator_controls_virtual_mc);
+				_attitude_sp_id = ORB_ID(mc_virtual_attitude_setpoint);
+
+				int32_t vt_type = -1;
+				if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {
+					_is_tailsitter = (vt_type == vtol_type::TAILSITTER);
+				}
 
 			} else {
 				_actuators_id = ORB_ID(actuator_controls_0);
+				_attitude_sp_id = ORB_ID(vehicle_attitude_setpoint);
 			}
 		}
 	}
@@ -407,7 +414,6 @@ void
 MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_sp)
 {
 	vehicle_attitude_setpoint_s attitude_setpoint{};
-	landing_gear_s landing_gear{};
 	const float yaw = Eulerf(Quatf(_v_att.q)).psi();
 
 	/* reset yaw setpoint to current position if needed */
@@ -495,12 +501,12 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
 	attitude_setpoint.q_d_valid = true;
 
 	attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z);
+	attitude_setpoint.timestamp = hrt_absolute_time();
+	if (_attitude_sp_id != nullptr) {
+		orb_publish_auto(_attitude_sp_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT);
+	}
 
 	_landing_gear.landing_gear = get_landing_gear_state();
-
-	attitude_setpoint.timestamp = landing_gear.timestamp = hrt_absolute_time();
-	orb_publish_auto(ORB_ID(vehicle_attitude_setpoint), &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT);
-
 	_landing_gear.timestamp = hrt_absolute_time();
 	orb_publish_auto(ORB_ID(landing_gear), &_landing_gear_pub, &_landing_gear, nullptr, ORB_PRIO_DEFAULT);
 }
@@ -805,7 +811,15 @@ MulticopterAttitudeControl::run()
 
 			bool attitude_setpoint_generated = false;
 
-			if (_v_control_mode.flag_control_attitude_enabled && _vehicle_status.is_rotary_wing) {
+			const bool is_hovering = _vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode;
+
+			// vehicle is a tailsitter in transition mode
+			const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter;
+
+			bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition);
+
+
+			if (run_att_ctrl) {
 				if (attitude_updated) {
 					// Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode
 					if (_v_control_mode.flag_control_manual_enabled &&
@@ -822,7 +836,7 @@ MulticopterAttitudeControl::run()
 
 			} else {
 				/* attitude controller disabled, poll rates setpoint topic */
-				if (_v_control_mode.flag_control_manual_enabled && _vehicle_status.is_rotary_wing) {
+				if (_v_control_mode.flag_control_manual_enabled && is_hovering) {
 					if (manual_control_updated) {
 						/* manual rates control - ACRO mode */
 						Vector3f man_rate_sp(
@@ -856,9 +870,12 @@ MulticopterAttitudeControl::run()
 			}
 
 			if (attitude_updated) {
+				// reset yaw setpoint during transitions, tailsitter.cpp generates
+				// attitude setpoint for the transition
 				reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) ||
 						_vehicle_land_detected.landed ||
-						(_vehicle_status.is_vtol && !_vehicle_status.is_rotary_wing); // VTOL in FW mode
+						(_vehicle_status.is_vtol && _vehicle_status.in_transition_mode);
+
 				attitude_dt = 0.f;
 			}
 
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -752,7 +752,7 @@ VtolAttitudeControl::start()
 	_control_task = px4_task_spawn_cmd("vtol_att_control",
 					   SCHED_DEFAULT,
 					   SCHED_PRIORITY_ATTITUDE_CONTROL + 1,
-					   1230,
+					   1320,
 					   (px4_main_t)&VtolAttitudeControl::task_main_trampoline,
 					   nullptr);
 
diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c
@@ -82,6 +82,7 @@ PARAM_DEFINE_INT32(VT_FW_PERM_STAB, 0);
  * @min 0
  * @max 2
  * @decimal 0
+ * @reboot_required true
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_INT32(VT_TYPE, 0);
diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp
@@ -370,7 +370,9 @@ bool VtolType::is_motor_off_channel(const int channel)
 	int tmp;
 	int channels = _params->fw_motors_off;
 
-	for (int i = 0; i < _params->vtol_motor_count; ++i) {
+	static constexpr int num_outputs_max = 8;
+
+	for (int i = 0; i < num_outputs_max; ++i) {
 		tmp = channels % 10;
 
 		if (tmp == 0) {
