fw pos ctrl: refactor takeoff mode

- post takeoff, the aircraft follows the infinite line sourced from the launch point in the direction of the takeoff waypoint
- takeoff waypoint altitude is used as a clearance altitude, set such that once above, the aircraft has cleared all ground occlusions and may proceed with the mission
- runway takeoff state machine simplified to throttle ramp, clamped to runway, climbout, and fly
- throttle ramp must complete before switching to next state to avoid a jump in throttle setpoint just after takeoff if the takeoff airspeed is reached before the ramp is complete
- roll constraints near ground post takeoff removed from runway takeoff class (handled externally now)
- minimum airspeed in TECS is reduced to takeoff speed (if necessary) to lower the underspeed detection bound
- lateral-directional guidance uses a different period parameter during ground roll

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1379,44 +1379,27 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 
 void
 FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const float control_interval,
-		const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
-		const position_setpoint_s &pos_sp_curr)
+		const Vector2d &global_position, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
 {
-	/* current waypoint (the one currently heading for) */
-	Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
-	Vector2d prev_wp{0, 0}; /* previous waypoint */
-	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
-	Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
-
-	if (pos_sp_prev.valid) {
-		prev_wp(0) = pos_sp_prev.lat;
-		prev_wp(1) = pos_sp_prev.lon;
-
-	} else {
-		/*
-		 * No valid previous waypoint, go for the current wp.
-		 * This is automatically handled by the L1 library.
-		 */
-		prev_wp(0) = pos_sp_curr.lat;
-		prev_wp(1) = pos_sp_curr.lon;
-	}
-
-	if (_skipping_takeoff_detection) {
-		_launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
-	}
-
 	if (!_control_mode.flag_armed) {
-		_runway_takeoff.reset();
-		_launchDetector.reset();
-		_launch_detection_state = LAUNCHDETECTION_RES_NONE;
-		_launch_detection_notify = 0;
+		reset_takeoff_state();
 	}
 
+	// for now taking current position setpoint altitude as clearance altitude. this is the altitude we need to
+	// clear all occlusions in the takeoff path
+	const float clearance_altitude_amsl = pos_sp_curr.alt;
+
+	// set the altitude to something above the clearance altitude to ensure the vehicle climbs past the value
+	// (navigator will accept the takeoff as complete once crossing the clearance altitude)
+	const float altitude_setpoint_amsl = clearance_altitude_amsl + _param_nav_fw_alt_rad.get();
+
+	Vector2f local_2D_position{_local_pos.x, _local_pos.y};
+
 	float terrain_alt = _takeoff_ground_alt;
 
 	if (_runway_takeoff.runwayTakeoffEnabled()) {
 		if (!_runway_takeoff.isInitialized()) {
-			_runway_takeoff.init(now, _yaw, _current_latitude, _current_longitude);
+			_runway_takeoff.init(now, _yaw, global_position);
 
 			/* need this already before takeoff is detected
 			 * doesn't matter if it gets reset when takeoff is detected eventually */
@@ -1432,57 +1415,94 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 		terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt);
 
-		// update runway takeoff helper
-		_runway_takeoff.update(now, _airspeed, _current_altitude - terrain_alt,
-				       _current_latitude, _current_longitude, &_mavlink_log_pub);
+		_runway_takeoff.update(now, _airspeed, _current_altitude - terrain_alt, clearance_altitude_amsl - _takeoff_ground_alt,
+				       &_mavlink_log_pub);
 
-		float target_airspeed = get_auto_airspeed_setpoint(control_interval,
-					_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed);
+		const float takeoff_airspeed = _runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get();
+		float target_airspeed = get_auto_airspeed_setpoint(control_interval, takeoff_airspeed, ground_speed);
 
-		/*
-		 * Update navigation: _runway_takeoff returns the start WP according to mode and phase.
-		 * If we use the navigator heading or not is decided later.
-		 */
-		Vector2f prev_wp_local = _global_local_proj_ref.project(_runway_takeoff.getStartWP()(0),
-					 _runway_takeoff.getStartWP()(1));
+		// yaw control is disabled once in "taking off" state
+		_att_sp.fw_control_yaw = _runway_takeoff.controlYaw();
+
+		// tune up the lateral position control guidance when on the ground
+		if (_att_sp.fw_control_yaw) {
+			_npfg.setPeriod(_param_rwto_l1_period.get());
+			_l1_control.set_l1_period(_param_rwto_l1_period.get());
+
+		}
+
+		const Vector2f start_pos_local = _global_local_proj_ref.project(_runway_takeoff.getStartPosition()(0),
+						 _runway_takeoff.getStartPosition()(1));
+		const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
+
+		// the bearing from runway start to the takeoff waypoint is followed until the clearance altitude is exceeded
+		const Vector2f takeoff_bearing_vector = calculateTakeoffBearingVector(start_pos_local, takeoff_waypoint_local);
 
 		if (_param_fw_use_npfg.get()) {
 			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-			// NOTE: current waypoint is passed twice to trigger the "point following" logic -- TODO: create
-			// point following navigation interface instead of this hack.
-			_npfg.navigateWaypoints(curr_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
+			_npfg.navigatePathTangent(local_2D_position, start_pos_local, takeoff_bearing_vector, ground_speed,
+						  _wind_vel, 0.0f);
+
 			_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());
+
 			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
+			// use npfg's bearing to commanded course, controlled via yaw angle while on runway
+			const float bearing = _npfg.getBearing();
+
+			// heading hold mode will override this bearing setpoint
+			_att_sp.yaw_body = _runway_takeoff.getYaw(bearing);
+
 		} else {
-			_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
-			_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint());
+			// make a fake waypoint in the direction of the takeoff bearing
+			const Vector2f virtual_waypoint = start_pos_local + takeoff_bearing_vector * HDG_HOLD_DIST_NEXT;
+
+			_l1_control.navigate_waypoints(start_pos_local, virtual_waypoint, local_2D_position, ground_speed);
+
+			const float l1_roll_setpoint = _l1_control.get_roll_setpoint();
+			_att_sp.roll_body = _runway_takeoff.getRoll(l1_roll_setpoint);
+
+			// use l1's nav bearing to command course, controlled via yaw angle while on runway
+			const float bearing = _l1_control.nav_bearing();
+
+			// heading hold mode will override this bearing setpoint
+			_att_sp.yaw_body = _runway_takeoff.getYaw(bearing);
 		}
 
-		_att_sp.yaw_body = _runway_takeoff.getYaw(_yaw);
-
-
 		// update tecs
 		const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get());
