fw pos ctrl: some incremental cleanup of the class

- make less ambiguous variable names
- fix some incorrect comments
- add units to variable descriptions
- start documenting methods

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -64,7 +64,7 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 		int32_t vt_type = -1;
 		param_get(param_find("VT_TYPE"), &vt_type);
 
-		_vtol_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
+		_is_vtol_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
 	}
 
 	// limit to 50 Hz
@@ -290,7 +290,7 @@ FixedwingPositionControl::airspeed_poll()
 
 			airspeed_valid = true;
 
-			_airspeed_last_valid = airspeed_validated.timestamp;
+			_time_airspeed_last_valid = airspeed_validated.timestamp;
 			_airspeed = airspeed_validated.calibrated_airspeed_m_s;
 
 			_eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, 0.9f, 2.0f);
@@ -298,7 +298,7 @@ FixedwingPositionControl::airspeed_poll()
 
 	} else {
 		// no airspeed updates for one second
-		if (airspeed_valid && (hrt_elapsed_time(&_airspeed_last_valid) > 1_s)) {
+		if (airspeed_valid && (hrt_elapsed_time(&_time_airspeed_last_valid) > 1_s)) {
 			airspeed_valid = false;
 		}
 	}
@@ -370,7 +370,7 @@ FixedwingPositionControl::vehicle_attitude_poll()
 
 		// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
 		// between multirotor and fixed wing flight
-		if (_vtol_tailsitter) {
+		if (_is_vtol_tailsitter) {
 			const Dcmf R_offset{Eulerf{0.f, M_PI_2_F, 0.f}};
 			R = R * R_offset;
 
@@ -755,19 +755,28 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
 
 	if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) ||
 	     _control_mode.flag_control_offboard_enabled) && pos_sp_curr_valid) {
+
 		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
-			// TAKEOFF: handle like a regular POSITION setpoint if already flying
-			if (!in_takeoff_situation() && (_airspeed >= _param_fw_airspd_min.get() || !_airspeed_valid)) {
-				// SETPOINT_TYPE_TAKEOFF -> SETPOINT_TYPE_POSITION
+
+			if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) {
 				_control_mode_current = FW_POSCTRL_MODE_AUTO;
 
+				// in this case we want the waypoint handled as a position setpoint -- a submode in control_auto()
+				_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+
 			} else {
 				_control_mode_current = FW_POSCTRL_MODE_AUTO_TAKEOFF;
 			}
 
-		} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND
-			   && !_vehicle_status.in_transition_mode) {
-			_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING;
+		} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
+
+			if (!_vehicle_status.in_transition_mode) {
+				_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING;
+
+			} else {
+				// in this case we want the waypoint handled as a position setpoint -- a submode in control_auto()
+				_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+			}
 
 		} else {
 			_control_mode_current = FW_POSCTRL_MODE_AUTO;
@@ -827,55 +836,26 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
 }
 
 void
-FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &curr_pos,
-				       const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
-				       const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next)
+FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
 {
-	const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
-					 MAX_AUTO_TIMESTEP);
-	_last_time_position_control_called = now;
-
-	if (_param_fw_use_npfg.get()) {
-		_npfg.setDt(dt);
-
-	} else {
-		_l1_control.set_dt(dt);
-	}
-
 	/* save time when airplane is in air */
 	if (!_was_in_air && !_landed) {
 		_was_in_air = true;
 		_time_went_in_air = now;
-		_takeoff_ground_alt = _current_altitude;
+		_takeoff_ground_alt = _current_altitude; // XXX: is this really a good idea?
 	}
 
 	/* reset flag when airplane landed */
 	if (_landed) {
 		_was_in_air = false;
 	}
+}
 
-	/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
-	if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
-		/* reset integrators */
-		_tecs.reset_state();
-	}
-
-	/* reset hold yaw */
-	_hdg_hold_yaw = _yaw;
-
-	/* get circle mode */
-	const bool was_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
-
-	/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
-	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
-	_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
-
-	/* Initialize attitude controller integrator reset flags to 0 */
-	_att_sp.roll_reset_integral = false;
-	_att_sp.pitch_reset_integral = false;
-	_att_sp.yaw_reset_integral = false;
-
-	position_setpoint_s current_sp = pos_sp_curr;
+void
+FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_setpoint_s &current_sp)
+{
+	// TODO: velocity, altitude, or just a heading hold position mode should be used for this, not position
+	// shifting hacks
 
 	if (_vehicle_status.in_transition_to_fw) {
 
@@ -899,6 +879,15 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 		_transition_waypoint(0) = static_cast<double>(NAN);
 		_transition_waypoint(1) = static_cast<double>(NAN);
 	}
+}
+
+void
+FixedwingPositionControl::control_auto(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 position_setpoint_s &pos_sp_next)
+{
+	position_setpoint_s current_sp = pos_sp_curr;
+	move_position_setpoint_for_vtol_transition(current_sp);
 
 	const uint8_t position_sp_type = handle_setpoint_type(current_sp.type, current_sp);
 
@@ -909,6 +898,8 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 		publishOrbitStatus(current_sp);
 	}
 
