vtol_att_control: consolidated standard parameters & fix usage of hrt_elapsed time

- standard vtol was implementing many custom parameters although they
are generic and should be shared between the vtol types
- removed heavy usage of hrt_elapsed_time() which is a system call and
could be computationally expensive

src/modules/vtol_att_control/standard.cpp

@@ -62,23 +62,13 @@ Standard::Standard(VtolAttitudeControl *attc) :
 	_mc_yaw_weight = 1.0f;
 	_mc_throttle_weight = 1.0f;
 
-	_params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR");
+	_params_handles_standard.pusher_ramp_dt = param_find("VT_PSHER_RMP_DT");
-	_params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR");
 	_params_handles_standard.back_trans_ramp = param_find("VT_B_TRANS_RAMP");
-	_params_handles_standard.pusher_trans = param_find("VT_TRANS_THR");
-	_params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND");
-	_params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS");
-	_params_handles_standard.front_trans_timeout = param_find("VT_TRANS_TIMEOUT");
-	_params_handles_standard.front_trans_time_min = param_find("VT_TRANS_MIN_TM");
 	_params_handles_standard.down_pitch_max = param_find("VT_DWN_PITCH_MAX");
 	_params_handles_standard.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
-	_params_handles_standard.airspeed_disabled = param_find("FW_ARSP_MODE");
 	_params_handles_standard.pitch_setpoint_offset = param_find("FW_PSP_OFF");
 	_params_handles_standard.reverse_output = param_find("VT_B_REV_OUT");
 	_params_handles_standard.reverse_delay = param_find("VT_B_REV_DEL");
-	_params_handles_standard.back_trans_throttle = param_find("VT_B_TRANS_THR");
-	_params_handles_standard.mpc_xy_cruise = param_find("MPC_XY_CRUISE");
-
 }
 
 Standard::~Standard()
@@ -89,39 +79,16 @@ void
 Standard::parameters_update()
 {
 	float v;
-	int i;
 
 	/* duration of a forwards transition to fw mode */
-	param_get(_params_handles_standard.front_trans_dur, &v);
+	param_get(_params_handles_standard.pusher_ramp_dt, &v);
-	_params_standard.front_trans_dur = math::constrain(v, 0.0f, 20.0f);
+	_params_standard.pusher_ramp_dt = math::constrain(v, 0.0f, 20.0f);
-
-	/* duration of a back transition to mc mode */
-	param_get(_params_handles_standard.back_trans_dur, &v);
-	_params_standard.back_trans_dur = math::constrain(v, 0.0f, 20.0f);
 
 	/* MC ramp up during back transition to mc mode */
 	param_get(_params_handles_standard.back_trans_ramp, &v);
-	_params_standard.back_trans_ramp = math::constrain(v, 0.0f, _params_standard.back_trans_dur);
+	_params_standard.back_trans_ramp = math::constrain(v, 0.0f, _params->back_trans_duration);
 
-	/* target throttle value for pusher motor during the transition to fw mode */
+	_airspeed_trans_blend_margin = _params->transition_airspeed - _params->airspeed_blend;
-	param_get(_params_handles_standard.pusher_trans, &v);
-	_params_standard.pusher_trans = math::constrain(v, 0.0f, 5.0f);
-
-	/* airspeed at which we should switch to fw mode */
-	param_get(_params_handles_standard.airspeed_trans, &v);
-	_params_standard.airspeed_trans = math::constrain(v, 1.0f, 20.0f);
-
-	/* airspeed at which we start blending mc/fw controls */
-	param_get(_params_handles_standard.airspeed_blend, &v);
-	_params_standard.airspeed_blend = math::constrain(v, 0.0f, 20.0f);
-
-	_airspeed_trans_blend_margin = _params_standard.airspeed_trans - _params_standard.airspeed_blend;
-
-	/* timeout for transition to fw mode */
-	param_get(_params_handles_standard.front_trans_timeout, &_params_standard.front_trans_timeout);
-
-	/* minimum time for transition to fw mode */
-	param_get(_params_handles_standard.front_trans_time_min, &_params_standard.front_trans_time_min);
 
 	/* maximum down pitch allowed */
 	param_get(_params_handles_standard.down_pitch_max, &v);
@@ -130,10 +97,6 @@ Standard::parameters_update()
 	/* scale for fixed wing thrust used for forward acceleration in multirotor mode */
 	param_get(_params_handles_standard.forward_thrust_scale, &_params_standard.forward_thrust_scale);
 
-	/* airspeed mode */
-	param_get(_params_handles_standard.airspeed_disabled, &i);
-	_params_standard.airspeed_disabled = math::constrain(i, 0, 1);
-
 	/* pitch setpoint offset */
 	param_get(_params_handles_standard.pitch_setpoint_offset, &v);
 	_params_standard.pitch_setpoint_offset = math::radians(v);
@@ -146,13 +109,6 @@ Standard::parameters_update()
 	param_get(_params_handles_standard.reverse_delay, &v);
 	_params_standard.reverse_delay = math::constrain(v, 0.0f, 10.0f);
 
-	/* reverse throttle */
-	param_get(_params_handles_standard.back_trans_throttle, &v);
-	_params_standard.back_trans_throttle = math::constrain(v, -1.0f, 1.0f);
-
-	/* mpc cruise speed */
-	param_get(_params_handles_standard.mpc_xy_cruise, &_params_standard.mpc_xy_cruise);
-
 }
 
 void Standard::update_vtol_state()
