refactored fixed wing position control module to use new API

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -56,8 +56,7 @@
 #include <drivers/drv_hrt.h>
 #include <lib/geo/geo.h>
 #include <lib/atmosphere/atmosphere.h>
-#include <lib/npfg/npfg.hpp>
-#include <lib/tecs/TECS.hpp>
+#include <lib/npfg/DirectionalGuidance.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
 #include <lib/slew_rate/SlewRate.hpp>
@@ -98,6 +97,10 @@
 #include <uORB/topics/wind.h>
 #include <uORB/topics/orbit_status.h>
 #include <uORB/uORB.h>
+#include <uORB/topics/fw_lateral_control_setpoint.h>
+#include <uORB/topics/fw_longitudinal_control_setpoint.h>
+#include <uORB/topics/longitudinal_control_limits.h>
+#include <uORB/topics/lateral_control_limits.h>
 
 #ifdef CONFIG_FIGURE_OF_EIGHT
 #include "figure_eight/FigureEight.hpp"
@@ -115,12 +118,6 @@ using matrix::Vector2f;
 // [m] initial distance of waypoint in front of plane in heading hold mode
 static constexpr float HDG_HOLD_DIST_NEXT = 3000.0f;
 
-// [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;
-
-// [m] distance by which previous waypoint is set behind the plane
-static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f;
-
 // [rad/s] max yawrate at which plane locks yaw for heading hold mode
 static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f;
 
@@ -169,14 +166,7 @@ static constexpr float POST_TOUCHDOWN_CLAMP_TIME = 0.5f;
 // [m/s] maximum reference altitude rate threshhold
 static constexpr float MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT = 0.1f;
 
-// [s] Timeout that has to pass in roll-constraining failsafe before warning is triggered
-static constexpr uint64_t ROLL_WARNING_TIMEOUT = 2_s;
 
-// [-] Can-run threshold needed to trigger the roll-constraining failsafe warning
-static constexpr float ROLL_WARNING_CAN_RUN_THRESHOLD = 0.9f;
-
-// [s] slew rate with which we change altitude time constant
-static constexpr float TECS_ALT_TIME_CONST_SLEW_RATE = 1.0f;
 
 class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
 	public px4::WorkItem
@@ -217,18 +207,19 @@ 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)};
-	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::Publication<launch_detection_status_s> _launch_detection_status_pub{ORB_ID(launch_detection_status)};
 	uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
 	uORB::Publication<landing_gear_s> _landing_gear_pub {ORB_ID(landing_gear)};
 	uORB::Publication<normalized_unsigned_setpoint_s> _flaps_setpoint_pub{ORB_ID(flaps_setpoint)};
 	uORB::Publication<normalized_unsigned_setpoint_s> _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)};
-	uORB::PublicationData<flight_phase_estimation_s> _flight_phase_estimation_pub{ORB_ID(flight_phase_estimation)};
+	uORB::PublicationData<fw_lateral_control_setpoint_s> _lateral_ctrl_sp_pub{ORB_ID(fw_lateral_control_setpoint)};
+	uORB::PublicationData<fw_lateral_control_setpoint_s> _lateral_ctrl_status_pub{ORB_ID(fw_lateral_control_status)};
+	uORB::PublicationData<lateral_control_limits_s> _lateral_ctrl_limits_pub{ORB_ID(lateral_control_limits)};
+	uORB::PublicationData<fw_longitudinal_control_setpoint_s> _longitudinal_ctrl_sp_pub{ORB_ID(fw_longitudinal_control_setpoint)};
+	uORB::PublicationData<longitudinal_control_limits_s> _longitudinal_ctrl_limits_pub{ORB_ID(longitudinal_control_limits)};
 
 	manual_control_setpoint_s _manual_control_setpoint{};
 	position_setpoint_triplet_s _pos_sp_triplet{};
@@ -276,8 +267,6 @@ private:
 	double _current_longitude{0};
 	float _current_altitude{0.f};
 
-	float _roll{0.f};
-	float _pitch{0.0f};
 	float _yaw{0.0f};
 	float _yawrate{0.0f};
 
@@ -397,24 +386,12 @@ private:
 
 	hrt_abstime _time_wind_last_received{0}; // [us]
 
-	// TECS
-
-	// total energy control system - airspeed / altitude control
-	TECS _tecs;
-
-	bool _tecs_is_running{false};
-
-	// Smooths changes in the altitude tracking error time constant value
-	SlewRate<float> _tecs_alt_time_const_slew_rate;
-
 	// VTOL / TRANSITION
-	bool _is_vtol_tailsitter{false};
 	matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
 	float _backtrans_heading{NAN};	// used to lock the initial heading for backtransition with no position control
 
 	// ESTIMATOR RESET COUNTERS
 	uint8_t _xy_reset_counter{0};