+	const bool was_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
+
 	switch (position_sp_type) {
 	case position_setpoint_s::SETPOINT_TYPE_IDLE:
 		_att_sp.thrust_body[0] = 0.0f;
@@ -920,28 +911,18 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 		break;
 
 	case position_setpoint_s::SETPOINT_TYPE_POSITION:
-		control_auto_position(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
+		control_auto_position(now, control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
 		break;
 
 	case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
-		control_auto_velocity(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
+		control_auto_velocity(now, control_interval, curr_pos, ground_speed, pos_sp_prev);
 		break;
 
 	case position_setpoint_s::SETPOINT_TYPE_LOITER:
-		control_auto_loiter(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
+		control_auto_loiter(now, control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
 		break;
 	}
 
-	/* reset landing state */
-	if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) {
-		reset_landing_state();
-	}
-
-	/* reset takeoff/launch state */
-	if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
-		reset_takeoff_state();
-	}
-
 	if (was_circle_mode && !_l1_control.circle_mode()) {
 		/* just kicked out of loiter, reset roll integrals */
 		_att_sp.roll_reset_integral = true;
@@ -968,8 +949,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 void
 FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const hrt_abstime &now)
 {
-	// only control altitude and airspeed ("fixed-bank loiter")
-
 	tecs_update_pitch_throttle(now, _current_altitude,
 				   _param_fw_airspd_trim.get(),
 				   radians(_param_fw_p_lim_min.get()),
@@ -999,8 +978,6 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const hrt_abstime &no
 void
 FixedwingPositionControl::control_auto_descend(const hrt_abstime &now)
 {
-	// only control height rate
-
 	// Hard-code descend rate to 0.5m/s. This is a compromise to give the system to recover,
 	// but not letting it drift too far away.
 	const float descend_rate = -0.5f;
@@ -1043,15 +1020,8 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
 
 	uint8_t position_sp_type = setpoint_type;
 
-	if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
-		// TAKEOFF: handle like a regular POSITION setpoint if already flying
-		if (!in_takeoff_situation() && (_airspeed >= _param_fw_airspd_min.get() || !_airspeed_valid)) {
-			// SETPOINT_TYPE_TAKEOFF -> SETPOINT_TYPE_POSITION
-			position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-		}
-
-	} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
-		   || pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
+	if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
+	    || pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
 
 		float dist_xy = -1.f;
 		float dist_z = -1.f;
@@ -1088,17 +1058,13 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
 		}
 	}
 
-	// set to type position during VTOL transitions in Land mode (to not start FW landing logic)
-	if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND && _vehicle_status.in_transition_mode) {
-		position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-	}
-
 	return position_sp_type;
 }
 
 void
-FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
-		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
+FixedwingPositionControl::control_auto_position(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 float acc_rad = (_param_fw_use_npfg.get()) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f);
 	Vector2d curr_wp{0, 0};
@@ -1151,8 +1117,7 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
 	// Altitude first order hold (FOH)
 	if (pos_sp_prev.valid && PX4_ISFINITE(pos_sp_prev.alt) &&
 	    ((pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_POSITION) ||
-	     (pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER) ||
-	     (pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF))
+	     (pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER))
 	   ) {
 		const float d_curr_prev = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1),
 					  pos_sp_prev.lat, pos_sp_prev.lon);
@@ -1181,7 +1146,7 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
 		}
 	}
 
-	float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt);
+	float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, control_interval);
 	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));
 	Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
@@ -1227,8 +1192,8 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
 }
 
 void
-FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
-		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
+FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const float control_interval,
+		const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
 {
 	float tecs_fw_thr_min;
 	float tecs_fw_thr_max;
@@ -1262,7 +1227,7 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl
 	Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy};
 	_target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0)));
 
-	float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt);
+	float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, control_interval);
 
 	if (_param_fw_use_npfg.get()) {
 		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
@@ -1295,7 +1260,8 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl
 }
 
 void
-FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
+FixedwingPositionControl::control_auto_loiter(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 position_setpoint_s &pos_sp_next)
 {
@@ -1376,7 +1342,7 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
 		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
 	}
 
-	float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, dt);
+	float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, control_interval);
 
 	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));
@@ -1429,20 +1395,10 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
 }
 
 void
-FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vector2d &curr_pos,
+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 float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
-					 MAX_AUTO_TIMESTEP);
-	_last_time_position_control_called = now;
-
-	if (_param_fw_use_npfg.get()) {
-		_npfg.setDt(dt);
-
-	} else {
-		_l1_control.set_dt(dt);
-	}
-
 	/* 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 */
@@ -1489,7 +1445,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
 
 		float target_airspeed = get_auto_airspeed_setpoint(now,
 					_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed,
-					dt);
+					control_interval);
 
 		/*
 		 * Update navigation: _runway_takeoff returns the start WP according to mode and phase.
@@ -1501,7 +1457,9 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
 		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);
+			// 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);
 			_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());
 			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
@@ -1554,7 +1512,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
 				}
 
 				/* Detect launch using body X (forward) acceleration */
-				_launchDetector.update(dt, _body_acceleration(0));
+				_launchDetector.update(control_interval, _body_acceleration(0));
 
 				/* update our copy of the launch detection state */
 				_launch_detection_state = _launchDetector.getLaunchDetected();
