WIP: Fixed wing control API

ROMFS/px4fmu_common/init.d/rc.vtol_apps

@@ -28,6 +28,7 @@ fi
 fw_rate_control start vtol
 fw_att_control start vtol
 fw_pos_control start vtol
+fw_lat_lon_control start vtol
 fw_autotune_attitude_control start vtol
 
 # Start Land Detector

boards/px4/sitl/default.px4board

@@ -21,6 +21,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_FIGURE_OF_EIGHT=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y

msg/CMakeLists.txt

@@ -92,6 +92,8 @@ set(msg_files
 	FollowTargetEstimator.msg
 	FollowTargetStatus.msg
 	FuelTankStatus.msg
+	FwLateralControlSetpoint.msg
+	FwLongitudinalControlSetpoint.msg
 	GeneratorStatus.msg
 	GeofenceResult.msg
 	GeofenceStatus.msg
@@ -121,10 +123,12 @@ set(msg_files
 	LandingGearWheel.msg
 	LandingTargetInnovations.msg
 	LandingTargetPose.msg
+		LateralControlLimits.msg
 	LaunchDetectionStatus.msg
 	LedControl.msg
 	LoggerStatus.msg
 	LogMessage.msg
+	LongitudinalControlLimits.msg
 	MagnetometerBiasEstimate.msg
 	MagWorkerData.msg
 	ManualControlSwitches.msg

msg/FwLateralControlSetpoint.msg

@@ -0,0 +1,12 @@
+uint64 timestamp
+
+# NOTE: At least one of course_setpoint, airspeed_reference_direction, or lateral_acceleration_setpoint must be finite
+float32 course_setpoint # NAN if not controlled directly, [-pi, pi]
+float32 airspeed_reference_direction    # angle of desired airspeed vector, NAN if not controlled directly, used as feedforward if course setpoint is finite, [-pi, pi],
+float32 lateral_acceleration_setpoint # NAN if not controlled directly, used as feedforward if either course setpoint or airspeed_reference_direction is finite, [m/s^2]
+float32 roll_sp # TODO: remove, only for testing
+
+float32 heading_sp_runway_takeoff # [-pi, pi] heading setpoint for runway takeoff
+bool reset_integral # TODO: remove, resets rate controller integrals
+
+# TOPICS fw_lateral_control_setpoint fw_lateral_control_status

msg/FwLongitudinalControlSetpoint.msg

@@ -0,0 +1,11 @@
+uint64 timestamp
+
+
+# Note: If not both pitch_sp and throttle_sp are finite, then either altitude_setpoint or height_rate_setpoint must be finite
+float32 height_rate_setpoint # NAN if not controlled directly, used as feeforward if altitude_setpoint is finite [m/s]
+float32 altitude_setpoint  # NAN if not controlled, MSL [m]
+float32 equivalent_airspeed_setpoint # [m/s]
+float32 pitch_sp # NAN if not controlled, overrides total energy controller [rad]
+float32 thrust_sp # NAN if not controlled, overrides total energy controller [0,1]
+
+# TOPICS fw_longitudinal_control_setpoint fw_longitudinal_control_status

msg/LateralControlLimits.msg

@@ -0,0 +1,3 @@
+uint64 timestamp
+
+float32 lateral_accel_max # maps 1:1 to a maximum roll angle, accel_max = tan(roll_max) * GRAVITY, m/s^2

msg/LongitudinalControlLimits.msg

@@ -0,0 +1,15 @@
+uint64 timestamp
+
+float32 pitch_min   # [rad]
+float32 pitch_max   # [rad]
+float32 throttle_min # [0,1]
+float32 throttle_max # [0,1]
+float32 climb_rate_target # target climbrate used to change altitude, [m/s]
+float32 sink_rate_target # target sinkrate used to change altitude, [m/s]
+float32 equivalent_airspeed_min # [m/s]
+float32 equivalent_airspeed_max # [m/s]
+
+float32 speed_weight # [0,2], 0=pitch controls altitude only, 2=pitch controls airspeed only
+
+bool enforce_low_height_condition # if true, total energy controller will use lower altitude control time constant
+bool disable_underspeed_protection

src/lib/npfg/AirspeedReferenceController.cpp

@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "AirspeedReferenceController.hpp"
+#include <matrix/math.hpp>
+#include <lib/mathlib/mathlib.h>
+
+using matrix::Vector2f;
+AirspeedReferenceController::AirspeedReferenceController()
+{
+	// Constructor
+}
+
+float AirspeedReferenceController::controlHeading(const float heading_sp, const float heading,
+		const float airspeed) const
+{
+
+	const Vector2f airspeed_vector = Vector2f{cosf(heading), sinf(heading)} * airspeed;
+	const Vector2f airspeed_sp_vector_unit = Vector2f{cosf(heading_sp), sinf(heading_sp)};
+
+	const float dot_air_vel_err = airspeed_vector.dot(airspeed_sp_vector_unit);
+	const float cross_air_vel_err = airspeed_vector.cross(airspeed_sp_vector_unit);
+
+	if (dot_air_vel_err < 0.0f) {
+		// hold max lateral acceleration command above 90 deg heading error
+		return p_gain_ * ((cross_air_vel_err < 0.0f) ? -airspeed : airspeed);
+
+	} else {
+		// airspeed/airspeed_ref is used to scale any incremented airspeed reference back to the current airspeed
+		// for acceleration commands in a "feedback" sense (i.e. at the current vehicle airspeed)
+		return p_gain_ * cross_air_vel_err;
+	}
+}

src/lib/npfg/AirspeedReferenceController.hpp

@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file AirspeedRefController.hpp
+ *
+ * Original Author:  Thomas Stastny <[email protected]>
+ * Refactored to better suite new control API: Roman Bapst <[email protected]>
+ *
+ * * Notes:
+ * - The wind estimate should be dynamic enough to capture ~1-2 second length gusts,
+ *   Otherwise the performance will suffer.
+ *
+ * Acknowledgements and References:
+ *
+ * The logic is mostly based on [1] and Paper III of [2].
+ * TODO: Concise, up to date documentation and stability analysis for the following
+ *       implementation.
+ *
+ * [1] T. Stastny and R. Siegwart. "On Flying Backwards: Preventing Run-away of
+ *     Small, Low-speed, Fixed-wing UAVs in Strong Winds". IEEE International Conference
+ *     on Intelligent Robots and Systems (IROS). 2019.
+ *     https://arxiv.org/pdf/1908.01381.pdf
+ * [2] T. Stastny. "Low-Altitude Control and Local Re-Planning Strategies for Small
+ *     Fixed-Wing UAVs". Doctoral Thesis, ETH Zürich. 2020.
+ *     https://tstastny.github.io/pdf/tstastny_phd_thesis_wcover.pdf
+ */
+
+#ifndef PX4_AIRSPEEDREFERENCECONTROLLER_HPP
+#define PX4_AIRSPEEDREFERENCECONTROLLER_HPP
+
+class AirspeedReferenceController
+{
+public:
+
+	AirspeedReferenceController();
+
+
+	float controlHeading(const float heading_sp, const float heading, const float airspeed) const;
+
+private:
+	float p_gain_{0.8885f}; // proportional gain (computed from period_ and damping_) [rad/s]
+};
+
+#endif //PX4_AIRSPEEDREFERENCECONTROLLER_HPP

src/lib/npfg/CMakeLists.txt

@@ -32,8 +32,12 @@
 ############################################################################
 
 px4_add_library(npfg
-	npfg.cpp
-	npfg.hpp
+	DirectionalGuidance.cpp
+	CourseToAirspeedRefMapper.cpp
+	AirspeedReferenceController.cpp
+	DirectionalGuidance.hpp
+	CourseToAirspeedRefMapper.hpp
+	AirspeedReferenceController.hpp
 )
 
 target_link_libraries(npfg PRIVATE geo)

src/lib/npfg/CourseToAirspeedRefMapper.cpp

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "CourseToAirspeedRefMapper.hpp"
+using matrix::Vector2f;
+
+float
+CourseToAirspeedRefMapper::mapCourseSetpointToHeadingSetpoint(const float bearing_setpoint, const Vector2f &wind_vel,
+		float airspeed_true) const
+{
+
+	Vector2f bearing_vector = Vector2f{cosf(bearing_setpoint), sinf(bearing_setpoint)};
+	const float wind_cross_bearing = wind_vel.cross(bearing_vector);
+	const float wind_dot_bearing = wind_vel.dot(bearing_vector);
+
+	const Vector2f air_vel_ref = refAirVelocity(wind_vel, bearing_vector, wind_cross_bearing,
+				     wind_dot_bearing, wind_vel.norm(), airspeed_true);
+
+	return atan2f(air_vel_ref(1), air_vel_ref(0));
+}
+
+matrix::Vector2f CourseToAirspeedRefMapper::refAirVelocity(const Vector2f &wind_vel, const Vector2f &bearing_vec,
+		const float wind_cross_bearing, const float wind_dot_bearing,
+		const float wind_speed, float airspeed_true) const
+{
+	Vector2f air_vel_ref;
+
+	if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_true, wind_speed)) {
+		const float airsp_dot_bearing = projectAirspOnBearing(airspeed_true, wind_cross_bearing);
+		air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);
+
+	} else {
+		air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vec, wind_speed, airspeed_true);
+	}
+
+	return air_vel_ref;
+}
+
+float CourseToAirspeedRefMapper::projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const
+{
+	// NOTE: wind_cross_bearing must be less than airspeed to use this function
+	// it is assumed that bearing feasibility is checked and found feasible (e.g. bearingIsFeasible() = true) prior to entering this method
+	// otherwise the return will be erroneous
+	return sqrtf(math::max(airspeed * airspeed - wind_cross_bearing * wind_cross_bearing, 0.0f));
+}
+
+int CourseToAirspeedRefMapper::bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing,
+		const float airspeed, const float wind_speed) const
+{
+	return (fabsf(wind_cross_bearing) < airspeed) && ((wind_dot_bearing > 0.0f) || (wind_speed < airspeed));
+}
+
+matrix::Vector2f
+CourseToAirspeedRefMapper::solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing,
+		const Vector2f &bearing_vec) const
+{
+	// essentially a 2D rotation with the speeds (magnitudes) baked in
+	return Vector2f{airsp_dot_bearing * bearing_vec(0) - wind_cross_bearing * bearing_vec(1),
+			wind_cross_bearing * bearing_vec(0) + airsp_dot_bearing * bearing_vec(1)};
+}
+
+matrix::Vector2f CourseToAirspeedRefMapper::infeasibleAirVelRef(const Vector2f &wind_vel, const Vector2f &bearing_vec,
+		const float wind_speed, const float airspeed) const
+{
+	// NOTE: wind speed must be greater than airspeed, and airspeed must be greater than zero to use this function
+	// it is assumed that bearing feasibility is checked and found infeasible (e.g. bearingIsFeasible() = false) prior to entering this method
+	// otherwise the normalization of the air velocity vector could have a division by zero
+	Vector2f air_vel_ref = sqrtf(math::max(wind_speed * wind_speed - airspeed * airspeed, 0.0f)) * bearing_vec - wind_vel;
+	return air_vel_ref.normalized() * airspeed;
+}