fixed-wing runway takeoff: ramp in pitch constraints and throttle setpoint on takeoff rotation

- consolidate takeoff rotation transition times for pitch constraints and throttle setpoint with a single param
- consolidate pitch takeoff constraint parameters (remove rwto_max_pitch, use nominal max)
- input correct units to rwto pitch constraint getters
- encapsulate absolute time interpolator method for transitions
- start runway ops from idle throttle

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1490,9 +1490,9 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		}
 
 		// update tecs
-		const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get());
-		const float takeoff_pitch_min_deg = _runway_takeoff.getMinPitch(_takeoff_pitch_min.get(),
-						    _param_fw_p_lim_min.get());
+		const float pitch_max = _runway_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get()));
+		const float pitch_min = _runway_takeoff.getMinPitch(math::radians(_takeoff_pitch_min.get()),
+					math::radians(_param_fw_p_lim_min.get()));
 
 		if (_runway_takeoff.resetIntegrators()) {
 			// reset integrals except yaw (which also counts for the wheel controller)
@@ -1505,19 +1505,19 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		tecs_update_pitch_throttle(control_interval,
 					   altitude_setpoint_amsl,
 					   target_airspeed,
-					   radians(takeoff_pitch_min_deg),
-					   radians(takeoff_pitch_max_deg),
+					   pitch_min,
+					   pitch_max,
 					   _param_fw_thr_min.get(),
 					   _param_fw_thr_max.get(),
 					   false,
-					   radians(takeoff_pitch_min_deg),
+					   pitch_min,
 					   _param_sinkrate_target.get(),
 					   _param_fw_t_clmb_max.get());
 
 		_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation
 
 		_att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());
-		_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust());
+		_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust());
 
 		// apply flaps for takeoff according to the corresponding scale factor set via FW_FLAPS_TO_SCL
 		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_TAKEOFF;

src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp

@@ -137,24 +137,32 @@ float RunwayTakeoff::getYaw(float external_yaw_setpoint)
 	}
 }
 
-float RunwayTakeoff::getThrottle(const hrt_abstime &time_now, float external_throttle_setpoint)
+float RunwayTakeoff::getThrottle(const float idle_throttle, const float external_throttle_setpoint) const
 {
-	float throttle = 0.0f;
+	float throttle = idle_throttle;
 
 	switch (takeoff_state_) {
 	case RunwayTakeoffState::THROTTLE_RAMP:
-		throttle = ((time_now - time_initialized_) / (param_rwto_ramp_time_.get() * 1_s)) * param_rwto_max_thr_.get();
-		throttle = math::constrain(throttle, 0.0f, param_rwto_max_thr_.get());
+
+		throttle = interpolateValuesOverAbsoluteTime(idle_throttle, param_rwto_max_thr_.get(), time_initialized_,
+				param_rwto_ramp_time_.get());
+
 		break;
 
 	case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
 		throttle = param_rwto_max_thr_.get();
+
 		break;
 
-	default:
-		const float interpolator = math::constrain((time_now - takeoff_time_ * 1_s) / (kThrottleHysteresisTime * 1_s), 0.0f,
-					   1.0f);
-		throttle = external_throttle_setpoint * interpolator + (1.0f - interpolator) * param_rwto_max_thr_.get();
+	case RunwayTakeoffState::CLIMBOUT:
+		// ramp in throttle setpoint over takeoff rotation transition time
+		throttle = interpolateValuesOverAbsoluteTime(param_rwto_max_thr_.get(), external_throttle_setpoint, takeoff_time_,
+				param_rwto_rot_time_.get());
+
+		break;
+
+	case RunwayTakeoffState::FLY:
+		throttle = external_throttle_setpoint;
 	}
 
 	return throttle;
@@ -166,21 +174,33 @@ bool RunwayTakeoff::resetIntegrators()
 	return takeoff_state_ < RunwayTakeoffState::CLIMBOUT;
 }
 