@@ -1570,7 +1528,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
 			/* Launch has been detected, hence we have to control the plane. */
 
 			float target_airspeed = get_auto_airspeed_setpoint(now, _param_fw_airspd_trim.get(), ground_speed,
-						dt);
+						control_interval);
 
 			Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
 
@@ -1673,19 +1631,12 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
 }
 
 void
-FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos,
+FixedwingPositionControl::control_auto_landing(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 float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
-					 MAX_AUTO_TIMESTEP);
-	_last_time_position_control_called = now;
-
-	if (_param_fw_use_npfg.get()) {
-		_npfg.setDt(dt);
-
-	} else {
-		_l1_control.set_dt(dt);
-	}
+	// Enable tighter altitude control for landings
+	_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
 
 	/* current waypoint (the one currently heading for) */
 	Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
@@ -1704,9 +1655,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 		prev_wp(1) = pos_sp_curr.lon;
 	}
 
-	// Enable tighter altitude control for landings
-	_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
-
 	// save time at which we started landing and reset abort_landing
 	if (_time_started_landing == 0) {
 		reset_landing_state();
@@ -1784,10 +1732,10 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 			// all good, have valid terrain altitude
 			float terrain_vpos = _local_pos.dist_bottom + _local_pos.z;
 			terrain_alt = (_local_pos.ref_alt - terrain_vpos);
-			_t_alt_prev_valid = terrain_alt;
-			_time_last_t_alt = now;
+			_last_valid_terrain_alt_estimate = terrain_alt;
+			_last_time_terrain_alt_was_valid = now;
 
-		} else if (_time_last_t_alt == 0) {
+		} else if (_last_time_terrain_alt_was_valid == 0) {
 			// we have started landing phase but don't have valid terrain
 			// wait for some time, maybe we will soon get a valid estimate
 			// until then just use the altitude of the landing waypoint
@@ -1800,16 +1748,16 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 				abort_landing(true);
 			}
 
-		} else if ((!_local_pos.dist_bottom_valid && (now - _time_last_t_alt) < T_ALT_TIMEOUT)
+		} else if ((!_local_pos.dist_bottom_valid && (now - _last_time_terrain_alt_was_valid) < T_ALT_TIMEOUT)
 			   || _land_noreturn_vertical) {
 			// use previous terrain estimate for some time and hope to recover
 			// if we are already flaring (land_noreturn_vertical) then just
 			//  go with the old estimate
-			terrain_alt = _t_alt_prev_valid;
+			terrain_alt = _last_valid_terrain_alt_estimate;
 
 		} else {
 			// terrain alt was not valid for long time, abort landing
-			terrain_alt = _t_alt_prev_valid;
+			terrain_alt = _last_valid_terrain_alt_estimate;
 			abort_landing(true);
 		}
 	}
@@ -1852,7 +1800,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 		}
 
 		const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
-		float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_land, ground_speed, dt);
+		float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_land, ground_speed, control_interval);
 
 		const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f;
 
@@ -1969,7 +1917,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 		}
 
 		const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
-		float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_approach, ground_speed, dt);
+		float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_approach, ground_speed, control_interval);
 
 		/* lateral guidance */
 		if (_param_fw_use_npfg.get()) {
@@ -2033,7 +1981,6 @@ FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const
 		const Vector2f &ground_speed)
 {
 	/* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
-	_last_time_position_control_called = now;
 
 	/* Get demanded airspeed */
 	float altctrl_airspeed = get_manual_airspeed_setpoint();
@@ -2096,13 +2043,10 @@ FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const
 }
 
 void
-FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const Vector2d &curr_pos,
+FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const float control_interval,
+		const Vector2d &curr_pos,
 		const Vector2f &ground_speed)
 {
-	const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
-					 MAX_AUTO_TIMESTEP);
-	_last_time_position_control_called = now;
-
 	// if we assume that user is taking off then help by demanding altitude setpoint well above ground
 	// and set limit to pitch angle to prevent steering into ground
 	// this will only affect planes and not VTOL
@@ -2218,9 +2162,9 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
 		float roll_sp_new = _manual_control_setpoint.y * radians(_param_fw_r_lim.get());
 		const float roll_rate_slew_rad = radians(_param_fw_l1_r_slew_max.get());
 