-	uint8_t _z_reset_counter{0};
 	uint64_t _time_last_xy_reset{0};
 
 	// LATERAL-DIRECTIONAL GUIDANCE
@@ -423,10 +400,7 @@ private:
 	matrix::Vector2f _closest_point_on_path;
 
 	// nonlinear path following guidance - lateral-directional position control
-	NPFG _npfg;
-	bool _need_report_npfg_uncertain_condition{false}; ///< boolean if reporting of uncertain npfg output condition is needed
-	hrt_abstime _time_since_first_reduced_roll{0U}; ///< absolute time since start when entering reduced roll angle for the first time
-	hrt_abstime _time_since_last_npfg_call{0U}; 	///< absolute time since start when the npfg reduced roll angle calculations was last performed
+	DirectionalGuidance _directional_guidance;
 
 	PerformanceModel _performance_model;
 
@@ -439,7 +413,6 @@ private:
 
 	hrt_abstime _time_in_fixed_bank_loiter{0}; // [us]
 	float _min_current_sp_distance_xy{FLT_MAX};
-	float _target_bearing{0.0f}; // [rad]
 
 #ifdef CONFIG_FIGURE_OF_EIGHT
 	/* Loitering */
@@ -465,27 +438,18 @@ private:
 
 	// 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 status_publish();
 	void landing_status_publish();
-	void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw,
-				 float throttle_trim);
-	void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
-	float getLoadFactor();
 
-	/**
-	 * @brief Get the NPFG roll setpoint with mitigation strategy if npfg is not certain about its output
-	 *
-	 * @return roll setpoint
-	 */
-	float getCorrectedNpfgRollSetpoint();
+	void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
+	float getLoadFactor() const;
 
 	/**
 	 * @brief Sets the landing abort status and publishes landing status.
@@ -503,24 +467,6 @@ private:
 	 */
 	bool checkLandingAbortBitMask(const uint8_t automatic_abort_criteria_bitmask, uint8_t landing_abort_criterion);
 
-	/**
-	 * @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
-	 * @param waypoint_prev the waypoint at the current position
-	 * @param waypoint_next the waypoint in the heading direction
-	 */
-	void get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev,
-					   position_setpoint_s &waypoint_next, bool flag_init);
-
-	/**
-	 * @brief Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available
-	 *
-	 * @param takeoff_alt Altitude AMSL at launch or when runway takeoff is detected [m]
-	 */
-	float get_terrain_altitude_takeoff(float takeoff_alt);
-
 	/**
 	 * @brief Maps the manual control setpoint (pilot sticks) to height rate commands
 	 *
@@ -535,13 +481,6 @@ private:
 	 */
 	void updateManualTakeoffStatus();
 