-float RunwayTakeoff::getMinPitch(float min_pitch_in_climbout, float min_pitch)
+float RunwayTakeoff::getMinPitch(float min_pitch_in_climbout, float min_pitch) const
 {
-	if (takeoff_state_ < RunwayTakeoffState::FLY) {
-		return min_pitch_in_climbout;
+	if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) {
+		// constrain to the taxi pitch setpoint
+		return math::radians(param_rwto_psp_.get() - 0.01f);
+
+	} else if (takeoff_state_ < RunwayTakeoffState::FLY) {
+		// ramp in the climbout pitch constraint over the rotation transition time
+		const float taxi_pitch_min = math::radians(param_rwto_psp_.get() - 0.01f);
+		return interpolateValuesOverAbsoluteTime(taxi_pitch_min, min_pitch_in_climbout, takeoff_time_,
+				param_rwto_rot_time_.get());
 
 	} else {
 		return min_pitch;
 	}
 }
 
-float RunwayTakeoff::getMaxPitch(const float max_pitch)
+float RunwayTakeoff::getMaxPitch(const float max_pitch) const
 {
-	// use max pitch from parameter if set
-	if (takeoff_state_ < RunwayTakeoffState::FLY && param_rwto_max_pitch_.get() > kMinMaxPitch) {
-		return param_rwto_max_pitch_.get();
+	if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) {
+		// constrain to the taxi pitch setpoint
+		return math::radians(param_rwto_psp_.get() + 0.01f);
+
+	} else if (takeoff_state_ < RunwayTakeoffState::FLY) {
+		// ramp in the climbout pitch constraint over the rotation transition time
+		const float taxi_pitch_max = math::radians(param_rwto_psp_.get() + 0.01f);
+		return interpolateValuesOverAbsoluteTime(taxi_pitch_max, max_pitch, takeoff_time_, param_rwto_rot_time_.get());
 
 	} else {
 		return max_pitch;
@@ -194,4 +214,13 @@ void RunwayTakeoff::reset()
 	takeoff_time_ = 0;
 }
 
+float RunwayTakeoff::interpolateValuesOverAbsoluteTime(const float start_value, const float end_value,
+		const hrt_abstime &start_time, const float interpolation_time) const
+{
+	const float seconds_since_start = hrt_elapsed_time(&start_time) / float(1_s);
+	const float interpolator = math::constrain(seconds_since_start / interpolation_time, 0.0f, 1.0f);
+
+	return interpolator * end_value + (1.0f - interpolator) * start_value;
+}
+
 } // namespace runwaytakeoff

src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h

@@ -140,27 +140,27 @@ public:
 	/**
 	 * @brief Returns the throttle setpoint.
 	 *
-	 * Ramps up over RWTO_RAMP_TIME to RWTO_MAX_THR until the aircraft lifts off the runway, then passes
-	 * through the externally defined throttle setting.
+	 * Ramps up over RWTO_RAMP_TIME to RWTO_MAX_THR until the aircraft lifts off the runway and then
+	 * ramps from RWTO_MAX_THR to the externally defined throttle setting over the takeoff rotation time
 	 *
-	 * @param time_now Absolute time since system boot [us]
+	 * @param idle_throttle normalized [0,1]
 	 * @param external_throttle_setpoint Externally commanded throttle setpoint (usually from TECS), normalized [0,1]
 	 * @return Throttle setpoint, normalized [0,1]
 	 */
-	float getThrottle(const hrt_abstime &time_now, const float external_throttle_setpoint);
+	float getThrottle(const float idle_throttle, const float external_throttle_setpoint) const;
 
 	/**
 	 * @param min_pitch_in_climbout Minimum pitch angle during climbout [rad]
 	 * @param min_pitch Externally commanded minimum pitch angle [rad]
 	 * @return Minimum pitch angle [rad]
 	 */
-	float getMinPitch(const float min_pitch_in_climbout, const float min_pitch);
+	float getMinPitch(const float min_pitch_in_climbout, const float min_pitch) const;
 
 	/**
 	 * @param max_pitch Externally commanded maximum pitch angle [rad]
 	 * @return Maximum pitch angle [rad]
 	 */