-		if (dt > 0.f && roll_rate_slew_rad > 0.f) {
-			roll_sp_new = constrain(roll_sp_new, _att_sp.roll_body - roll_rate_slew_rad * dt,
-						_att_sp.roll_body + roll_rate_slew_rad * dt);
+		if (control_interval > 0.f && roll_rate_slew_rad > 0.f) {
+			roll_sp_new = constrain(roll_sp_new, _att_sp.roll_body - roll_rate_slew_rad * control_interval,
+						_att_sp.roll_body + roll_rate_slew_rad * control_interval);
 		}
 
 		_att_sp.roll_body = roll_sp_new;
@@ -2281,6 +2225,10 @@ FixedwingPositionControl::Run()
 
 	if (_local_pos_sub.update(&_local_pos)) {
 
+		const float control_interval = math::constrain((_local_pos.timestamp - _last_time_position_control_called) * 1e-6f,
+					       MIN_AUTO_TIMESTEP, MAX_AUTO_TIMESTEP);
+		_last_time_position_control_called = _local_pos.timestamp;
+
 		// check for parameter updates
 		if (_parameter_update_sub.updated()) {
 			// clear update
@@ -2421,11 +2369,31 @@ FixedwingPositionControl::Run()
 
 		set_control_mode_current(_local_pos.timestamp, _pos_sp_triplet.current.valid);
 
-		_att_sp.fw_control_yaw = false;		// by default we don't want yaw to be contoller directly with rudder
+		update_in_air_states(_local_pos.timestamp);
+
+		// update lateral guidance timesteps for slewrates
+		if (_param_fw_use_npfg.get()) {
+			_npfg.setDt(control_interval);
+
+		} else {
+			_l1_control.set_dt(control_interval);
+		}
+
+		// restore nominal TECS parameters in case changed intermittently (e.g. in landing handling)
+		_tecs.set_speed_weight(_param_fw_t_spdweight.get());
+		_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
+
+		_att_sp.roll_reset_integral = false;
+		_att_sp.pitch_reset_integral = false;
+		_att_sp.yaw_reset_integral = false;
+
+		// by default we don't want yaw to be contoller directly with rudder
+		_att_sp.fw_control_yaw = false;
 
 		switch (_control_mode_current) {
 		case FW_POSCTRL_MODE_AUTO: {
-				control_auto(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
+				control_auto(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous,
+					     _pos_sp_triplet.current,
 					     _pos_sp_triplet.next);
 				break;
 			}
@@ -2441,17 +2409,19 @@ FixedwingPositionControl::Run()
 			}
 
 		case FW_POSCTRL_MODE_AUTO_LANDING: {
-				control_auto_landing(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current);
+				control_auto_landing(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous,
+						     _pos_sp_triplet.current);
 				break;
 			}
 
 		case FW_POSCTRL_MODE_AUTO_TAKEOFF: {
-				control_auto_takeoff(_local_pos.timestamp, 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.previous,
+						     _pos_sp_triplet.current);
 				break;
 			}
 
 		case FW_POSCTRL_MODE_MANUAL_POSITION: {
-				control_manual_position(_local_pos.timestamp, curr_pos, ground_speed);
+				control_manual_position(_local_pos.timestamp, control_interval, curr_pos, ground_speed);
 				break;
 			}
 
@@ -2472,11 +2442,21 @@ FixedwingPositionControl::Run()
 				_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
 				_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
 
+				_tecs.reset_state();
+
 				break;
 			}
 
 		}
 
+		if (_control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING) {
+			reset_landing_state();
+		}
+
+		if (_control_mode_current != FW_POSCTRL_MODE_AUTO_TAKEOFF) {
+			reset_takeoff_state();
+		}
+
 		if (_control_mode_current != FW_POSCTRL_MODE_OTHER) {
 
 			if (_control_mode.flag_control_manual_enabled) {
@@ -2537,7 +2517,7 @@ FixedwingPositionControl::reset_landing_state()
 	_time_started_landing = 0;
 
 	// reset terrain estimation relevant values
-	_time_last_t_alt = 0;
+	_last_time_terrain_alt_was_valid = 0;
 
 	_land_noreturn_horizontal = false;
 	_land_noreturn_vertical = false;
@@ -2588,8 +2568,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 		bool climbout_mode, float climbout_pitch_min_rad,
 		bool disable_underspeed_detection, float hgt_rate_sp)
 {
-	const float dt = math::constrain((now - _last_tecs_update) * 1e-6f, MIN_AUTO_TIMESTEP, MAX_AUTO_TIMESTEP);
-	_last_tecs_update = now;
+	const float dt = math::constrain((now - _time_last_tecs_update) * 1e-6f, MIN_AUTO_TIMESTEP, MAX_AUTO_TIMESTEP);
+	_time_last_tecs_update = now;
 
 	// do not run TECS if we are not in air
 	bool run_tecs = !_landed;
@@ -2608,24 +2588,25 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 			// set this to transition airspeed to init tecs correctly
 			if (_param_fw_arsp_mode.get() == 1 && PX4_ISFINITE(_param_airspeed_trans)) {
 				// some vtols fly without airspeed sensor
-				_asp_after_transition = _param_airspeed_trans;
+				_airspeed_after_transition = _param_airspeed_trans;
 
 			} else {
-				_asp_after_transition = _airspeed;
+				_airspeed_after_transition = _airspeed;
 			}
 
-			_asp_after_transition = constrain(_asp_after_transition, _param_fw_airspd_min.get(), _param_fw_airspd_max.get());
+			_airspeed_after_transition = constrain(_airspeed_after_transition, _param_fw_airspd_min.get(),
+							       _param_fw_airspd_max.get());
 
 		} else if (_was_in_transition) {
 			// after transition we ramp up desired airspeed from the speed we had coming out of the transition
-			_asp_after_transition += dt * 2.0f; // increase 2m/s
+			_airspeed_after_transition += dt * 2.0f; // increase 2m/s
 
-			if (_asp_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
-				airspeed_sp = max(_asp_after_transition, _airspeed);
+			if (_airspeed_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
+				airspeed_sp = max(_airspeed_after_transition, _airspeed);
 
 			} else {
 				_was_in_transition = false;
-				_asp_after_transition = 0.0f;
+				_airspeed_after_transition = 0.0f;
 			}
 		}
 	}

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -103,25 +103,38 @@ using namespace time_literals;
 using matrix::Vector2d;
 using matrix::Vector2f;
 
-static constexpr float HDG_HOLD_DIST_NEXT =
-	3000.0f; // initial distance of waypoint in front of plane in heading hold mode
-static constexpr float HDG_HOLD_REACHED_DIST =
-	1000.0f; // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
-static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f; // distance by which previous waypoint is set behind the plane
-static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f;	// max yawrate at which plane locks yaw for heading hold mode
-static constexpr float HDG_HOLD_MAN_INPUT_THRESH =
-	0.01f; // max manual roll/yaw input from user which does not change the locked heading
+// [m] initial distance of waypoint in front of plane in heading hold mode
+static constexpr float HDG_HOLD_DIST_NEXT = 3000.0f;
 
-static constexpr hrt_abstime T_ALT_TIMEOUT = 1_s; // time after which we abort landing if terrain estimate is not valid
+// [m] distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
+static constexpr float HDG_HOLD_REACHED_DIST = 1000.0f;
 
-static constexpr float THROTTLE_THRESH =
-	0.05f;	///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
-static constexpr float ASPD_SP_SLEW_RATE = 1.f; // slew rate limit for airspeed setpoint changes [m/s/S]
-static constexpr hrt_abstime T_WIND_EST_TIMEOUT =
-	10_s; // time after which the wind estimate is disabled if no longer updating
+// [m] distance by which previous waypoint is set behind the plane
+static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f;
 
-static constexpr float MIN_AUTO_TIMESTEP = 0.01f;  // minimum time step between auto control updates [s]
-static constexpr float MAX_AUTO_TIMESTEP = 0.05f;  // maximum time step between auto control updates [s]
+// [rad/s] max yawrate at which plane locks yaw for heading hold mode
+static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f;
+
+// [.] max manual roll/yaw normalized input from user which does not change the locked heading
+static constexpr float HDG_HOLD_MAN_INPUT_THRESH = 0.01f;
+
+// [us] time after which we abort landing if terrain estimate is not valid
+static constexpr hrt_abstime T_ALT_TIMEOUT = 1_s;
+
+// [.] max throttle from user which will not lead to motors spinning up in altitude controlled modes
+static constexpr float THROTTLE_THRESH = 0.05f;
+
+// [m/s/s] slew rate limit for airspeed setpoint changes
+static constexpr float ASPD_SP_SLEW_RATE = 1.f;
+
+// [us] time after which the wind estimate is disabled if no longer updating
+static constexpr hrt_abstime T_WIND_EST_TIMEOUT = 10_s;
+
+// [s] minimum time step between auto control updates
+static constexpr float MIN_AUTO_TIMESTEP = 0.01f;
+
+// [s] maximum time step between auto control updates
+static constexpr float MAX_AUTO_TIMESTEP = 0.05f;
 
 class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
 	public px4::WorkItem
@@ -164,39 +177,43 @@ private:
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
-	uORB::Publication<vehicle_attitude_setpoint_s>		_attitude_sp_pub;
-	uORB::Publication<vehicle_local_position_setpoint_s> 	_local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};	///< vehicle local position setpoint publication
-	uORB::Publication<npfg_status_s> _npfg_status_pub{ORB_ID(npfg_status)}; ///< NPFG status publication
-	uORB::Publication<position_controller_status_s>		_pos_ctrl_status_pub{ORB_ID(position_controller_status)};			///< navigation capabilities publication
-	uORB::Publication<position_controller_landing_status_s>	_pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};	///< landing status publication
-	uORB::Publication<tecs_status_s>			_tecs_status_pub{ORB_ID(tecs_status)};						///< TECS status publication
-	uORB::PublicationMulti<orbit_status_s>			_orbit_status_pub{ORB_ID(orbit_status)};
+	uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
+	uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};
+	uORB::Publication<npfg_status_s> _npfg_status_pub{ORB_ID(npfg_status)};
+	uORB::Publication<position_controller_status_s>	_pos_ctrl_status_pub{ORB_ID(position_controller_status)};
+	uORB::Publication<position_controller_landing_status_s>	_pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};
+	uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};
+	uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
 