-	/**
-	 * @brief Update desired altitude base on user pitch stick input
-	 *
-	 * @param dt Time step
-	 */
-	void update_desired_altitude(float dt);
-
 	/**
 	 * @brief Updates timing information for landed and in-air states.
 	 *
@@ -724,9 +663,6 @@ private:
 	void control_backtransition_line_follow(const Vector2f &ground_speed,
 						const position_setpoint_s &pos_sp_curr);
 
-	float get_tecs_pitch();
-	float get_tecs_thrust();
-
 	float get_manual_airspeed_setpoint();
 
 	/**
@@ -759,38 +695,8 @@ private:
 	void publishOrbitStatus(const position_setpoint_s pos_sp);
 
 	SlewRate<float> _airspeed_slew_rate_controller;
-	SlewRate<float> _roll_slew_rate;
 
-	/**
-	 * @brief A wrapper function to call the TECS implementation
-	 *
-	 * @param control_interval Time since the last position control update [s]
-	 * @param alt_sp Altitude setpoint, AMSL [m]
-	 * @param airspeed_sp Calibrated airspeed setpoint [m/s]
-	 * @param pitch_min_rad Nominal pitch angle command minimum [rad]
-	 * @param pitch_max_rad Nominal pitch angle command maximum [rad]
-	 * @param throttle_min Minimum throttle command [0,1]
-	 * @param throttle_max Maximum throttle command [0,1]
-	 * @param desired_max_sink_rate The desired max sink rate commandable when altitude errors are large [m/s]
-	 * @param desired_max_climb_rate The desired max climb rate commandable when altitude errors are large [m/s]
-	 * @param is_low_height Define whether we are in low-height flight for tighter altitude tracking
-	 * @param disable_underspeed_detection True if underspeed detection should be disabled
-	 * @param hgt_rate_sp Height rate setpoint [m/s]
-	 */
-	void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp, float pitch_min_rad,
-					float pitch_max_rad, float throttle_min, float throttle_max,
-					const float desired_max_sink_rate, const float desired_max_climb_rate, const bool is_low_height,
-					bool disable_underspeed_detection = false, float hgt_rate_sp = NAN);
-
-	/**
-	 * @brief Constrains the roll angle setpoint near ground to avoid wingtip strike.
-	 *
-	 * @param roll_setpoint Unconstrained roll angle setpoint [rad]
-	 * @param altitude Vehicle altitude (AMSL) [m]
-	 * @param terrain_altitude Terrain altitude (AMSL) [m]
-	 * @return Constrained roll angle setpoint [rad]
-	 */
-	float constrainRollNearGround(const float roll_setpoint, const float altitude, const float terrain_altitude) const;
+	float getMaxRollAngleNearGround(const float altitude, const float terrain_altitude) const;
 
 	/**
 	 * @brief Calculates the touchdown position for landing with optional manual lateral adjustments.
@@ -838,21 +744,6 @@ private:
 	void initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev,
 				   const float land_point_alt, const Vector2f &local_position, const Vector2f &local_land_point);
 
-	/*
-	 * Checks if the vehicle satisfies conditions for low-height flight
-	 *
-	 * @return bool True if conditions are satisfied, false otherwise
-	 */
-	bool checkLowHeightConditions();
-
-	/*
-	 * Updates TECS altitude time constant according to the is_low_height parameter.
-	 *
-	 * @param is_low_height Boolean flag defining whether we are in low-height flight
-	 * @param dt Update time step [s]
-	 */
-	void updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt);
-
 	/*
 	 * Waypoint handling logic following closely to the ECL_L1_Pos_Controller
 	 * method of the same name. Takes two waypoints, steering the vehicle to track
@@ -864,9 +755,10 @@ private:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateWaypoints(const matrix::Vector2f &start_waypoint, const matrix::Vector2f &end_waypoint,
-			       const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel,
-			       const matrix::Vector2f &wind_vel);
+	DirectionalGuidanceOutput navigateWaypoints(const matrix::Vector2f &start_waypoint,
+			const matrix::Vector2f &end_waypoint,
+			const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel,
+			const matrix::Vector2f &wind_vel);
 
 	/*
 	 * Takes one waypoint and steers the vehicle towards this.
@@ -879,8 +771,8 @@ private:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateWaypoint(const matrix::Vector2f &waypoint_pos, const matrix::Vector2f &vehicle_pos,
-			      const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel);
+	DirectionalGuidanceOutput navigateWaypoint(const matrix::Vector2f &waypoint_pos, const matrix::Vector2f &vehicle_pos,
+			const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel);
 
 	/*
 	 * Line (infinite) following logic. Two points on the line are used to define the
@@ -893,8 +785,9 @@ private:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2, const Vector2f &vehicle_pos,
-			  const Vector2f &ground_vel, const Vector2f &wind_vel);
+	DirectionalGuidanceOutput navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2,
+					       const Vector2f &vehicle_pos,
+					       const Vector2f &ground_vel, const Vector2f &wind_vel);
 
 	/*
 	 * Line (infinite) following logic. One point on the line and a line bearing are used to define
@@ -907,8 +800,9 @@ private:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateLine(const Vector2f &point_on_line, const float line_bearing, const Vector2f &vehicle_pos,
-			  const Vector2f &ground_vel, const Vector2f &wind_vel);
+	DirectionalGuidanceOutput navigateLine(const Vector2f &point_on_line, const float line_bearing,
+					       const Vector2f &vehicle_pos,
+					       const Vector2f &ground_vel, const Vector2f &wind_vel);
 
 	/*
 	 * Loitering (unlimited) logic. Takes loiter center, radius, and direction and
@@ -922,9 +816,9 @@ private:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateLoiter(const matrix::Vector2f &loiter_center, const matrix::Vector2f &vehicle_pos,
-			    float radius, bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_vel,
-			    const matrix::Vector2f &wind_vel);
+	DirectionalGuidanceOutput navigateLoiter(const matrix::Vector2f &loiter_center, const matrix::Vector2f &vehicle_pos,
+			float radius, bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_vel,
+			const matrix::Vector2f &wind_vel);
 
 	/*
 	 * Path following logic. Takes poisiton, path tangent, curvature and
@@ -939,9 +833,10 @@ private:
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 * @param[in] curvature of the path setpoint [1/m]
 	 */