+		const float takeoff_pitch_min_climbout_deg = _runway_takeoff.getMinPitch(_takeoff_pitch_min.get(),
+				_param_fw_p_lim_min.get());
+
+		if (_runway_takeoff.resetIntegrators()) {
+			// reset integrals except yaw (which also counts for the wheel controller)
+			_att_sp.roll_reset_integral = true;
+			_att_sp.pitch_reset_integral = true;
+
+			// throttle is open loop anyway during ground roll, no need to wind up the integrator
+			_tecs.resetIntegrals();
+		}
+
+		if (takeoff_airspeed < _param_fw_airspd_min.get()) {
+			// adjust underspeed detection bounds for takeoff airspeed
+			_tecs.set_equivalent_airspeed_min(takeoff_airspeed);
+		}
 
 		tecs_update_pitch_throttle(control_interval,
-					   pos_sp_curr.alt,
+					   altitude_setpoint_amsl,
 					   target_airspeed,
 					   radians(_param_fw_p_lim_min.get()),
 					   radians(takeoff_pitch_max_deg),
 					   _param_fw_thr_min.get(),
-					   _param_fw_thr_max.get(), // XXX should we also set runway_takeoff_throttle here?
+					   _param_fw_thr_max.get(),
 					   _runway_takeoff.climbout(),
-					   radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get())));
+					   radians(takeoff_pitch_min_climbout_deg));
+
+		_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation
 
-		// assign values
-		_att_sp.fw_control_yaw = _runway_takeoff.controlYaw();
 		_att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());