-	manual_control_setpoint_s	_manual_control_setpoint {};			///< r/c channel data
-	position_setpoint_triplet_s	_pos_sp_triplet {};		///< triplet of mission items
-	vehicle_attitude_setpoint_s	_att_sp {};			///< vehicle attitude setpoint
-	vehicle_control_mode_s		_control_mode {};		///< control mode
-	vehicle_local_position_s	_local_pos {};			///< vehicle local position
-	vehicle_status_s		_vehicle_status {};		///< vehicle status
+	manual_control_setpoint_s _manual_control_setpoint {}; // r/c channel data
+	position_setpoint_triplet_s _pos_sp_triplet {}; // triplet of mission items
+	vehicle_attitude_setpoint_s _att_sp {}; // vehicle attitude setpoint
+	vehicle_control_mode_s _control_mode {};
+	vehicle_local_position_s _local_pos {};	// vehicle local position
+	vehicle_status_s _vehicle_status {}; // vehicle status
 
 	double _current_latitude{0};
 	double _current_longitude{0};
 	float _current_altitude{0.f};
 
-	perf_counter_t	_loop_perf;				///< loop performance counter
+	perf_counter_t _loop_perf; // loop performance counter
 
 	MapProjection _global_local_proj_ref{};
-	float	_global_local_alt0{NAN};
+	float _global_local_alt0{NAN};
 