-	void navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &position_setpoint,
-				 const matrix::Vector2f &tangent_setpoint,
-				 const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature);
+	DirectionalGuidanceOutput navigatePathTangent(const matrix::Vector2f &vehicle_pos,
+			const matrix::Vector2f &position_setpoint,
+			const matrix::Vector2f &tangent_setpoint,
+			const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature);
 
 	/*
 	 * Navigate on a fixed bearing.
@@ -954,10 +849,12 @@ private:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing, const matrix::Vector2f &ground_vel,
-			     const matrix::Vector2f &wind_vel);
+	DirectionalGuidanceOutput navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing,
+			const matrix::Vector2f &ground_vel,
+			const matrix::Vector2f &wind_vel);
 
 	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
 		(ParamFloat<px4::params::FW_GND_SPD_MIN>) _param_fw_gnd_spd_min,
 
 		(ParamFloat<px4::params::FW_PN_R_SLEW_MAX>) _param_fw_pn_r_slew_max,
@@ -980,7 +877,6 @@ private:
 		(ParamFloat<px4::params::FW_LND_FL_PMAX>) _param_fw_lnd_fl_pmax,
 		(ParamFloat<px4::params::FW_LND_FL_PMIN>) _param_fw_lnd_fl_pmin,
 		(ParamFloat<px4::params::FW_LND_FLALT>) _param_fw_lnd_flalt,
-		(ParamFloat<px4::params::FW_LND_THRTC_SC>) _param_fw_thrtc_sc,
 		(ParamFloat<px4::params::FW_T_THR_LOW_HGT>) _param_fw_t_thr_low_hgt,
 		(ParamBool<px4::params::FW_LND_EARLYCFG>) _param_fw_lnd_earlycfg,
 		(ParamInt<px4::params::FW_LND_USETER>) _param_fw_lnd_useter,
@@ -988,30 +884,14 @@ private:
 		(ParamFloat<px4::params::FW_P_LIM_MAX>) _param_fw_p_lim_max,
 		(ParamFloat<px4::params::FW_P_LIM_MIN>) _param_fw_p_lim_min,
 
-		(ParamFloat<px4::params::FW_T_HRATE_FF>) _param_fw_t_hrate_ff,
-		(ParamFloat<px4::params::FW_T_ALT_TC>) _param_fw_t_h_error_tc,
-		(ParamFloat<px4::params::FW_T_F_ALT_ERR>) _param_fw_t_fast_alt_err,
-		(ParamFloat<px4::params::FW_T_THR_INTEG>) _param_fw_t_thr_integ,
-		(ParamFloat<px4::params::FW_T_I_GAIN_PIT>) _param_fw_t_I_gain_pit,
-		(ParamFloat<px4::params::FW_T_PTCH_DAMP>) _param_fw_t_ptch_damp,
-		(ParamFloat<px4::params::FW_T_RLL2THR>) _param_fw_t_rll2thr,
-		(ParamFloat<px4::params::FW_T_SINK_MAX>) _param_fw_t_sink_max,
-		(ParamFloat<px4::params::FW_T_SPDWEIGHT>) _param_fw_t_spdweight,
-		(ParamFloat<px4::params::FW_T_TAS_TC>) _param_fw_t_tas_error_tc,
-		(ParamFloat<px4::params::FW_T_THR_DAMPING>) _param_fw_t_thr_damping,
-		(ParamFloat<px4::params::FW_T_VERT_ACC>) _param_fw_t_vert_acc,
-		(ParamFloat<px4::params::FW_T_STE_R_TC>) _param_ste_rate_time_const,
-		(ParamFloat<px4::params::FW_T_SEB_R_FF>) _param_seb_rate_ff,
 		(ParamFloat<px4::params::FW_T_CLMB_R_SP>) _param_climbrate_target,
 		(ParamFloat<px4::params::FW_T_SINK_R_SP>) _param_sinkrate_target,
-		(ParamFloat<px4::params::FW_T_SPD_STD>) _param_speed_standard_dev,
-		(ParamFloat<px4::params::FW_T_SPD_DEV_STD>) _param_speed_rate_standard_dev,
-		(ParamFloat<px4::params::FW_T_SPD_PRC_STD>) _param_process_noise_standard_dev,
+		(ParamFloat<px4::params::FW_T_SINK_MAX>) _param_fw_t_sink_max,
+		(ParamFloat<px4::params::FW_T_SPDWEIGHT>) _param_fw_t_spdweight,
 
 		(ParamFloat<px4::params::FW_THR_IDLE>) _param_fw_thr_idle,
 		(ParamFloat<px4::params::FW_THR_MAX>) _param_fw_thr_max,
 		(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
-		(ParamFloat<px4::params::FW_THR_SLEW_MAX>) _param_fw_thr_slew_max,
 
 		(ParamFloat<px4::params::FW_FLAPS_LND_SCL>) _param_fw_flaps_lnd_scl,
 		(ParamFloat<px4::params::FW_FLAPS_TO_SCL>) _param_fw_flaps_to_scl,
@@ -1025,8 +905,6 @@ private:
 		// external parameters
 		(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
 
-		(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
-
 		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,
 
 		(ParamFloat<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min,
@@ -1054,6 +932,11 @@ private:
 		(ParamBool<px4::params::FW_LAUN_DETCN_ON>) _param_fw_laun_detcn_on
 	)
 
+	void control_idle();
+
+	void setDefaultLongControlLimits(longitudinal_control_limits_s &longitudinal_control_limits) const;
+
+	float rollAngleToLateralAccel(float roll_body) const;
 };
 
 #endif // FIXEDWINGPOSITIONCONTROL_HPP_

src/modules/fw_pos_control/figure_eight/FigureEight.cpp

@@ -48,11 +48,10 @@ static constexpr bool SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE{true};
 static constexpr float DEFAULT_MAJOR_TO_MINOR_AXIS_RATIO{2.5f};
 static constexpr float MINIMAL_FEASIBLE_MAJOR_TO_MINOR_AXIS_RATIO{2.0f};
 
-FigureEight::FigureEight(NPFG &npfg, matrix::Vector2f &wind_vel, float &eas2tas) :
+FigureEight::FigureEight(DirectionalGuidance &npfg, matrix::Vector2f &wind_vel, float &eas2tas) :
 	ModuleParams(nullptr),
-	_npfg(npfg),
-	_wind_vel(wind_vel),
-	_eas2tas(eas2tas)
+	_directional_guidance(npfg),
+	_wind_vel(wind_vel)
 {
 
 }
@@ -64,8 +63,9 @@ void FigureEight::resetPattern()
 	_pos_passed_circle_center_along_major_axis = false;
 }
 
-void FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-				 const FigureEightPatternParameters &parameters, float target_airspeed)
+DirectionalGuidanceOutput FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local,
+		const matrix::Vector2f &ground_speed,
+		const FigureEightPatternParameters &parameters, float target_airspeed)
 {
 	// Sanitize inputs
 	FigureEightPatternParameters valid_parameters{sanitizeParameters(parameters)};
@@ -81,7 +81,7 @@ void FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local, const m
 	updateSegment(curr_pos_local, valid_parameters,  pattern_points);
 
 	// Apply control logic based on segment
-	applyControl(curr_pos_local, ground_speed, valid_parameters, target_airspeed, pattern_points);
+	return applyControl(curr_pos_local, ground_speed, valid_parameters, target_airspeed, pattern_points);
 }
 
 FigureEight::FigureEightPatternParameters FigureEight::sanitizeParameters(const FigureEightPatternParameters
@@ -118,7 +118,8 @@ void FigureEight::initializePattern(const matrix::Vector2f &curr_pos_local, cons
 		const bool north_is_closer = north_center_to_pos_local.norm() < south_center_to_pos_local.norm();
 
 		// Get the normalized switch distance.
-		float switch_distance_normalized = _npfg.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;
+		float switch_distance_normalized = _directional_guidance.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS /
+						   parameters.loiter_radius;
 
 		//Far away from current figure of eight. Fly towards closer circle
 
@@ -194,7 +195,8 @@ void FigureEight::updateSegment(const matrix::Vector2f &curr_pos_local, const Fi
 	calculatePositionToCenterNormalizedRotated(center_to_pos_local, curr_pos_local, parameters);
 
 	// Get the normalized switch distance.
-	float switch_distance_normalized = _npfg.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;
+	float switch_distance_normalized = _directional_guidance.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS /
+					   parameters.loiter_radius;
 
 	// Update segment if segment exit condition has been reached
 	switch (_current_segment) {
@@ -275,56 +277,60 @@ void FigureEight::updateSegment(const matrix::Vector2f &curr_pos_local, const Fi
 	}
 }
 
-void FigureEight::applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-			       const FigureEightPatternParameters &parameters, float target_airspeed,
-			       const FigureEightPatternPoints &pattern_points)
+DirectionalGuidanceOutput FigureEight::applyControl(const matrix::Vector2f &curr_pos_local,
+		const matrix::Vector2f &ground_speed,
+		const FigureEightPatternParameters &parameters, float target_airspeed,
+		const FigureEightPatternPoints &pattern_points)
 {
 	Vector2f center_to_pos_local;
 	calculatePositionToCenterNormalizedRotated(center_to_pos_local, curr_pos_local, parameters);
 
 	switch (_current_segment) {
 	case FigureEightSegment::SEGMENT_CIRCLE_NORTH: {
-			applyCircle(NORTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_north_circle_offset, curr_pos_local,
-				    ground_speed, parameters, target_airspeed);
+			return applyCircle(NORTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_north_circle_offset, curr_pos_local,
+					   ground_speed, parameters, target_airspeed);
 		}
 		break;
 
 	case FigureEightSegment::SEGMENT_NORTHEAST_SOUTHWEST: {
 			// Follow path from north-east to south-west
-			applyLine(pattern_points.normalized_north_exit_offset, pattern_points.normalized_south_entry_offset, curr_pos_local,
-				  ground_speed, parameters, target_airspeed);
+			return applyLine(pattern_points.normalized_north_exit_offset, pattern_points.normalized_south_entry_offset,
+					 curr_pos_local,
+					 ground_speed, parameters, target_airspeed);
 		}
 		break;
 
 	case FigureEightSegment::SEGMENT_CIRCLE_SOUTH: {
-			applyCircle(SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_south_circle_offset, curr_pos_local,
-				    ground_speed, parameters, target_airspeed);
+			return applyCircle(SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_south_circle_offset, curr_pos_local,
+					   ground_speed, parameters, target_airspeed);
 		}
 		break;
 
 	case FigureEightSegment::SEGMENT_SOUTHEAST_NORTHWEST: {
 			// follow path from south-east to north-west
-			applyLine(pattern_points.normalized_south_exit_offset, pattern_points.normalized_north_entry_offset, curr_pos_local,
-				  ground_speed, parameters, target_airspeed);
+			return applyLine(pattern_points.normalized_south_exit_offset, pattern_points.normalized_north_entry_offset,
+					 curr_pos_local,
+					 ground_speed, parameters, target_airspeed);
 		}
 		break;
 
 	case FigureEightSegment::SEGMENT_POINT_SOUTHWEST: {
 			// Follow path from current position to south-west
-			applyLine(center_to_pos_local, pattern_points.normalized_south_entry_offset, curr_pos_local,
-				  ground_speed, parameters, target_airspeed);
+			return applyLine(center_to_pos_local, pattern_points.normalized_south_entry_offset, curr_pos_local,
+					 ground_speed, parameters, target_airspeed);
 		}
 		break;
 
 	case FigureEightSegment::SEGMENT_POINT_NORTHWEST: {
 			// Follow path from current position to north-west
-			applyLine(center_to_pos_local, pattern_points.normalized_north_entry_offset, curr_pos_local,
-				  ground_speed, parameters, target_airspeed);
+			return applyLine(center_to_pos_local, pattern_points.normalized_north_entry_offset, curr_pos_local,
+					 ground_speed, parameters, target_airspeed);
 		}
 		break;
 
 	case FigureEightSegment::SEGMENT_UNDEFINED:
 	default:
+		return DirectionalGuidanceOutput{};
 		break;
 	}
 }
@@ -356,9 +362,10 @@ float FigureEight::calculateRotationAngle(const FigureEightPatternParameters &pa
 	return yaw_rotation;
 }
 
-void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const matrix::Vector2f &normalized_circle_offset,
-			      const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-			      const FigureEightPatternParameters &parameters, float target_airspeed)
+DirectionalGuidanceOutput FigureEight::applyCircle(bool loiter_direction_counter_clockwise,
+		const matrix::Vector2f &normalized_circle_offset,
+		const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+		const FigureEightPatternParameters &parameters, float target_airspeed)
 {
 	const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;
 
@@ -366,8 +373,6 @@ void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const mat
 	Vector2f circle_offset_rotated = Dcm2f(calculateRotationAngle(parameters)) * circle_offset;
 	Vector2f circle_center = parameters.center_pos_local + circle_offset_rotated;
 
-	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
 
 	const Vector2f vector_center_to_vehicle = curr_pos_local - circle_center;
 	const float dist_to_center = vector_center_to_vehicle.norm();
@@ -392,16 +397,14 @@ void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const mat
 	float path_curvature = loiter_direction_multiplier / parameters.loiter_minor_radius;
 	_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
 	_closest_point_on_path = unit_vec_center_to_closest_pt * parameters.loiter_minor_radius + circle_center;
-	_npfg.guideToPath(curr_pos_local, ground_speed, _wind_vel, unit_path_tangent,
-			  _closest_point_on_path, path_curvature);
+	return _directional_guidance.guideToPath(curr_pos_local, ground_speed, _wind_vel, unit_path_tangent,
+			_closest_point_on_path, path_curvature);
 
-	_roll_setpoint = _npfg.getRollSetpoint();
-	_indicated_airspeed_setpoint = _npfg.getAirspeedRef() / _eas2tas;
 }
 
-void FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset,
-			    const matrix::Vector2f &normalized_line_end_offset, const matrix::Vector2f &curr_pos_local,
-			    const matrix::Vector2f &ground_speed, const FigureEightPatternParameters &parameters, float target_airspeed)
+DirectionalGuidanceOutput FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset,
+		const matrix::Vector2f &normalized_line_end_offset, const matrix::Vector2f &curr_pos_local,
+		const matrix::Vector2f &ground_speed, const FigureEightPatternParameters &parameters, float target_airspeed)
 {
 	const Dcm2f rotation_matrix(calculateRotationAngle(parameters));
 
@@ -414,15 +417,12 @@ void FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset
 	const Vector2f end_offset_rotated = rotation_matrix * end_offset;
 	const Vector2f line_segment_end_position = parameters.center_pos_local + end_offset_rotated;
 
-	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
 	const Vector2f path_tangent = line_segment_end_position - line_segment_start_position;
 	const Vector2f unit_path_tangent = path_tangent.normalized();
 	_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
 	const Vector2f vector_A_to_vehicle = curr_pos_local - line_segment_start_position;
 	_closest_point_on_path = line_segment_start_position + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent;
-	_npfg.guideToPath(curr_pos_local, ground_speed, _wind_vel, path_tangent.normalized(), line_segment_start_position,
-			  0.0f);
-	_roll_setpoint = _npfg.getRollSetpoint();
-	_indicated_airspeed_setpoint = _npfg.getAirspeedRef() / _eas2tas;
+	return _directional_guidance.guideToPath(curr_pos_local, ground_speed, _wind_vel, path_tangent.normalized(),
+			line_segment_start_position,
+			0.0f);
 }

src/modules/fw_pos_control/figure_eight/FigureEight.hpp

@@ -45,7 +45,7 @@
 #include <px4_platform_common/param.h>
 #include <lib/matrix/matrix/math.hpp>
 
-#include "lib/npfg/npfg.hpp"
+#include "lib/npfg/DirectionalGuidance.hpp"
 
 class FigureEight : public ModuleParams
 {
@@ -89,7 +89,7 @@ public:
 	 * @param[in] wind_vel is the reference to the parent wind velocity [m/s].
 	 * @param[in] eas2tas is the reference to the parent indicated airspeed to true airspeed conversion.
 	 */