-
-		// reset integrals except yaw (which also counts for the wheel controller)
-		_att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators();
-		_att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators();
+		_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, 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;
@@ -1490,7 +1510,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 	} else {
 		/* Perform launch detection */
-		if (_launchDetector.launchDetectionEnabled() &&
+		if (!_skipping_takeoff_detection && _launchDetector.launchDetectionEnabled() &&
 		    _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
 
 			if (_control_mode.flag_armed) {
@@ -1515,23 +1535,36 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 			_launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
 		}
 
+		if (!_launch_detected && _launch_detection_state != LAUNCHDETECTION_RES_NONE) {
+			_launch_detected = true;
+			_launch_global_position = global_position;
+		}
+
+		const Vector2f launch_local_position = _global_local_proj_ref.project(_launch_global_position(0),
+						       _launch_global_position(1));
+		const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
+
+		// the bearing from launch to the takeoff waypoint is followed until the clearance altitude is exceeded
+		const Vector2f takeoff_bearing_vector = calculateTakeoffBearingVector(launch_local_position, takeoff_waypoint_local);
+
 		/* Set control values depending on the detection state */
 		if (_launch_detection_state != LAUNCHDETECTION_RES_NONE) {
 			/* Launch has been detected, hence we have to control the plane. */
 
 			float target_airspeed = get_auto_airspeed_setpoint(control_interval, _param_fw_airspd_trim.get(), ground_speed);
 
-			Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
-
 			if (_param_fw_use_npfg.get()) {
 				_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 				_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-				_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
+				_npfg.navigatePathTangent(local_2D_position, launch_local_position, takeoff_bearing_vector, ground_speed, _wind_vel,
+							  0.0f);
 				_att_sp.roll_body = _npfg.getRollSetpoint();
 				target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
 			} else {
-				_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
+				// make a fake waypoint in the direction of the takeoff bearing
+				const Vector2f virtual_waypoint = launch_local_position + takeoff_bearing_vector * HDG_HOLD_DIST_NEXT;
+				_l1_control.navigate_waypoints(launch_local_position, virtual_waypoint, local_2D_position, ground_speed);
 				_att_sp.roll_body = _l1_control.get_roll_setpoint();
 			}
 
@@ -1545,16 +1578,13 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 				takeoff_throttle = _param_fw_thr_idle.get();
 			}
 
-			/* select maximum pitch: the launchdetector may impose another limit for the pitch
-			 * depending on the state of the launch */
-			const float takeoff_pitch_max_deg = _launchDetector.getPitchMax(_param_fw_p_lim_max.get());
-			const float altitude_error = pos_sp_curr.alt - _current_altitude;
+			if (_current_altitude < clearance_altitude_amsl) {
+				// select maximum pitch: the launchdetector may impose another limit for the pitch
+				// depending on the state of the launch
+				const float takeoff_pitch_max_deg = _launchDetector.getPitchMax(_param_fw_p_lim_max.get());
 
-			/* apply minimum pitch and limit roll if target altitude is not within climbout_diff meters */
-			if (_param_fw_clmbout_diff.get() > 0.0f && altitude_error > _param_fw_clmbout_diff.get()) {
-				/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
 				tecs_update_pitch_throttle(control_interval,
-							   pos_sp_curr.alt,
+							   altitude_setpoint_amsl,
 							   target_airspeed,
 							   radians(_param_fw_p_lim_min.get()),
 							   radians(takeoff_pitch_max_deg),
@@ -1565,7 +1595,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 			} else {
 				tecs_update_pitch_throttle(control_interval,
-							   pos_sp_curr.alt,
+							   altitude_setpoint_amsl,
 							   target_airspeed,
 							   radians(_param_fw_p_lim_min.get()),
 							   radians(_param_fw_p_lim_max.get()),
@@ -1575,6 +1605,16 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 							   radians(_param_fw_p_lim_min.get()));
 			}
 
+			if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
+				// explicitly set idle throttle until motors are enabled
+				_att_sp.thrust_body[0] = _param_fw_thr_idle.get();
+
+			} else {
+				_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
+			}
+
+			_att_sp.pitch_body = get_tecs_pitch();
+
 		} else {
 			/* Tell the attitude controller to stop integrating while we are waiting
 			 * for the launch */
@@ -1584,6 +1624,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 			/* Set default roll and pitch setpoints during detection phase */
 			_att_sp.roll_body = 0.0f;
+			_att_sp.thrust_body[0] = _param_fw_thr_idle.get();
 			_att_sp.pitch_body = radians(_takeoff_pitch_min.get());
 		}
 
@@ -1591,28 +1632,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
 	}
 
-	if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && !_runway_takeoff.runwayTakeoffEnabled()) {
-
-		/* making sure again that the correct thrust is used,
-		 * without depending on library calls for safety reasons.
-		   the pre-takeoff throttle and the idle throttle normally map to the same parameter. */
-		_att_sp.thrust_body[0] = _param_fw_thr_idle.get();
-
-	} else if (_runway_takeoff.runwayTakeoffEnabled()) {
-
-		_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust());
-
-	} else {
-		/* Copy thrust and pitch values from tecs */
-		// when we are landed state we want the motor to spin at idle speed
-		_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
-	}
-
-	// only use TECS pitch setpoint if launch has not been detected and runway takeoff is not enabled
-	if (!(_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled())) {
-		_att_sp.pitch_body = get_tecs_pitch();
-	}
-
 	_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt);
 
 	if (!_vehicle_status.in_transition_to_fw) {
@@ -2329,6 +2348,10 @@ FixedwingPositionControl::Run()
 		_tecs.set_speed_weight(_param_fw_t_spdweight.get());
 		_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
 
+		// restore lateral-directional guidance parameters (changed in takeoff mode)
+		_npfg.setPeriod(_param_npfg_period.get());
+		_l1_control.set_l1_period(_param_fw_l1_period.get());
+
 		_att_sp.roll_reset_integral = false;
 		_att_sp.pitch_reset_integral = false;
 		_att_sp.yaw_reset_integral = false;
@@ -2368,8 +2391,7 @@ FixedwingPositionControl::Run()
 			}
 
 		case FW_POSCTRL_MODE_AUTO_TAKEOFF: {
-				control_auto_takeoff(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous,
-						     _pos_sp_triplet.current);
+				control_auto_takeoff(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
 				break;
 			}
 
@@ -2440,6 +2462,8 @@ FixedwingPositionControl::reset_takeoff_state()
 	_launchDetector.reset();
 	_launch_detection_state = LAUNCHDETECTION_RES_NONE;
 	_launch_detection_notify = 0;
+
+	_launch_detected = false;
 }
 
 void
@@ -2624,6 +2648,27 @@ FixedwingPositionControl::constrainRollNearGround(const float roll_setpoint, con
 	return math::constrain(roll_setpoint, -roll_wingtip_strike, roll_wingtip_strike);
 }
 