-	float	_takeoff_ground_alt{0.0f};			///< ground altitude at which plane was launched
-	float	_hdg_hold_yaw{0.0f};				///< hold heading for velocity mode
-	bool	_hdg_hold_enabled{false};			///< heading hold enabled
-	bool	_yaw_lock_engaged{false};			///< yaw is locked for heading hold
+	// [m] ground altitude at which plane was launched
+	float _takeoff_ground_alt{0.0f};
 
-	float	_min_current_sp_distance_xy{FLT_MAX};
+	// [rad] yaw setpoint for manual position mode heading hold
+	float _hdg_hold_yaw{0.0f};
 
-	position_setpoint_s _hdg_hold_prev_wp {};		///< position where heading hold started
-	position_setpoint_s _hdg_hold_curr_wp {};		///< position to which heading hold flies
+	bool _hdg_hold_enabled{false}; // heading hold enabled
+	bool _yaw_lock_engaged{false}; // yaw is locked for heading hold
+
+	float _min_current_sp_distance_xy{FLT_MAX};
+
+	position_setpoint_s _hdg_hold_prev_wp {}; // position where heading hold started
+	position_setpoint_s _hdg_hold_curr_wp {}; // position to which heading hold flies
 
 	/**
 	 * @brief Last absolute time position control has been called [us]
@@ -216,38 +233,60 @@ private:
 
 	Landingslope _landingslope;
 
-	hrt_abstime _time_started_landing{0};			///< time at which landing started
+	// [us] time at which landing started
+	hrt_abstime _time_started_landing{0};
 
-	float _t_alt_prev_valid{0};				///< last terrain estimate which was valid
-	hrt_abstime _time_last_t_alt{0};			///< time at which we had last valid terrain alt
+	// [m] last terrain estimate which was valid
+	float _last_valid_terrain_alt_estimate{0.0f};
 
-	float _flare_height{0.0f};				///< estimated height to ground at which flare started
-	float _flare_pitch_sp{0.0f};			///< Current forced (i.e. not determined using TECS) flare pitch setpoint
+	// [us] time at which we had last valid terrain alt
+	hrt_abstime _last_time_terrain_alt_was_valid{0};
+
+	// [m] estimated height to ground at which flare started
+	float _flare_height{0.0f};
+
+	// [m] current forced (i.e. not determined using TECS) flare pitch setpoint
+	float _flare_pitch_sp{0.0f};
+
+	// [m] estimated height to ground at which flare started
 	float _flare_curve_alt_rel_last{0.0f};
-	float _target_bearing{0.0f};				///< estimated height to ground at which flare started
 
-	bool _was_in_air{false};				///< indicated wether the plane was in the air in the previous interation*/
-	hrt_abstime _time_went_in_air{0};			///< time at which the plane went in the air
+	float _target_bearing{0.0f}; // [rad]
 
-	/* Takeoff launch detection and runway */
+	// indicates whether the plane was in the air in the previous interation
+	bool _was_in_air{false};
+
+	// [us] time at which the plane went in the air
+	hrt_abstime _time_went_in_air{0};
+
+	// Takeoff launch detection and runway
 	LaunchDetector _launchDetector;
 	LaunchDetectionResult _launch_detection_state{LAUNCHDETECTION_RES_NONE};
 	hrt_abstime _launch_detection_notify{0};
 
 	RunwayTakeoff _runway_takeoff;
 
-	bool _last_manual{false};				///< true if the last iteration was in manual mode (used to determine when a reset is needed)
+	// true if the last iteration was in manual mode (used to determine when a reset is needed)
+	bool _last_manual{false};
 
 	/* throttle and airspeed states */
-	bool _airspeed_valid{false};				///< flag if a valid airspeed estimate exists
-	hrt_abstime _airspeed_last_valid{0};			///< last time airspeed was received. Used to detect timeouts.
+
+	bool _airspeed_valid{false};
+
+	// [us] last time airspeed was received. used to detect timeouts.
+	hrt_abstime _time_airspeed_last_valid{0};
+
 	float _airspeed{0.0f};
 	float _eas2tas{1.0f};
 
 	/* wind estimates */
-	Vector2f _wind_vel{0.0f, 0.0f}; ///< wind velocity vector [m/s]
-	bool _wind_valid{false}; ///< flag if a valid wind estimate exists
-	hrt_abstime _time_wind_last_received{0}; ///< last time wind estimate was received in microseconds. Used to detect timeouts.
+
+	// [m/s] wind velocity vector
+	Vector2f _wind_vel{0.0f, 0.0f};
+
+	bool _wind_valid{false};
+
+	hrt_abstime _time_wind_last_received{0}; // [us]
 
 	float _pitch{0.0f};
 	float _yaw{0.0f};
