VTOL tiltrotor: introduce spin up time with spin up tilt angle

As some tiltrotor systems need a certain tilt angle of their motors in oder to spin up freely,
this commit introduces an additional parameter VT_TILT_SPINUP and sets the motor tilt to
this value if disarmed or within 1s since arming.

The spinup consists of 2 phases:
    1st phase fixed tilt (duration hardcoded to 1s)
    2nd phase transition from spinup tilt to multicopter tilt (hardcoded to 0.5s)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/vtol_att_control/tiltrotor.cpp

@@ -43,6 +43,7 @@
 #include "vtol_att_control_main.h"
 
 using namespace matrix;
+using namespace time_literals;
 
 #define ARSP_YAW_CTRL_DISABLE 7.0f	// airspeed at which we stop controlling yaw during a front transition
 
@@ -61,6 +62,7 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
 	_params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC");
 	_params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS");
 	_params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW");
+	_params_handles_tiltrotor.tilt_spinup = param_find("VT_TILT_SPINUP");
 	_params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
 }
 
@@ -81,6 +83,10 @@ Tiltrotor::parameters_update()
 	param_get(_params_handles_tiltrotor.tilt_fw, &v);
 	_params_tiltrotor.tilt_fw = v;
 
+	/* vtol tilt mechanism position during motor spinup */
+	param_get(_params_handles_tiltrotor.tilt_spinup, &v);
+	_params_tiltrotor.tilt_spinup = v;
+
 	/* vtol front transition phase 2 duration */
 	param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v);
 	_params_tiltrotor.front_trans_dur_p2 = v;
@@ -209,9 +215,46 @@ void Tiltrotor::update_mc_state()
 {
 	VtolType::update_mc_state();
 
-	_tilt_control = VtolType::pusher_assist();
+	/*Motor spin up: define the first second after arming as motor spin up time, during which
+	* the tilt is set to the value of VT_TILT_SPINUP. This allowes the user to set a spin up
+	* tilt angle in case the propellers don't spin up smootly in full upright (MC mode) position.
+	*/
+
+	const int spin_up_duration_p1 = 1000_ms; // duration of 1st phase of spinup (at fixed tilt)
+	const int spin_up_duration_p2 = 700_ms; // duration of 2nd phase of spinup (transition from spinup tilt to mc tilt)
+
+	// reset this timestamp while disarmed
+	if (!_v_control_mode->flag_armed) {
+		_last_timestamp_disarmed = hrt_absolute_time();
+		_tilt_motors_for_startup = true;
+
+	} else if (_tilt_motors_for_startup) {
+		// leave motors tilted forward after arming to allow them to spin up easier
+		if (hrt_absolute_time() - _last_timestamp_disarmed > (spin_up_duration_p1 + spin_up_duration_p2)) {
+			_tilt_motors_for_startup = false;
+		}
+	}
+
+	if (_tilt_motors_for_startup) {
+		if (hrt_absolute_time() - _last_timestamp_disarmed < spin_up_duration_p1) {
+			_tilt_control = _params_tiltrotor.tilt_spinup;
+
+		} else {
+			// duration phase 2: begin to adapt tilt to multicopter tilt
+			float delta_tilt = (_params_tiltrotor.tilt_mc - _params_tiltrotor.tilt_spinup);
+			_tilt_control = _params_tiltrotor.tilt_spinup + delta_tilt / spin_up_duration_p2 * (hrt_absolute_time() -
+					(_last_timestamp_disarmed + spin_up_duration_p1));
+		}
+
+		_mc_yaw_weight = 0.0f; //disable yaw control during spinup
+
+	} else {
+		// normal operation
+		_tilt_control = VtolType::pusher_assist();
+		_mc_yaw_weight = 1.0f;
+		_v_att_sp->thrust_body[2] = Tiltrotor::thrust_compensation_for_tilt();
+	}
 
-	_v_att_sp->thrust_body[2] = Tiltrotor::thrust_compensation_for_tilt();
 }
 
 void Tiltrotor::update_fw_state()

src/modules/vtol_att_control/tiltrotor.h

@@ -63,9 +63,10 @@ public:
 private:
 
 	struct {
-		float tilt_mc;					/**< actuator value corresponding to mc tilt */
+		float tilt_mc;				/**< actuator value corresponding to mc tilt */
 		float tilt_transition;			/**< actuator value corresponding to transition tilt (e.g 45 degrees) */
-		float tilt_fw;					/**< actuator value corresponding to fw tilt */
+		float tilt_fw;				/**< actuator value corresponding to fw tilt */
+		float tilt_spinup;			/**< actuator value corresponding to spinup tilt */
 		float front_trans_dur_p2;
 	} _params_tiltrotor;
 
@@ -73,6 +74,7 @@ private:
 		param_t tilt_mc;
 		param_t tilt_transition;
 		param_t tilt_fw;
+		param_t tilt_spinup;
 		param_t front_trans_dur_p2;
 	} _params_handles_tiltrotor;
 
@@ -99,6 +101,8 @@ private:
 	float _tilt_control{0.0f};		/**< actuator value for the tilt servo */
 
 	void parameters_update() override;
+	hrt_abstime _last_timestamp_disarmed{0}; /**< used for calculating time since arming */
+	bool _tilt_motors_for_startup{false};
 
 };
 #endif

src/modules/vtol_att_control/tiltrotor_params.c

@@ -71,6 +71,20 @@ PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.3f);
  */
 PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f);
 
+/**
+ * Tilt actuator control value commanded when disarmed and during the first second after arming.
+ *
+ * This specific tilt during spin-up is necessary for some systems whose motors otherwise don't
+ * spin-up freely.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @increment 0.01
+ * @decimal 3
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_TILT_SPINUP, 0.0f);
+
 /**
  * Duration of front transition phase 2
  *