-	float getMaxPitch(const float max_pitch);
+	float getMaxPitch(const float max_pitch) const;
 
 	/**
 	 * @return Runway takeoff starting position in global frame (lat, lon) [deg]
@@ -181,19 +181,19 @@ public:
 	 */
 	void reset();
 
+	/**
+	 * @brief Linearly interpolates between a start and end value over an absolute time span.
+	 *
+	 * @param start_value
+	 * @param end_value
+	 * @param start_time Absolute start time [us]
+	 * @param interpolation_time The time span to interpolate over [s]
+	 * @return interpolated value
+	 */
+	float interpolateValuesOverAbsoluteTime(const float start_value, const float end_value, const hrt_abstime &start_time,
+						const float interpolation_time) const;
+
 private:
-	/**
-	 * Minimum allowed maximum pitch constraint (from parameter) for runway takeoff. [rad]
-	 */
-	static constexpr float kMinMaxPitch = 0.1f;
-
-	/**
-	 * Time from takeoff during which the takeoff throttle is transitioned to the externally commanded throttle.
-	 * Used to avoid throttle jumps immediately on lift off when switching from the constant, parameterized, takeoff
-	 * throttle to the airspeed/altitude controller's commanded throttle [s]
-	 */
-	static constexpr float kThrottleHysteresisTime = 2.0f;
-
 	/**
 	 * Current state of runway takeoff procedure
 	 */
@@ -232,9 +232,9 @@ private:
 		(ParamInt<px4::params::RWTO_HDG>) param_rwto_hdg_,
 		(ParamFloat<px4::params::RWTO_MAX_THR>) param_rwto_max_thr_,
 		(ParamFloat<px4::params::RWTO_PSP>) param_rwto_psp_,
-		(ParamFloat<px4::params::RWTO_MAX_PITCH>) param_rwto_max_pitch_,
 		(ParamFloat<px4::params::RWTO_RAMP_TIME>) param_rwto_ramp_time_,
-		(ParamFloat<px4::params::RWTO_ROT_AIRSPD>) param_rwto_rot_airspd_
+		(ParamFloat<px4::params::RWTO_ROT_AIRSPD>) param_rwto_rot_airspd_,
+		(ParamFloat<px4::params::RWTO_ROT_TIME>) param_rwto_rot_time_
 	)
 };
 

src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c

@@ -77,7 +77,7 @@ PARAM_DEFINE_INT32(RWTO_HDG, 0);
 PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0);
 
 /**
- * Pitch setpoint during taxi / before takeoff airspeed is reached.
+ * Pitch setpoint during taxi / before takeoff rotation airspeed is reached.
  *
  * A taildragger with steerable wheel might need to pitch up
  * a little to keep its wheel on the ground before airspeed
@@ -92,21 +92,6 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0);
  */
 PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0);
 
-/**
- * Max pitch during takeoff.
- *
- * Fixed-wing settings are used if set to 0. Note that there is also a minimum
- * pitch of 10 degrees during takeoff, so this must be larger if set.
- *
- * @unit deg
- * @min 0.0
- * @max 60.0
- * @decimal 1
- * @increment 0.5
- * @group Runway Takeoff
- */
-PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0);
-
 /**
  * Throttle ramp up time for runway takeoff
  *
@@ -157,3 +142,16 @@ PARAM_DEFINE_INT32(RWTO_NUDGE, 1);
  * @group Runway Takeoff
  */
 PARAM_DEFINE_FLOAT(RWTO_ROT_AIRSPD, -1.0f);
+
+/**
+ * Takeoff rotation time
+ *
+ * This is the time desired to linearly ramp in takeoff pitch constraints during the takeoff rotation
+ *
+ * @unit s
+ * @min 0.1
+ * @decimal 1
+ * @increment 0.1
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_ROT_TIME, 1.0f);