@@ -256,32 +295,40 @@ private:
 	matrix::Vector3f _body_acceleration{};
 	matrix::Vector3f _body_velocity{};
 
-	bool _reinitialize_tecs{true};				///< indicates if the TECS states should be reinitialized (used for VTOL)
+	bool _reinitialize_tecs{true};
 	bool _is_tecs_running{false};
-	hrt_abstime _last_tecs_update{0};
 
-	float _asp_after_transition{0.0f};
+	hrt_abstime _time_last_tecs_update{0}; // [us]
+
+	float _airspeed_after_transition{0.0f};
 	bool _was_in_transition{false};
 
-	bool _vtol_tailsitter{false};
+	bool _is_vtol_tailsitter{false};
 
 	matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
 
 	// estimator reset counters
-	uint8_t _pos_reset_counter{0};				///< captures the number of times the estimator has reset the horizontal position
-	uint8_t _alt_reset_counter{0};				///< captures the number of times the estimator has reset the altitude state
 
-	float _manual_control_setpoint_altitude{0.0f};
-	float _manual_control_setpoint_airspeed{0.0f};
-	float _commanded_airspeed_setpoint{NAN};		///< airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED
+	// captures the number of times the estimator has reset the horizontal position
+	uint8_t _pos_reset_counter{0};
 
-	hrt_abstime _time_in_fixed_bank_loiter{0};
+	// captures the number of times the estimator has reset the altitude state
+	uint8_t _alt_reset_counter{0};
 
-	ECL_L1_Pos_Controller	_l1_control;
+	float _manual_control_setpoint_altitude{0.0f}; // [m]
+	float _manual_control_setpoint_airspeed{0.0f}; // [m/s]
+
+	// [m/s] airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED
+	float _commanded_airspeed_setpoint{NAN};
+
+	hrt_abstime _time_in_fixed_bank_loiter{0}; // [us]
+
+	ECL_L1_Pos_Controller _l1_control;
 	NPFG _npfg;
-	TECS			_tecs;
+	TECS _tecs;
 
 	uint8_t _position_sp_type{0};
+
 	enum FW_POSCTRL_MODE {
 		FW_POSCTRL_MODE_AUTO,
 		FW_POSCTRL_MODE_AUTO_ALTITUDE,
@@ -291,10 +338,11 @@ private:
 		FW_POSCTRL_MODE_MANUAL_POSITION,
 		FW_POSCTRL_MODE_MANUAL_ALTITUDE,
 		FW_POSCTRL_MODE_OTHER
-	} _control_mode_current{FW_POSCTRL_MODE_OTHER};		///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
+	} _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed
 
 	param_t _param_handle_airspeed_trans{PARAM_INVALID};
-	float _param_airspeed_trans{NAN};
+
+	float _param_airspeed_trans{NAN}; // [m/s]
 
 	enum StickConfig {
 		STICK_CONFIG_SWAP_STICKS_BIT = (1 << 0),
@@ -302,27 +350,27 @@ private:
 	};
 
 	// Update our local parameter cache.
-	int		parameters_update();
+	int parameters_update();
 
 	// Update subscriptions
-	void		airspeed_poll();
-	void		control_update();
-	void 		manual_control_setpoint_poll();
-	void		vehicle_attitude_poll();
-	void		vehicle_command_poll();
-	void		vehicle_control_mode_poll();
-	void		vehicle_status_poll();
-	void        wind_poll();
+	void airspeed_poll();
+	void control_update();
+	void manual_control_setpoint_poll();
+	void vehicle_attitude_poll();
+	void vehicle_command_poll();
+	void vehicle_control_mode_poll();
+	void vehicle_status_poll();
+	void wind_poll();
 
-	void		status_publish();
-	void		landing_status_publish();
-	void		tecs_status_publish();
-	void 		publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
+	void status_publish();
+	void landing_status_publish();
+	void tecs_status_publish();
+	void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
 
-	void		abort_landing(bool abort);
+	void abort_landing(bool abort);
 
 	/**
-	 * Get a new waypoint based on heading and distance from current position
+	 * @brief Get a new waypoint based on heading and distance from current position
 	 *
 	 * @param heading the heading to fly to
 	 * @param distance the distance of the generated waypoint
@@ -333,72 +381,168 @@ private:
 			position_setpoint_s &waypoint_next, bool flag_init);
 
 	/**
-	 * Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available
+	 * @brief Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available
 	 */
-	float		get_terrain_altitude_takeoff(float takeoff_alt);
+	float get_terrain_altitude_takeoff(float takeoff_alt);
 
 	float getManualHeightRateSetpoint();
 
 	/**
-	 * Check if we are in a takeoff situation
+	 * @brief Check if we are in a takeoff situation
 	 */
-	bool 		in_takeoff_situation();
+	bool in_takeoff_situation();
 
 	/**
-	 * Update desired altitude base on user pitch stick input
+	 * @brief Update desired altitude base on user pitch stick input
 	 *
 	 * @param dt Time step
 	 */