-	FigureEight(NPFG &npfg, matrix::Vector2f &wind_vel, float &eas2tas);
+	FigureEight(DirectionalGuidance &directional_guidance, matrix::Vector2f &wind_vel, float &eas2tas);
 
 	/**
 	 * @brief reset the figure eight pattern.
@@ -107,20 +107,8 @@ public:
 	 * @param[in] parameters is the parameter set defining the figure eight shape.
 	 * @param[in] target_airspeed is the current targeted indicated airspeed [m/s].
 	 */
-	void updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-			    const FigureEightPatternParameters &parameters, float target_airspeed);
-	/**
-	 * @brief Get the roll setpoint
-	 *
-	 * @return the roll setpoint in [rad].
-	 */
-	float getRollSetpoint() const {return _roll_setpoint;};
-	/**
-	 * @brief Get the indicated airspeed setpoint
-	 *
-	 * @return the indicated airspeed setpoint in [m/s].
-	 */
-	float getAirspeedSetpoint() const {return _indicated_airspeed_setpoint;};
+	DirectionalGuidanceOutput updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+			const FigureEightPatternParameters &parameters, float target_airspeed);
 	/**
 	 * @brief Get the target bearing of current point on figure of eight
 	 *
@@ -175,9 +163,9 @@ private:
 	 * @param[in] target_airspeed is the current targeted indicated airspeed [m/s].
 	 * @param[in] pattern_points are the relevant points defining the figure eight pattern.
 	 */