@@ -163,6 +119,8 @@ void Standard::update_vtol_state()
 	 */
 
 	float mc_weight = _mc_roll_weight;
+	float time_now = (float)hrt_absolute_time();
+	float time_since_trans_start = time_now - (float)_vtol_schedule.transition_start;
 
 	if (!_attc->is_fixed_wing_requested()) {
 
@@ -209,9 +167,8 @@ void Standard::update_vtol_state()
 
 			float x_vel = vel(0);
 
-			if (hrt_elapsed_time(&_vtol_schedule.transition_start) >
+			if (time_since_trans_start > _params->back_trans_duration * 1e6f ||
-			    (_params_standard.back_trans_dur * 1000000.0f) ||
+			    (_local_pos->v_xy_valid && x_vel <= _params->mpc_xy_cruise)) {
-			    (_local_pos->v_xy_valid && x_vel <= _params_standard.mpc_xy_cruise)) {
 				_vtol_schedule.flight_mode = MC_MODE;
 			}
 
@@ -233,10 +190,9 @@ void Standard::update_vtol_state()
 
 		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
 			// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
-			if (((_params_standard.airspeed_disabled == 1 ||
+			if (((_params->airspeed_mode == 1 ||
-			      _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans) &&
+			      _airspeed->indicated_airspeed_m_s >= _params->transition_airspeed) &&
-			     (float)hrt_elapsed_time(&_vtol_schedule.transition_start)
+			     time_since_trans_start > _params->front_trans_time_min * 1e6f) ||
-			     > (_params_standard.front_trans_time_min * 1000000.0f)) ||
 			    can_transition_on_ground()) {
 
 				_vtol_schedule.flight_mode = FW_MODE;
@@ -277,6 +233,9 @@ void Standard::update_vtol_state()
 void Standard::update_transition_state()
 {
 	float mc_weight = 1.0f;
+	float time_now = (float)hrt_absolute_time();
+	float time_since_trans_start = time_now - (float)
+				       _vtol_schedule.transition_start;	// time in microseconds since transition started
 
 	VtolType::update_transition_state();
 
@@ -284,32 +243,28 @@ void Standard::update_transition_state()
 	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 
 	if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
-		if (_params_standard.front_trans_dur <= 0.0f) {
+		if (_params_standard.pusher_ramp_dt <= 0.0f) {
 			// just set the final target throttle value
-			_pusher_throttle = _params_standard.pusher_trans;
+			_pusher_throttle = _params->front_trans_throttle;
 
-		} else if (_pusher_throttle <= _params_standard.pusher_trans) {
+		} else if (_pusher_throttle <= _params->front_trans_throttle) {
 			// ramp up throttle to the target throttle value
-			_pusher_throttle = _params_standard.pusher_trans *
+			_pusher_throttle = _params->front_trans_throttle * time_since_trans_start / (_params_standard.pusher_ramp_dt * 1e6f);
-					   (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f);
 		}
 
 		// do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed
 		if (_airspeed_trans_blend_margin > 0.0f &&
-		    _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_blend &&
+		    _airspeed->indicated_airspeed_m_s >= _params->airspeed_blend &&
-		    (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f)
+		    time_since_trans_start > _params->front_trans_time_min * 1e6f) {
-		   ) {
+			mc_weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params->airspeed_blend) /
-			mc_weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params_standard.airspeed_blend) /
 				    _airspeed_trans_blend_margin;
 			// time based blending when no airspeed sensor is set
 
-		} else if (_params_standard.airspeed_disabled &&
+		} else if (_params->airspeed_mode == 1 &&
-			   hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1e6f) &&
+			   time_since_trans_start < _params->front_trans_time_min * 1e6f &&
-			   hrt_elapsed_time(&_vtol_schedule.transition_start) > ((_params_standard.front_trans_time_min / 2.0f) * 1e6f)
+			   time_since_trans_start > _params->front_trans_time_min * 1e6f / 2.0f) {
-			  ) {
+			mc_weight = 1.0f - ((time_since_trans_start -  _params->front_trans_time_min * 1e6f / 2.0f) /
-			mc_weight = 1.0f - ((float)(hrt_elapsed_time(&_vtol_schedule.transition_start) - ((
+					    (_params->front_trans_time_min * 1e6f / 2.0f));
-							    _params_standard.front_trans_time_min / 2.0f) * 1000000.0f)) /
-					    ((_params_standard.front_trans_time_min / 2.0f) * 1000000.0f));
 
 		}
 
@@ -320,8 +275,8 @@ void Standard::update_transition_state()
 		_v_att_sp->q_d_valid = true;
 
 		// check front transition timeout
-		if (_params_standard.front_trans_timeout > FLT_EPSILON) {
+		if (_params->front_trans_timeout > FLT_EPSILON) {
-			if (hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_timeout * 1e6f)) {
+			if (time_since_trans_start > _params->front_trans_timeout * 1e6f) {
 				// transition timeout occured, abort transition
 				_attc->abort_front_transition("Transition timeout");
 			}
@@ -335,22 +290,21 @@ void Standard::update_transition_state()
 		q_sp.copyTo(_v_att_sp->q_d);
 		_v_att_sp->q_d_valid = true;
 
-		hrt_abstime btrans_start;
+		float btrans_start = (float)_vtol_schedule.transition_start + _params_standard.reverse_delay * 1e6f;
-		btrans_start = _vtol_schedule.transition_start + uint64_t(_params_standard.reverse_delay) * 1000000.0f;
+
 		_pusher_throttle = 0.0f;
 
-		if (hrt_absolute_time() >= btrans_start) {
+		if (time_now >= btrans_start) {
 			// Handle throttle reversal for active breaking
-			float thrscale = (float)hrt_elapsed_time(&btrans_start) / (_params_standard.front_trans_dur *
+			float thrscale = (time_now - btrans_start) / (_params_standard.pusher_ramp_dt * 1e6f);
-					 1000000.0f);
 			thrscale = math::constrain(thrscale, 0.0f, 1.0f);
-			_pusher_throttle = thrscale * _params_standard.back_trans_throttle;
+			_pusher_throttle = thrscale * _params->back_trans_throttle;
 		}
 
 		// continually increase mc attitude control as we transition back to mc mode
 		if (_params_standard.back_trans_ramp > FLT_EPSILON) {
-			mc_weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
+			mc_weight = time_since_trans_start /
-				    ((_params_standard.back_trans_ramp) * 1000000.0f);
+				    ((_params_standard.back_trans_ramp) * 1e6f);
 
 		}
 

src/modules/vtol_att_control/standard.h

@@ -67,41 +67,23 @@ public:
 private:
 
 	struct {
-		float front_trans_dur;
+		float pusher_ramp_dt;
-		float back_trans_dur;
 		float back_trans_ramp;
-		float pusher_trans;
-		float airspeed_blend;
-		float airspeed_trans;
-		float front_trans_timeout;
-		float front_trans_time_min;
 		float down_pitch_max;
 		float forward_thrust_scale;
-		int32_t airspeed_disabled;
 		float pitch_setpoint_offset;
 		float reverse_output;
 		float reverse_delay;
-		float back_trans_throttle;
-		float mpc_xy_cruise;
 	} _params_standard;
 
 	struct {
-		param_t front_trans_dur;
+		param_t pusher_ramp_dt;
-		param_t back_trans_dur;
 		param_t back_trans_ramp;
-		param_t pusher_trans;
-		param_t airspeed_blend;
-		param_t airspeed_trans;
-		param_t front_trans_timeout;
-		param_t front_trans_time_min;
 		param_t down_pitch_max;
 		param_t forward_thrust_scale;
-		param_t airspeed_disabled;
 		param_t pitch_setpoint_offset;
 		param_t reverse_output;
 		param_t reverse_delay;
-		param_t back_trans_throttle;
-		param_t mpc_xy_cruise;
 	} _params_handles_standard;
 
 	enum vtol_mode {

src/modules/vtol_att_control/standard_params.c

@@ -39,16 +39,6 @@
  * @author Roman Bapst	<roman@px4.io>
  */
 
-/**
- * Target throttle value for pusher/puller motor during the transition to fw mode
- *
- * @min 0.0
- * @max 1.0
- * @increment 0.01
- * @decimal 3
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_FLOAT(VT_TRANS_THR, 0.6f);
 
 /**
  * Maximum allowed down-pitch the controller is able to demand. This prevents large, negative
@@ -113,14 +103,12 @@ PARAM_DEFINE_FLOAT(VT_B_REV_OUT, 0.0f);
 PARAM_DEFINE_FLOAT(VT_B_REV_DEL, 0.0f);
 
 /**
- * Thottle output during back transition
+ * Defines the time window during which the pusher throttle will be ramped up linearly to VT_F_TRANS_THR during a transition
- * For ESCs and mixers that support reverse thrust on low PWM values set this to a negative value to apply active breaking
+ * to fixed wing mode. Zero or negative values will produce an instant throttle rise to VT_F_TRANS_THR.
- * For ESCs that support thrust reversal with a control channel please set VT_B_REV_OUT and set this to a positive value to apply active breaking
  *
- * @min -1
+ * @max 20
- * @max 1
  * @increment 0.01
  * @decimal 2
  * @group VTOL Attitude Control
  */
-PARAM_DEFINE_FLOAT(VT_B_TRANS_THR, 0.0f);
+PARAM_DEFINE_FLOAT(VT_PSHER_RMP_DT, 3.0f);

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -77,6 +77,17 @@ VtolAttitudeControl::VtolAttitudeControl()
 	_params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM");
 	_params_handles.front_trans_time_min = param_find("VT_TRANS_MIN_TM");
 
+	_params_handles.front_trans_duration = param_find("VT_F_TRANS_DUR");
+	_params_handles.back_trans_duration = param_find("VT_B_TRANS_DUR");
+	_params_handles.transition_airspeed = param_find("VT_ARSP_TRANS");
+	_params_handles.front_trans_throttle = param_find("VT_F_TRANS_THR");
+	_params_handles.back_trans_throttle = param_find("VT_B_TRANS_THR");
+	_params_handles.airspeed_blend = param_find("VT_ARSP_BLEND");
+	_params_handles.airspeed_mode = param_find("FW_ARSP_MODE");
+	_params_handles.front_trans_timeout = param_find("VT_TRANS_TIMEOUT");
+	_params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE");
+
+
 	_params_handles.wv_takeoff = param_find("VT_WV_TKO_EN");
 	_params_handles.wv_land = param_find("VT_WV_LND_EN");
 	_params_handles.wv_loiter = param_find("VT_WV_LTR_EN");
@@ -487,6 +498,17 @@ VtolAttitudeControl::parameters_update()
 	param_get(_params_handles.wv_land, &l);
 	_params.wv_land = (l == 1);
 
+
+	param_get(_params_handles.front_trans_duration, &_params.front_trans_duration);
+	param_get(_params_handles.back_trans_duration, &_params.back_trans_duration);
+	param_get(_params_handles.transition_airspeed, &_params.transition_airspeed);
+	param_get(_params_handles.front_trans_throttle, &_params.front_trans_throttle);
+	param_get(_params_handles.back_trans_throttle, &_params.back_trans_throttle);
+	param_get(_params_handles.airspeed_blend, &_params.airspeed_blend);
+	param_get(_params_handles.airspeed_mode, &_params.airspeed_mode);
+	param_get(_params_handles.front_trans_timeout, &_params.front_trans_timeout);
+	param_get(_params_handles.mpc_xy_cruise, &_params.mpc_xy_cruise);
+
 	// update the parameters of the instances of base VtolType
 	if (_vtol_type != nullptr) {
 		_vtol_type->parameters_update();

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -192,6 +192,15 @@ private:
 		param_t wv_takeoff;
 		param_t wv_loiter;
 		param_t wv_land;
+		param_t front_trans_duration;
+		param_t back_trans_duration;
+		param_t transition_airspeed;
+		param_t front_trans_throttle;
+		param_t back_trans_throttle;
+		param_t airspeed_blend;
+		param_t airspeed_mode;
+		param_t front_trans_timeout;
+		param_t mpc_xy_cruise;
 	} _params_handles{};
 
 	/* for multicopters it is usual to have a non-zero idle speed of the engines

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -137,6 +137,36 @@ PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 5.0f);
  */
 PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 4.0f);
 
+/**
+ * Target throttle value for the transition to fixed wing flight.
+ * standard vtol: pusher
+ * tailsitter, tiltrotor: main throttle
+ *
+ * @min 0.0
+ * @max 1.0
+ * @increment 0.01
+ * @decimal 3
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_F_TRANS_THR, 0.7f);
+
+/**
+ * Target throttle value for the transition to hover flight.
+ * standard vtol: pusher
+ * tailsitter, tiltrotor: main throttle
+ *
+ * Note for standard vtol:
+ * For ESCs and mixers that support reverse thrust on low PWM values set this to a negative value to apply active breaking
+ * For ESCs that support thrust reversal with a control channel please set VT_B_REV_OUT and set this to a positive value to apply active breaking
+ *
+ * @min -1
+ * @max 1
+ * @increment 0.01
+ * @decimal 2
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_B_TRANS_THR, 0.0f);
+
 /**
  * Approximate deceleration during back transition
  *

src/modules/vtol_att_control/vtol_type.h

@@ -61,6 +61,15 @@ struct Params {
 	bool wv_takeoff;
 	bool wv_loiter;
 	bool wv_land;
+	float front_trans_duration;
+	float back_trans_duration;
+	float transition_airspeed;
+	float front_trans_throttle;
+	float back_trans_throttle;
+	float airspeed_blend;
+	int32_t airspeed_mode;
+	float front_trans_timeout;
+	float mpc_xy_cruise;
 };
 
 // Has to match 1:1 msg/vtol_vehicle_status.msg