+Vector2f
+FixedwingPositionControl::calculateTakeoffBearingVector(const Vector2f &launch_position,
+		const Vector2f &takeoff_waypoint) const
+{
+	Vector2f takeoff_bearing_vector = takeoff_waypoint - launch_position;
+
+	if (takeoff_bearing_vector.norm_squared() > FLT_EPSILON) {
+		takeoff_bearing_vector.normalize();
+
+	} else {
+		// TODO: a new bearing only based fixed-wing takeoff command / mission item will get rid of the need
+		// for this check
+
+		// takeoff in the direction of the airframe
+		takeoff_bearing_vector(0) = cosf(_yaw);
+		takeoff_bearing_vector(0) = sinf(_yaw);
+	}
+
+	return takeoff_bearing_vector;
+}
+
 void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint)
 {
 	if (_global_local_proj_ref.isInitialized()) {

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -216,6 +216,12 @@ private:
 	// [m] ground altitude where the plane was launched
 	float _takeoff_ground_alt{0.0f};
 
+	// true if a launch, specifically using the launch detector, has been detected
+	bool _launch_detected{false};
+
+	// [deg] global position of the vehicle at the time launch is detected (using launch detector)
+	Vector2d _launch_global_position{0, 0};
+
 	// [rad] yaw setpoint for manual position mode heading hold
 	float _hdg_hold_yaw{0.0f};
 
@@ -525,13 +531,12 @@ private:
 	 *
 	 * @param now Current system time [us]
 	 * @param control_interval Time since last position control call [s]
-	 * @param curr_pos Current 2D local position vector of vehicle [m]
+	 * @param global_position Vechile global position [deg]
 	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
-	 * @param pos_sp_prev previous position setpoint
 	 * @param pos_sp_curr current position setpoint
 	 */
-	void control_auto_takeoff(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
-				  const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
+	void control_auto_takeoff(const hrt_abstime &now, const float control_interval, const Vector2d &global_position,
+				  const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr);
 
 	/**
 	 * @brief Controls automatic landing.
@@ -656,6 +661,15 @@ private:
 	 */
 	float constrainRollNearGround(const float roll_setpoint, const float altitude, const float terrain_altitude) const;
 
+	/**
+	 * @brief Calculates the unit takeoff bearing vector from the launch position to takeoff waypont.
+	 *
+	 * @param launch_position Vehicle launch position in local coordinates (NE) [m]
+	 * @param takeoff_waypoint Takeoff waypoint position in local coordinates (NE) [m]
+	 * @return Unit takeoff bearing vector
+	 */
+	Vector2f calculateTakeoffBearingVector(const Vector2f &launch_position, const Vector2f &takeoff_waypoint) const;
+
 	DEFINE_PARAMETERS(
 
 		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
@@ -749,7 +763,9 @@ private:
 		(ParamFloat<px4::params::WEIGHT_GROSS>) _param_weight_gross,
 
 		(ParamFloat<px4::params::FW_WING_SPAN>) _param_fw_wing_span,
-		(ParamFloat<px4::params::FW_WING_HEIGHT>) _param_fw_wing_height
+		(ParamFloat<px4::params::FW_WING_HEIGHT>) _param_fw_wing_height,
+
+		(ParamFloat<px4::params::RWTO_L1_PERIOD>) _param_rwto_l1_period
 	)
 
 };

src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp

@@ -52,75 +52,45 @@ using namespace time_literals;
 namespace runwaytakeoff
 {
 
-RunwayTakeoff::RunwayTakeoff(ModuleParams *parent) :
-	ModuleParams(parent),
-	_state(),
-	_initialized(false),
-	_initialized_time(0),
-	_init_yaw(0),
-	_climbout(false)
+void RunwayTakeoff::init(const hrt_abstime &time_now, const float initial_yaw, const matrix::Vector2d &start_pos_global)
 {
+	initial_yaw_ = initial_yaw;
+	start_pos_global_ = start_pos_global;
+	takeoff_state_ = RunwayTakeoffState::THROTTLE_RAMP;
+	climbout_ = true; // this is true until climbout is finished
+	initialized_ = true;
+	time_initialized_ = time_now;
+	takeoff_time_ = 0;
 }
 
-void RunwayTakeoff::init(const hrt_abstime &now, float yaw, double current_lat, double current_lon)
+void RunwayTakeoff::update(const hrt_abstime &time_now, const float calibrated_airspeed, const float vehicle_altitude,
+			   const float clearance_altitude, orb_advert_t *mavlink_log_pub)
 {
-	_init_yaw = yaw;
-	_initialized = true;
-	_state = RunwayTakeoffState::THROTTLE_RAMP;
-	_initialized_time = now;
-	_climbout = true; // this is true until climbout is finished
-	_start_wp(0) = current_lat;
-	_start_wp(1) = current_lon;
-}
-
-void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl,
-			   double current_lat, double current_lon, orb_advert_t *mavlink_log_pub)
-{
-	switch (_state) {
+	switch (takeoff_state_) {
 	case RunwayTakeoffState::THROTTLE_RAMP:
-		if (((now - _initialized_time) > (_param_rwto_ramp_time.get() * 1_s))
-		    || (airspeed > (_param_fw_airspd_min.get() * _param_rwto_airspd_scl.get() * 0.9f))) {
-
-			_state = RunwayTakeoffState::CLAMPED_TO_RUNWAY;
+		if ((time_now - time_initialized_) > (param_rwto_ramp_time_.get() * 1_s)) {
+			takeoff_state_ = RunwayTakeoffState::CLAMPED_TO_RUNWAY;
 		}
 
 		break;
 
 	case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
-		if (airspeed > _param_fw_airspd_min.get() * _param_rwto_airspd_scl.get()) {
-			_state = RunwayTakeoffState::TAKEOFF;
-			mavlink_log_info(mavlink_log_pub, "#Takeoff airspeed reached\t");
+		if (calibrated_airspeed > param_fw_airspd_min_.get() * param_rwto_airspd_scl_.get()) {
+			takeoff_time_ = time_now;
+			takeoff_state_ = RunwayTakeoffState::CLIMBOUT;
+			mavlink_log_info(mavlink_log_pub, "Takeoff airspeed reached, climbout\t");
 			events::send(events::ID("runway_takeoff_reached_airspeed"), events::Log::Info,
-				     "Takeoff airspeed reached");
-		}
-
-		break;
-
-	case RunwayTakeoffState::TAKEOFF:
-		if (alt_agl > _param_rwto_nav_alt.get()) {
-			_state = RunwayTakeoffState::CLIMBOUT;
-
-			/*
-			 * If we started in heading hold mode, move the navigation start WP to the current location now.
-			 * The navigator will take this as starting point to navigate towards the takeoff WP.
-			 */
-			if (_param_rwto_hdg.get() == 0) {
-				_start_wp(0) = current_lat;
-				_start_wp(1) = current_lon;
-			}
-
-			mavlink_log_info(mavlink_log_pub, "#Climbout\t");
-			events::send(events::ID("runway_takeoff_climbout"), events::Log::Info, "Climbout");
+				     "Takeoff airspeed reached, climbout");
 		}
 
 		break;
 
 	case RunwayTakeoffState::CLIMBOUT:
-		if (alt_agl > _param_fw_clmbout_diff.get()) {
-			_climbout = false;
-			_state = RunwayTakeoffState::FLY;
-			mavlink_log_info(mavlink_log_pub, "#Navigating to waypoint\t");
-			events::send(events::ID("runway_takeoff_nav_to_wp"), events::Log::Info, "Navigating to waypoint");
+		if (vehicle_altitude > clearance_altitude) {
+			climbout_ = false;
+			takeoff_state_ = RunwayTakeoffState::FLY;
+			mavlink_log_info(mavlink_log_pub, "Reached clearance altitude\t");
+			events::send(events::ID("runway_takeoff_reached_clearance_altitude"), events::Log::Info, "Reached clearance altitude");
 		}
 
 		break;
@@ -130,132 +100,96 @@ void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl
 	}
 }
 
-/*
- * Returns true as long as we're below navigation altitude
- */
 bool RunwayTakeoff::controlYaw()
 {
 	// keep controlling yaw directly until we start navigation
-	return _state < RunwayTakeoffState::CLIMBOUT;
+	return takeoff_state_ < RunwayTakeoffState::CLIMBOUT;
 }
 
-/*
- * Returns pitch setpoint to use.
- *
- * Limited (parameter) as long as the plane is on runway. Otherwise
- * use the one from TECS
- */
-float RunwayTakeoff::getPitch(float tecsPitch)
+float RunwayTakeoff::getPitch(float external_pitch_setpoint)
 {
-	if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) {
-		return math::radians(_param_rwto_psp.get());
+	if (takeoff_state_ <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) {
+		return math::radians(param_rwto_psp_.get());
 	}
 
-	return tecsPitch;
+	return external_pitch_setpoint;
 }
 
-/*
- * Returns the roll setpoint to use.
- */
-float RunwayTakeoff::getRoll(float navigatorRoll)
+float RunwayTakeoff::getRoll(float external_roll_setpoint)
 {
 	// until we have enough ground clearance, set roll to 0
-	if (_state < RunwayTakeoffState::CLIMBOUT) {
+	if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) {
 		return 0.0f;
 	}
 
-	// allow some roll during climbout
-	else if (_state < RunwayTakeoffState::FLY) {
-		return math::constrain(navigatorRoll,
-				       math::radians(-_param_rwto_max_roll.get()),
-				       math::radians(_param_rwto_max_roll.get()));
-	}
-
-	return navigatorRoll;
+	return external_roll_setpoint;
 }
 
-/*
- * Returns the yaw setpoint to use.
- *
- * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the
- * runway. When it has enough ground clearance we start navigation towards WP.
- */
-float RunwayTakeoff::getYaw(float navigatorYaw)
+float RunwayTakeoff::getYaw(float external_yaw_setpoint)
 {
-	if (_param_rwto_hdg.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) {
-		return _init_yaw;
+	if (param_rwto_hdg_.get() == 0 && takeoff_state_ < RunwayTakeoffState::CLIMBOUT) {
+		return initial_yaw_;
 
 	} else {
-		return navigatorYaw;
+		return external_yaw_setpoint;
 	}
 }
 
-/*
- * Returns the throttle setpoint to use.
- *
- * Ramps up in the beginning, until it lifts off the runway it is set to
- * parameter value, then it returns the TECS throttle.
- */
-float RunwayTakeoff::getThrottle(const hrt_abstime &now, float tecsThrottle)
+float RunwayTakeoff::getThrottle(const hrt_abstime &time_now, float external_throttle_setpoint)
 {
-	switch (_state) {
-	case RunwayTakeoffState::THROTTLE_RAMP: {
-			float throttle = ((now - _initialized_time) / (_param_rwto_ramp_time.get() * 1_s)) * _param_rwto_max_thr.get();
-			return math::min(throttle, _param_rwto_max_thr.get());
-		}
+	float throttle = 0.0f;
+
+	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());
+		break;
 
 	case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
-		return _param_rwto_max_thr.get();
+		throttle = param_rwto_max_thr_.get();
+		break;
 
 	default:
-		return tecsThrottle;
+		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();
 	}
+
+	return throttle;
 }
 
 bool RunwayTakeoff::resetIntegrators()
 {
 	// reset integrators if we're still on runway
-	return _state < RunwayTakeoffState::TAKEOFF;
+	return takeoff_state_ < RunwayTakeoffState::CLIMBOUT;
 }
 
-/*
- * Returns the minimum pitch for TECS to use.
- *
- * In climbout we either want what was set on the waypoint (sp_min) but at least
- * the climbtout minimum pitch (parameter).
- * Otherwise use the minimum that is enforced generally (parameter).
- */
-float RunwayTakeoff::getMinPitch(float climbout_min, float min)
+float RunwayTakeoff::getMinPitch(float min_pitch_in_climbout, float min_pitch)
 {
-	if (_state < RunwayTakeoffState::FLY) {
-		return climbout_min;
+	if (takeoff_state_ < RunwayTakeoffState::FLY) {
+		return min_pitch_in_climbout;
 
 	} else {
-		return min;
+		return min_pitch;
 	}
 }
 
-/*
- * Returns the maximum pitch for TECS to use.
- *
- * Limited by parameter (if set) until climbout is done.
- */
-float RunwayTakeoff::getMaxPitch(float max)
+float RunwayTakeoff::getMaxPitch(const float max_pitch)
 {
-	// use max pitch from parameter if set (> 0.1)
-	if (_state < RunwayTakeoffState::FLY && _param_rwto_max_pitch.get() > 0.1f) {
-		return _param_rwto_max_pitch.get();
-	}
+	// use max pitch from parameter if set
+	if (takeoff_state_ < RunwayTakeoffState::FLY && param_rwto_max_pitch_.get() > kMinMaxPitch) {
+		return param_rwto_max_pitch_.get();
 
-	else {
-		return max;
+	} else {
+		return max_pitch;
 	}
 }
 
 void RunwayTakeoff::reset()
 {
-	_initialized = false;
-	_state = RunwayTakeoffState::THROTTLE_RAMP;
+	initialized_ = false;
+	takeoff_state_ = RunwayTakeoffState::THROTTLE_RAMP;
+	takeoff_time_ = 0;
 }
 
-}
+} // namespace runwaytakeoff

src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h

@@ -55,71 +55,204 @@ namespace runwaytakeoff
 {
 
 enum RunwayTakeoffState {
-	THROTTLE_RAMP = 0, /**< ramping up throttle */
-	CLAMPED_TO_RUNWAY = 1, /**< clamped to runway, controlling yaw directly (wheel or rudder) */
-	TAKEOFF = 2, /**< taking off, get ground clearance, roll 0 */
-	CLIMBOUT = 3, /**< climbout to safe height before navigation, roll limited */
-	FLY = 4 /**< fly towards takeoff waypoint */
+	THROTTLE_RAMP = 0, // ramping up throttle
+	CLAMPED_TO_RUNWAY, // clamped to runway, controlling yaw directly (wheel or rudder)
+	CLIMBOUT, // climbout to safe height before navigation
+	FLY // navigate freely
 };
 
 class __EXPORT RunwayTakeoff : public ModuleParams
 {
 public:
-	RunwayTakeoff(ModuleParams *parent);
+	RunwayTakeoff(ModuleParams *parent) : ModuleParams(parent) {}
 	~RunwayTakeoff() = default;
 
-	void init(const hrt_abstime &now, float yaw, double current_lat, double current_lon);
-	void update(const hrt_abstime &now, float airspeed, float alt_agl, double current_lat, double current_lon,
-		    orb_advert_t *mavlink_log_pub);
+	/**
+	 * @brief Initializes the state machine.
+	 *
+	 * @param time_now Absolute time since system boot [us]
+	 * @param initial_yaw Vehicle yaw angle at time of initialization [us]
+	 * @param start_pos_global Vehicle global (lat, lon) position at time of initialization [deg]
+	 */
+	void init(const hrt_abstime &time_now, const float initial_yaw, const matrix::Vector2d &start_pos_global);
 
-	RunwayTakeoffState getState() { return _state; }
-	bool isInitialized() { return _initialized; }
+	/**
+	 * @brief Updates the state machine based on the current vehicle condition.
+	 *
+	 * @param time_now Absolute time since system boot [us]
+	 * @param calibrated_airspeed Vehicle calibrated airspeed [m/s]
+	 * @param vehicle_altitude Vehicle altitude (AGL) [m]
+	 * @param clearance_altitude Altitude (AGL) above which we have cleared all occlusions in the runway path [m]
+	 * @param mavlink_log_pub
+	 */
+	void update(const hrt_abstime &time_now, const float calibrated_airspeed, const float vehicle_altitude,
+		    const float clearance_altitude, orb_advert_t *mavlink_log_pub);
 
-	bool runwayTakeoffEnabled() { return _param_rwto_tkoff.get(); }
-	float getMinAirspeedScaling() { return _param_rwto_airspd_scl.get(); }
-	float getInitYaw() { return _init_yaw; }
+	/**
+	 * @return Current takeoff state
+	 */
+	RunwayTakeoffState getState() { return takeoff_state_; }
 
+	/**
+	 * @return The state machine is initialized
+	 */
+	bool isInitialized() { return initialized_; }
+
+	/**
+	 * @return Runway takeoff is enabled
+	 */
+	bool runwayTakeoffEnabled() { return param_rwto_tkoff_.get(); }
+
+	/**
+	 * @return Scale factor for minimum indicated airspeed
+	 */
+	float getMinAirspeedScaling() { return param_rwto_airspd_scl_.get(); }
+
+	/**
+	 * @return Initial vehicle yaw angle [rad]
+	 */
+	float getInitYaw() { return initial_yaw_; }
+
+	/**
+	 * @return The vehicle should control yaw via rudder or nose gear
+	 */
 	bool controlYaw();
-	bool climbout() { return _climbout; }
-	float getPitch(float tecsPitch);
-	float getRoll(float navigatorRoll);
-	float getYaw(float navigatorYaw);
-	float getThrottle(const hrt_abstime &now, float tecsThrottle);
-	bool resetIntegrators();
-	float getMinPitch(float climbout_min, float min);
-	float getMaxPitch(float max);
-	const matrix::Vector2d &getStartWP() const { return _start_wp; };
+
+	/**
+	 * @return TECS should be commanded to climbout mode
+	 */
+	bool climbout() { return climbout_; }
+
+	/**
+	 * @param external_pitch_setpoint Externally commanded pitch angle setpoint (usually from TECS) [rad]
+	 * @return Pitch angle setpoint (limited while plane is on runway) [rad]
+	 */
+	float getPitch(float external_pitch_setpoint);
+
+	/**
+	 * @param external_roll_setpoint Externally commanded roll angle setpoint (usually from L1) [rad]
+	 * @return Roll angle setpoint [rad]
+	 */
+	float getRoll(float external_roll_setpoint);
+
+	/**
+	 * @brief Returns the appropriate yaw angle setpoint.
+	 *
+	 * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the runway.
+	 * When it has enough ground clearance we start navigation towards WP.
+	 *
+	 * @param external_yaw_setpoint Externally commanded yaw angle setpoint [rad]
+	 * @return Yaw angle setpoint [rad]
+	 */
+	float getYaw(float external_yaw_setpoint);
+
+	/**
+	 * @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.
+	 *
+	 * @param time_now Absolute time since system boot [us]
+	 * @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);
+
+	/**
+	 * @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);
+
+	/**
+	 * @param max_pitch Externally commanded maximum pitch angle [rad]
+	 * @return Maximum pitch angle [rad]
+	 */
+	float getMaxPitch(const float max_pitch);
+
+	/**
+	 * @return Runway takeoff starting position in global frame (lat, lon) [deg]
+	 */
+	const matrix::Vector2d &getStartPosition() const { return start_pos_global_; };
 
 	// NOTE: this is only to be used for mistaken mode transitions to takeoff while already in air
-	void forceSetFlyState() { _state = RunwayTakeoffState::FLY; }
+	void forceSetFlyState() { takeoff_state_ = RunwayTakeoffState::FLY; }
 
+	/**
+	 * @return If the attitude / rate control integrators should be continually reset.
+	 * This is the case during ground roll.
+	 */
+	bool resetIntegrators();
+
+	/**
+	 * @brief Reset the state machine.
+	 */
 	void reset();
 
 private:
-	/** state variables **/
-	RunwayTakeoffState _state{THROTTLE_RAMP};
-	bool _initialized{false};
-	hrt_abstime _initialized_time{0};
-	float _init_yaw{0.f};
-	bool _climbout{false};
-	matrix::Vector2d _start_wp;
+	/**
+	 * 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
+	 */
+	RunwayTakeoffState takeoff_state_{THROTTLE_RAMP};
+
+	/**
+	 * True if the runway state machine is initialized
+	 */
+	bool initialized_{false};
+
+	/**
+	 * The absolute time since system boot at which the state machine was intialized [us]
+	 */
+	hrt_abstime time_initialized_{0};
+
+	/**
+	 * The absolute time the takeoff state transitions from CLAMPED_TO_RUNWAY -> CLIMBOUT [us]
+	 */
+	hrt_abstime takeoff_time_{0};
+
+	/**
+	 * Initial yaw of the vehicle on first pass through the runway takeoff state machine.
+	 * used for heading hold mode. [rad]
+	 */
+	float initial_yaw_{0.f};
+
+	/**
+	 * True if TECS should be commanded to "climbout" mode.
+	 */
+	bool climbout_{false};
+
+	/**
+	 * The global (lat, lon) position of the vehicle on first pass through the runway takeoff state machine. The
+	 * takeoff path emanates from this point to correct for any GNSS uncertainty from the planned takeoff point. The
+	 * vehicle should accordingly be set on the center of the runway before engaging the mission. [deg]
+	 */
+	matrix::Vector2d start_pos_global_{};
 
 	DEFINE_PARAMETERS(
-		(ParamBool<px4::params::RWTO_TKOFF>) _param_rwto_tkoff,
-		(ParamInt<px4::params::RWTO_HDG>) _param_rwto_hdg,
-		(ParamFloat<px4::params::RWTO_NAV_ALT>) _param_rwto_nav_alt,
-		(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_MAX_ROLL>) _param_rwto_max_roll,
-		(ParamFloat<px4::params::RWTO_AIRSPD_SCL>) _param_rwto_airspd_scl,
-		(ParamFloat<px4::params::RWTO_RAMP_TIME>) _param_rwto_ramp_time,
-		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
-		(ParamFloat<px4::params::FW_CLMBOUT_DIFF>) _param_fw_clmbout_diff
+		(ParamBool<px4::params::RWTO_TKOFF>) param_rwto_tkoff_,
+		(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_AIRSPD_SCL>) param_rwto_airspd_scl_,
+		(ParamFloat<px4::params::RWTO_RAMP_TIME>) param_rwto_ramp_time_,
+		(ParamFloat<px4::params::FW_AIRSPD_MIN>) param_fw_airspd_min_
 	)
-
 };
 
-}
+} // namespace runwaytakeoff
 
 #endif // RUNWAYTAKEOFF_H

src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c

@@ -50,32 +50,17 @@ PARAM_DEFINE_INT32(RWTO_TKOFF, 0);
 /**
  * Specifies which heading should be held during runnway takeoff.
  *
- * 0: airframe heading, 1: heading towards takeoff waypoint
+ * 0: airframe heading
+ * 1: position control along runway direction (defined by operator in MAV_CMD)
  *
  * @value 0 Airframe
- * @value 1 Waypoint
+ * @value 1 Runway
  * @min 0
  * @max 1
  * @group Runway Takeoff
  */
 PARAM_DEFINE_INT32(RWTO_HDG, 0);
 
-/**
- * Altitude AGL at which we have enough ground clearance to allow some roll.
- *
- * Until RWTO_NAV_ALT is reached the plane is held level and only
- * rudder is used to keep the heading (see RWTO_HDG). This should be below
- * FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0.
- *
- * @unit m
- * @min 0.0
- * @max 100.0
- * @decimal 1
- * @increment 1
- * @group Runway Takeoff
- */
-PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0);
-
 /**
  * Max throttle during runway takeoff.
  *
@@ -121,21 +106,6 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0);
  */
 PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0);
 
-/**
- * Max roll during climbout.
- *
- * Roll is limited during climbout to ensure enough lift and prevents aggressive
- * navigation before we're on a safe height.
- *
- * @unit deg
- * @min 0.0
- * @max 60.0
- * @decimal 1
- * @increment 0.5
- * @group Runway Takeoff
- */
-PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0);
-
 /**
  * Min airspeed scaling factor for takeoff.
  *
@@ -162,3 +132,15 @@ PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3);
  * @group Runway Takeoff
  */
 PARAM_DEFINE_FLOAT(RWTO_RAMP_TIME, 2.0f);
+
+/**
+ * L1 period while steering on runway
+ *
+ * @unit s
+ * @min 1.0
+ * @max 100.0
+ * @decimal 1
+ * @increment 0.1
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_L1_PERIOD, 5.0f);

src/modules/navigator/mission_block.cpp

@@ -182,6 +182,13 @@ MissionBlock::is_mission_item_reached()
 				_waypoint_position_reached = true;
 			}
 
+		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
+			   && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+			/* fixed-wing takeoff is reached once the vehicle has exceeded the takeoff altitude */
+			if (_navigator->get_global_position()->alt > mission_item_altitude_amsl) {
+				_waypoint_position_reached = true;
+			}
+
 		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
 			   && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) {
 			/* for takeoff mission items use the parameter for the takeoff acceptance radius */