-	void applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-			  const FigureEightPatternParameters &parameters, float target_airspeed,
-			  const FigureEightPatternPoints &pattern_points);
+	DirectionalGuidanceOutput applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+					       const FigureEightPatternParameters &parameters, float target_airspeed,
+					       const FigureEightPatternPoints &pattern_points);
 	/**
 	 * @brief Update active segment.
 	 *
@@ -214,9 +202,10 @@ private:
 	 * @param[in] parameters is the parameter set defining the figure eight shape.
 	 * @param[in] target_airspeed is the current targeted indicated airspeed [m/s].
 	 */
-	void applyCircle(bool loiter_direction_counter_clockwise, const matrix::Vector2f &normalized_circle_offset,
-			 const matrix::Vector2f &curr_pos_local,
-			 const matrix::Vector2f &ground_speed, const FigureEightPatternParameters &parameters, float target_airspeed);
+	DirectionalGuidanceOutput applyCircle(bool loiter_direction_counter_clockwise,
+					      const matrix::Vector2f &normalized_circle_offset,
+					      const matrix::Vector2f &curr_pos_local,
+					      const matrix::Vector2f &ground_speed, const FigureEightPatternParameters &parameters, float target_airspeed);
 	/**
 	 * @brief Apply path lateral control
 	 *
@@ -227,16 +216,17 @@ private:
 	 * @param[in] parameters is the parameter set defining the figure eight shape.
 	 * @param[in] target_airspeed is the current targeted indicated airspeed [m/s].
 	 */