-	void		update_desired_altitude(float dt);
-	uint8_t		handle_setpoint_type(const uint8_t setpoint_type, const position_setpoint_s &pos_sp_curr);
-	void		control_auto(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
-				     const position_setpoint_s &pos_sp_prev,
-				     const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
-
-	void		control_auto_fixed_bank_alt_hold(const hrt_abstime &now);
-	void		control_auto_descend(const hrt_abstime &now);
-
-	void		control_auto_position(const hrt_abstime &now, const float dt, 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_loiter(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
-					    const Vector2f &ground_speed,
-					    const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
-	void		control_auto_velocity(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
-					      const Vector2f &ground_speed,
-					      const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
+	void update_desired_altitude(float dt);
 
 	/**
-	 * @brief Vehicle control while in takeoff
+	 * @brief Updates timing information for landed and in-air states.
 	 *
 	 * @param now Current system time [us]
+	 */
+	void update_in_air_states(const hrt_abstime now);
+
+	/**
+	 * @brief Moves the current position setpoint to a value far ahead of the current vehicle yaw when in  a VTOL
+	 * transition.
+	 *
+	 * @param[in,out] current_sp current position setpoint
+	 */
+	void move_position_setpoint_for_vtol_transition(position_setpoint_s &current_sp);
+
+	uint8_t	handle_setpoint_type(const uint8_t setpoint_type, const position_setpoint_s &pos_sp_curr);
+
+	/**
+	 * @brief Position control for all automatic modes except takeoff and landing
+	 *
+	 * @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 ground_speed Local 2D ground speed of vehicle [m/s]
+	 * @param pos_sp_prev previous position setpoint
+	 * @param pos_sp_curr current position setpoint
+	 * @param pos_sp_next next position setpoint
+	 */
+	void control_auto(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 position_setpoint_s &pos_sp_next);
+
+	/**
+	 * @brief Controls altitude and airspeed for a fixed-bank loiter.
+	 *
+	 * @param now Current system time [us]
+	 */
+	void control_auto_fixed_bank_alt_hold(const hrt_abstime &now);
+
+	/**
+	 * @brief Control airspeed with a fixed descent rate and roll angle.
+	 *
+	 * @param now Current system time [us]
+	 */
+	void control_auto_descend(const hrt_abstime &now);
+
+	/**
+	 * @brief Vehicle control for position waypoints.
+	 *
+	 * @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 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 Vector2d &curr_pos,
-					     const Vector2f &ground_speed,
-					     const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
-	void		control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos,
-					     const Vector2f &ground_speed,
-					     const position_setpoint_s &pos_sp_prev,
-					     const position_setpoint_s &pos_sp_curr);
-	void		control_manual_altitude(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
-	void		control_manual_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
+	void control_auto_position(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);
 
-	float		get_tecs_pitch();
-	float		get_tecs_thrust();
+	/**
+	 * @brief Vehicle control for loiter waypoints.
+	 *
+	 * @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 ground_speed Local 2D ground speed of vehicle [m/s]
+	 * @param pos_sp_prev previous position setpoint
+	 * @param pos_sp_curr current position setpoint
+	 * @param pos_sp_next next position setpoint
+	 */
+	void control_auto_loiter(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 position_setpoint_s &pos_sp_next);
 
-	float		get_manual_airspeed_setpoint();
-	float		get_auto_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed, const Vector2f &ground_speed,
-			float dt);
+	/**
+	 * @brief Controls a desired airspeed, bearing, and height rate.
+	 *
+	 * @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 ground_speed Local 2D ground speed of vehicle [m/s]
+	 * @param pos_sp_curr current position setpoint
+	 */
+	void control_auto_velocity(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
+				   const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr);
 
-	void		reset_takeoff_state(bool force = false);
-	void		reset_landing_state();
-	bool		using_npfg_with_wind_estimate() const;
-	Vector2f 	get_nav_speed_2d(const Vector2f &ground_speed);
-	void		set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid);
+	/**
+	 * @brief Controls automatic takeoff.
+	 *
+	 * @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 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);
+
+	/**
+	 * @brief Controls automatic landing.
+	 *
+	 * @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 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_landing(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);
+
+	/**
+	 * @brief Controls altitude and airspeed, user commands roll setpoint.
+	 *
+	 * @param now Current system time [us]
+	 * @param curr_pos Current 2D local position vector of vehicle [m]
+	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
+	 */
+	void control_manual_altitude(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
+
+	/**
+	 * @brief Controls user commanded altitude, airspeed, and bearing.
+	 *
+	 * @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 ground_speed Local 2D ground speed of vehicle [m/s]
+	 */
+	void control_manual_position(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
+				     const Vector2f &ground_speed);
+
+	float get_tecs_pitch();
+	float get_tecs_thrust();
+
+	float get_manual_airspeed_setpoint();
+	float get_auto_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed, const Vector2f &ground_speed,
+					 float dt);
+
+	void reset_takeoff_state(bool force = false);
+	void reset_landing_state();
+	bool using_npfg_with_wind_estimate() const;
+	Vector2f get_nav_speed_2d(const Vector2f &ground_speed);
+	void set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid);
 
 	void publishOrbitStatus(const position_setpoint_s pos_sp);