-	void applyLine(const matrix::Vector2f &normalized_line_start_offset, const matrix::Vector2f &normalized_line_end_offset,
-		       const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-		       const FigureEightPatternParameters &parameters, float target_airspeed);
+	DirectionalGuidanceOutput applyLine(const matrix::Vector2f &normalized_line_start_offset,
+					    const matrix::Vector2f &normalized_line_end_offset,
+					    const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+					    const FigureEightPatternParameters &parameters, float target_airspeed);
 
 private:
 	/**
 	 * @brief npfg lateral control object.
 	 *
 	 */
-	NPFG &_npfg;
+	DirectionalGuidance &_directional_guidance;
 
 	/**
 	 * @brief Wind velocity in [m/s].
@@ -244,24 +234,9 @@ private:
 	 */
 	const matrix::Vector2f &_wind_vel;
 	/**
-	 * @brief Conversion factor from indicated to true airspeed.
-	 *
-	 */
-	const float &_eas2tas;
-	/**
-	 * @brief Roll setpoint in [rad].
-	 *
-	 */
-	float _roll_setpoint;
-	/**
-	 * @brief Indicated airspeed setpoint in [m/s].
-	 *
-	 */
-	float _indicated_airspeed_setpoint;
-	/**
-	 * @brief active figure eight position setpoint.
-	 *
-	 */
+	* @brief active figure eight position setpoint.
+	*
+	*/
 	FigureEightPatternParameters _active_parameters;
 
 	/**

src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.cpp

@@ -105,23 +105,23 @@ bool RunwayTakeoff::controlYaw()
 	return takeoff_state_ < RunwayTakeoffState::CLIMBOUT;
 }
 
-float RunwayTakeoff::getPitch(float external_pitch_setpoint)
+float RunwayTakeoff::getPitch()
 {
 	if (takeoff_state_ <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) {
 		return math::radians(param_rwto_psp_.get());
 	}
 
-	return external_pitch_setpoint;
+	return NAN;
 }
 
-float RunwayTakeoff::getRoll(float external_roll_setpoint)
+float RunwayTakeoff::getRoll()
 {
 	// until we have enough ground clearance, set roll to 0
 	if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) {
 		return 0.0f;
 	}
 
-	return external_roll_setpoint;
+	return NAN;
 }
 
 float RunwayTakeoff::getYaw(float external_yaw_setpoint)
@@ -134,7 +134,7 @@ float RunwayTakeoff::getYaw(float external_yaw_setpoint)
 	}
 }
 
-float RunwayTakeoff::getThrottle(const float idle_throttle, const float external_throttle_setpoint) const
+float RunwayTakeoff::getThrottle(const float idle_throttle) const
 {
 	float throttle = idle_throttle;
 
@@ -147,19 +147,13 @@ float RunwayTakeoff::getThrottle(const float idle_throttle, const float external
 		break;
 
 	case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
+	case RunwayTakeoffState::CLIMBOUT:
 		throttle = param_rwto_max_thr_.get();
 
 		break;
 
-	case RunwayTakeoffState::CLIMBOUT:
-		// ramp in throttle setpoint over takeoff rotation transition time
-		throttle = interpolateValuesOverAbsoluteTime(param_rwto_max_thr_.get(), external_throttle_setpoint, takeoff_time_,
-				param_rwto_rot_time_.get());
-
-		break;
-
 	case RunwayTakeoffState::FLY:
-		throttle = external_throttle_setpoint;
+		throttle = NAN;
 	}
 
 	return throttle;

src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.h

@@ -116,13 +116,13 @@ public:
 	 * @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);
+	float getPitch();
 
 	/**
 	 * @param external_roll_setpoint Externally commanded roll angle setpoint (usually from path navigation) [rad]
 	 * @return Roll angle setpoint [rad]
 	 */
-	float getRoll(float external_roll_setpoint);
+	float getRoll();
 
 	/**
 	 * @brief Returns the appropriate yaw angle setpoint.
@@ -145,7 +145,7 @@ public:
 	 * @param external_throttle_setpoint Externally commanded throttle setpoint (usually from TECS), normalized [0,1]
 	 * @return Throttle setpoint, normalized [0,1]
 	 */
-	float getThrottle(const float idle_throttle, const float external_throttle_setpoint) const;
+	float getThrottle(const float idle_throttle) const;
 
 	/**
 	 * @param min_pitch_in_climbout Minimum pitch angle during climbout [rad]