From ee55a3c4b8f2f8774b8785ac7d15a2cb9d8d10b8 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 7 Dec 2023 11:19:32 +0000 Subject: [PATCH 01/19] Plane: extend handling of SET_POSITION_TARGET_GLOBAL_INT - Extend to handle position as well as altitude. - Check for non-zero velocity and acceleration - Calculate and update loiter radius and direction. - Force update of adjust_altitude_target. - Disable debug output to GCS. Signed-off-by: Rhys Mainwaring --- ArduPlane/GCS_Mavlink.cpp | 90 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 88 insertions(+), 2 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index c8ee1e7a20c37..0968ac99e0f11 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1364,7 +1364,7 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess bool msg_valid = true; AP_Mission::Mission_Command cmd = {0}; - + if (pos_target.type_mask & alt_mask) { cmd.content.location.alt = pos_target.alt * 100; @@ -1391,7 +1391,93 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess if (msg_valid) { handle_change_alt_request(cmd); } - } // end if alt_mask + } // end if alt_mask + + //! @todo(srmainwaring) handle mode general control... + //! check the ignore flags + //! check for valid inputs + if (pos_target.type_mask == 0) { + + cmd.content.location.lat = pos_target.lat_int; + cmd.content.location.lng = pos_target.lon_int; + + cmd.content.location.alt = pos_target.alt * 100; + cmd.content.location.relative_alt = false; + cmd.content.location.terrain_alt = false; + switch (pos_target.coordinate_frame) + { + case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + cmd.content.location.relative_alt = true; + break; + default: + gcs().send_text(MAV_SEVERITY_WARNING, "Invalid coord frame in SET_POSTION_TARGET_GLOBAL_INT"); + msg_valid = false; + break; + } + if (msg_valid) { + //! @todo(srmainwaring) remove - used for debugging + if (0) { + float act_lat = plane.current_loc.lat * 1.0E-7; + float act_lng = plane.current_loc.lng * 1.0E-7; + int32_t act_alt_cm; + bool alt_valid = plane.current_loc.get_alt_cm( + Location::AltFrame::ABOVE_HOME, act_alt_cm); + + float tgt_lat = cmd.content.location.lat * 1.0E-7; + float tgt_lng = cmd.content.location.lng * 1.0E-7; + int32_t tgt_alt_cm; + bool tgt_valid = cmd.content.location.get_alt_cm( + Location::AltFrame::ABOVE_HOME, tgt_alt_cm); + + if (alt_valid && tgt_valid) { + gcs().send_text(MAV_SEVERITY_DEBUG, + "err: lat: %f, lng: %f, alt: %f", + tgt_lat - act_lat, tgt_lng - act_lng, + (tgt_alt_cm - act_alt_cm) * 1.0E-2); + } + } + + handle_guided_request(cmd); + + { + //! @todo(srmainwaring) add checks that velocity and accel + //! are present and valid. + + // if the setpoint includes velocity and acceleration, + // calculate a loiter radius and direction. + float vx = pos_target.vx; + float vy = pos_target.vy; + float ax = pos_target.afx; + float ay = pos_target.afy; + + // loiter radius is determined from the acceleration normal + // to the planar velocity and the equation for uniform + // circular motion: a = v^2 / r. + Vector2f vel(vx, vy); + Vector2f accel(ax, ay); + if (!vel.is_zero() && !accel.is_zero()) { + Vector2f vel_unit = vel.normalized(); + Vector2f accel_normal = accel - vel_unit * accel.dot(vel); + Vector2f accel_normal_unit = accel_normal.normalized(); + // % is cross product, -1 converts to body frame with z-dn + // See: ModeGuided::set_radius_and_direction + // plane.loiter.direction = direction_is_ccw ? -1 : 1; + float dir = -1.0 * (vel_unit % accel_normal_unit); + float radius = vel.length_squared() / accel_normal.length(); + bool dir_is_ccw = dir < 0.0; + plane.mode_guided.set_radius_and_direction(radius, dir_is_ccw); + + // //! @todo(srmainwaring) remove - used for debugging + // gcs().send_text(MAV_SEVERITY_DEBUG, + // "radius: %f, dir: %f", radius, dir); + } + } + + // update adjust_altitude_target immediately rather than wait + // for the scheduler. See also: Plane::set_next_WP + plane.adjust_altitude_target(); + } + } } MAV_RESULT GCS_MAVLINK_Plane::handle_command_do_set_mission_current(const mavlink_command_int_t &packet) From 03c49341f6893053baafa391e8cd5b892ee4b88d Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 19 Dec 2023 13:50:31 +0000 Subject: [PATCH 02/19] RC_Channel: add new mode TERRAIN_NAVIGATION Signed-off-by: Rhys Mainwaring --- libraries/RC_Channel/RC_Channel.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 4ad9e532fa049..21ef74b2e4a8e 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -250,6 +250,7 @@ class RC_Channel { CAMERA_LENS = 175, // camera lens selection VFWD_THR_OVERRIDE = 176, // force enabled VTOL forward throttle method + TERRAIN_NAVIGATION = 177, // guided terrain navigation mode // inputs from 200 will eventually used to replace RCMAP ROLL = 201, // roll input From d3ef7db7db700a0b5e9a3eaacb4e59a40eed38bd Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 19 Dec 2023 13:51:53 +0000 Subject: [PATCH 03/19] Plane: add new mode TERRAIN_NAVIGATION - Initial implementation forwards to GUIDED. - Add mode to parameters. - Send msg to GCS when switching to TERRAIN_NAVIGATION Signed-off-by: Rhys Mainwaring --- ArduPlane/ArduPlane.cpp | 1 + ArduPlane/GCS_Mavlink.cpp | 1 + ArduPlane/GCS_Plane.cpp | 1 + ArduPlane/Parameters.cpp | 2 +- ArduPlane/Plane.h | 2 ++ ArduPlane/RC_Channel.cpp | 4 ++++ ArduPlane/altitude.cpp | 1 + ArduPlane/control_modes.cpp | 3 +++ ArduPlane/events.cpp | 2 ++ ArduPlane/mode.cpp | 2 ++ ArduPlane/mode.h | 28 +++++++++++++++++++++++ ArduPlane/mode_terrain_navigation.cpp | 33 +++++++++++++++++++++++++++ ArduPlane/quadplane.h | 1 + ArduPlane/reverse_thrust.cpp | 1 + ArduPlane/system.cpp | 3 ++- 15 files changed, 83 insertions(+), 2 deletions(-) create mode 100644 ArduPlane/mode_terrain_navigation.cpp diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index b8661a45be9fe..0d3f230e6870f 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -830,6 +830,7 @@ bool Plane::get_target_location(Location& target_loc) switch (control_mode->mode_number()) { case Mode::Number::RTL: case Mode::Number::AVOID_ADSB: + case Mode::Number::TERRAIN_NAVIGATION: case Mode::Number::GUIDED: case Mode::Number::AUTO: case Mode::Number::LOITER: diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 0968ac99e0f11..2797d6bb99cd6 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -56,6 +56,7 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const case Mode::Number::LOITER: case Mode::Number::THERMAL: case Mode::Number::AVOID_ADSB: + case Mode::Number::TERRAIN_NAVIGATION: case Mode::Number::GUIDED: case Mode::Number::CIRCLE: case Mode::Number::TAKEOFF: diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp index 39989dba93c64..25a9d4909a413 100644 --- a/ArduPlane/GCS_Plane.cpp +++ b/ArduPlane/GCS_Plane.cpp @@ -67,6 +67,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void) case Mode::Number::RTL: case Mode::Number::LOITER: case Mode::Number::AVOID_ADSB: + case Mode::Number::TERRAIN_NAVIGATION: case Mode::Number::GUIDED: case Mode::Number::CIRCLE: case Mode::Number::TAKEOFF: diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index fe571b2b58293..ea467883d8c19 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -473,7 +473,7 @@ const AP_Param::Info Plane::var_info[] = { // @Param: FLTMODE1 // @DisplayName: FlightMode1 // @Description: Flight mode for switch position 1 (910 to 1230 and above 2049) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,13:TAKEOFF,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO,24:THERMAL,25:Loiter to QLand + // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,13:TAKEOFF,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO,24:THERMAL,25:Loiter to QLand,26:TERRAIN_NAVIGATION // @User: Standard GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1), diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 3d08a2bca9add..5adcf0c77f9a0 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -152,6 +152,7 @@ class Plane : public AP_Vehicle { friend class ModeRTL; friend class ModeLoiter; friend class ModeAvoidADSB; + friend class ModeTerrainNavigation; friend class ModeGuided; friend class ModeInitializing; friend class ModeManual; @@ -273,6 +274,7 @@ class Plane : public AP_Vehicle { #if HAL_ADSB_ENABLED ModeAvoidADSB mode_avoidADSB; #endif + ModeTerrainNavigation mode_terrain_navigation; ModeGuided mode_guided; ModeInitializing mode_initializing; ModeManual mode_manual; diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index d61413c3d0f3a..87cfed2c644ec 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -244,6 +244,10 @@ bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch do_aux_function_change_mode(Mode::Number::LOITER, ch_flag); break; + case AUX_FUNC::TERRAIN_NAVIGATION: + do_aux_function_change_mode(Mode::Number::TERRAIN_NAVIGATION, ch_flag); + break; + case AUX_FUNC::GUIDED: do_aux_function_change_mode(Mode::Number::GUIDED, ch_flag); break; diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 04ca844057074..949ffe1f9e70f 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -68,6 +68,7 @@ void Plane::setup_glide_slope(void) switch (control_mode->mode_number()) { case Mode::Number::RTL: case Mode::Number::AVOID_ADSB: + case Mode::Number::TERRAIN_NAVIGATION: case Mode::Number::GUIDED: /* glide down slowly if above target altitude, but ascend more rapidly if below it. See diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index 7f15e0e2663f9..8ef4aad3b0982 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -52,6 +52,9 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num) case Mode::Number::GUIDED: ret = &mode_guided; break; + case Mode::Number::TERRAIN_NAVIGATION: + ret = &mode_terrain_navigation; + break; case Mode::Number::INITIALISING: ret = &mode_initializing; break; diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index a699625779e02..715016abc2d3c 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -73,6 +73,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso FALLTHROUGH; } case Mode::Number::AVOID_ADSB: + case Mode::Number::TERRAIN_NAVIGATION: case Mode::Number::GUIDED: case Mode::Number::LOITER: case Mode::Number::THERMAL: @@ -186,6 +187,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason FALLTHROUGH; case Mode::Number::AVOID_ADSB: + case Mode::Number::TERRAIN_NAVIGATION: case Mode::Number::GUIDED: if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) { diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 30ec5fc5b0fb1..775da43a947dd 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -12,6 +12,8 @@ Mode::Mode() : { } +Mode::~Mode() = default; + void Mode::exit() { // call sub-classes exit diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 3721eb8de32d7..76267ed8f7d93 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -54,6 +54,7 @@ class Mode #if HAL_QUADPLANE_ENABLED LOITER_ALT_QLAND = 25, #endif + TERRAIN_NAVIGATION = 26, }; // Constructor @@ -142,6 +143,8 @@ class Mode virtual bool use_battery_compensation() const; protected: + // Destructor + virtual ~Mode(); // subclasses override this to perform checks before entering the mode virtual bool _enter() { return true; } @@ -309,6 +312,31 @@ class ModeGuided : public Mode float active_radius_m; }; +class ModeTerrainNavigation : public ModeGuided +{ +public: + + Number mode_number() const override { return Number::TERRAIN_NAVIGATION; } + const char *name() const override { return "TERRAIN_NAVIGATION"; } + const char *name4() const override { return "GUID"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + + void navigate() override; + + // handle a guided target request from GCS + bool handle_guided_request(Location target_loc) override; + + void set_radius_and_direction(const float radius, const bool direction_is_ccw); + + void update_target_altitude() override; + +protected: + + bool _enter() override; +}; + class ModeCircle: public Mode { public: diff --git a/ArduPlane/mode_terrain_navigation.cpp b/ArduPlane/mode_terrain_navigation.cpp new file mode 100644 index 0000000000000..f31bd1d3dae07 --- /dev/null +++ b/ArduPlane/mode_terrain_navigation.cpp @@ -0,0 +1,33 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeTerrainNavigation::_enter() +{ + gcs().send_text(MAV_SEVERITY_DEBUG, "Entering ModeTerrainNavigation"); + return ModeGuided::_enter(); +} + +void ModeTerrainNavigation::update() +{ + ModeGuided::update(); +} + +void ModeTerrainNavigation::navigate() +{ + ModeGuided::navigate(); +} + +bool ModeTerrainNavigation::handle_guided_request(Location target_loc) +{ + return ModeGuided::handle_guided_request(target_loc); +} + +void ModeTerrainNavigation::set_radius_and_direction(const float radius, const bool direction_is_ccw) +{ + ModeGuided::set_radius_and_direction(radius, direction_is_ccw); +} + +void ModeTerrainNavigation::update_target_altitude() +{ + ModeGuided::update_target_altitude(); +} diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 5b2b35fa92dff..7963beadb519b 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -51,6 +51,7 @@ class QuadPlane friend class ModeAuto; friend class ModeRTL; friend class ModeAvoidADSB; + friend class ModeTerrainNavigation; friend class ModeGuided; friend class ModeQHover; friend class ModeQLand; diff --git a/ArduPlane/reverse_thrust.cpp b/ArduPlane/reverse_thrust.cpp index 16dc514b54764..e844fb45057f3 100644 --- a/ArduPlane/reverse_thrust.cpp +++ b/ArduPlane/reverse_thrust.cpp @@ -84,6 +84,7 @@ bool Plane::allow_reverse_thrust(void) const allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_FBWB); break; case Mode::Number::AVOID_ADSB: + case Mode::Number::TERRAIN_NAVIGATION: case Mode::Number::GUIDED: allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_GUIDED); break; diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 593dfb1c16e8c..44e9be9cbfc85 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -219,9 +219,10 @@ bool Plane::gcs_mode_enabled(const Mode::Number mode_num) const (uint8_t)Mode::Number::QLOITER, (uint8_t)Mode::Number::QACRO, #if QAUTOTUNE_ENABLED - (uint8_t)Mode::Number::QAUTOTUNE + (uint8_t)Mode::Number::QAUTOTUNE, #endif #endif + (uint8_t)Mode::Number::TERRAIN_NAVIGATION }; return !block_GCS_mode_change((uint8_t)mode_num, mode_list, ARRAY_SIZE(mode_list)); From 2bffc458b821c4dc1e349db1a280bf302323128c Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 19 Dec 2023 16:51:00 +0000 Subject: [PATCH 04/19] AP_NPFG: add library for non-linear path following guidance Signed-off-by: Rhys Mainwaring --- libraries/AP_NPFG/AP_NPFG.cpp | 71 +++++++++++++++++++++ libraries/AP_NPFG/AP_NPFG.h | 113 ++++++++++++++++++++++++++++++++++ 2 files changed, 184 insertions(+) create mode 100644 libraries/AP_NPFG/AP_NPFG.cpp create mode 100644 libraries/AP_NPFG/AP_NPFG.h diff --git a/libraries/AP_NPFG/AP_NPFG.cpp b/libraries/AP_NPFG/AP_NPFG.cpp new file mode 100644 index 0000000000000..5358acc211a0a --- /dev/null +++ b/libraries/AP_NPFG/AP_NPFG.cpp @@ -0,0 +1,71 @@ +#include "AP_NPFG.h" + +#include + +extern const AP_HAL::HAL& hal; + +int32_t AP_NPFG::nav_roll_cd(void) const { + return 0; +} + +float AP_NPFG::lateral_acceleration(void) const { + return 0.0; +} + +int32_t AP_NPFG::nav_bearing_cd(void) const { + return 0; +} + +int32_t AP_NPFG::bearing_error_cd(void) const { + return 0; +} + +int32_t AP_NPFG::target_bearing_cd(void) const { + return 0; +} + +float AP_NPFG::crosstrack_error(void) const { + return 0.0; +} + +float AP_NPFG::crosstrack_error_integrator(void) const { + return 0.0; +} + +float AP_NPFG::turn_distance(float wp_radius) const { + return 0.0; +} + +float AP_NPFG::turn_distance(float wp_radius, float turn_angle) const { + return 0.0; +} + +float AP_NPFG::loiter_radius(const float radius) const { + return 0.0; +} + +void AP_NPFG::update_waypoint(const class Location &prev_WP, const class Location &next_WP, float dist_min) { +} + +void AP_NPFG::update_loiter(const class Location ¢er_WP, float radius, int8_t loiter_direction) { +} + +void AP_NPFG::update_heading_hold(int32_t navigation_heading_cd) { +} + +void AP_NPFG::update_level_flight(void) { +} + +bool AP_NPFG::reached_loiter_target(void) { + return false; +} + +void AP_NPFG::set_data_is_stale(void) { +} + +bool AP_NPFG::data_is_stale(void) const { + return false; +} + +void AP_NPFG::set_reverse(bool reverse) { +} diff --git a/libraries/AP_NPFG/AP_NPFG.h b/libraries/AP_NPFG/AP_NPFG.h new file mode 100644 index 0000000000000..b3e542dd11caf --- /dev/null +++ b/libraries/AP_NPFG/AP_NPFG.h @@ -0,0 +1,113 @@ +/// @file AP_NPFG.h +/// @brief Non-Linear path following guidance + +#pragma once + +#include +#include + +class AP_NPFG : public AP_Navigation { +public: + // return the desired roll angle in centi-degrees to move towards + // the target waypoint + int32_t nav_roll_cd(void) const override; + + // return the desired lateral acceleration in m/s/s to move towards + // the target waypoint + float lateral_acceleration(void) const override; + + // note: all centi-degree values returned in AP_Navigation should + // be wrapped at -18000 to 18000 in centi-degrees. + + // return the tracking bearing that the navigation controller is + // using in centi-degrees. This is used to display an arrow on + // ground stations showing the effect of the cross-tracking in the + // controller + int32_t nav_bearing_cd(void) const override; + + // return the difference between the vehicles current course and + // the nav_bearing_cd() in centi-degrees. A positive value means + // the vehicle is tracking too far to the left of the correct + // bearing. + int32_t bearing_error_cd(void) const override; + + // return the target bearing in centi-degrees. This is the bearing + // from the vehicles current position to the target waypoint. This + // should be calculated in the update_*() functions below. + int32_t target_bearing_cd(void) const override; + + // return the crosstrack error in meters. This is the distance in + // the X-Y plane that we are off the desired track + float crosstrack_error(void) const override; + float crosstrack_error_integrator(void) const override; + + // return the distance in meters at which a turn should commence + // to allow the vehicle to neatly move to the next track in the + // mission when approaching a waypoint. Assumes 90 degree turn + float turn_distance(float wp_radius) const override; + + // return the distance in meters at which a turn should commence + // to allow the vehicle to neatly move to the next track in the + // mission when approaching a waypoint + float turn_distance(float wp_radius, float turn_angle) const override; + + // return the target loiter radius for the current location that + // will not cause excessive airframe loading + float loiter_radius(const float radius) const override; + + // update the internal state of the navigation controller, given + // the previous and next waypoints. This is the step function for + // navigation control for path following between two points. This + // function is called at regular intervals (typically 10Hz). The + // main flight code will call an output function (such as + // nav_roll_cd()) after this function to ask for the new required + // navigation attitude/steering. + void update_waypoint(const class Location &prev_WP, const class Location &next_WP, float dist_min = 0.0f) override; + + // update the internal state of the navigation controller for when + // the vehicle has been commanded to circle about a point. This + // is the step function for navigation control for circling. This + // function is called at regular intervals (typically 10Hz). The + // main flight code will call an output function (such as + // nav_roll_cd()) after this function to ask for the new required + // navigation attitude/steering. + void update_loiter(const class Location ¢er_WP, float radius, int8_t loiter_direction) override; + + // update the internal state of the navigation controller, given a + // fixed heading. This is the step function for navigation control + // for a fixed heading. This function is called at regular + // intervals (typically 10Hz). The main flight code will call an + // output function (such as nav_roll_cd()) after this function to + // ask for the new required navigation attitude/steering. + void update_heading_hold(int32_t navigation_heading_cd) override; + + // update the internal state of the navigation controller for + // level flight on the current heading. This is the step function + // for navigation control for level flight. This function is + // called at regular intervals (typically 10Hz). The main flight + // code will call an output function (such as nav_roll_cd()) after + // this function to ask for the new required navigation + // attitude/steering. + void update_level_flight(void) override; + + // return true if we have reached the target loiter location. This + // may be a fuzzy decision, depending on internal navigation + // parameters. For example the controller may return true only + // when on the circular path around the waypoint, and not when + // tracking towards the center. This function is only valid when + // the update_loiter() method is used + bool reached_loiter_target(void) override; + + // notify Navigation controller that a new waypoint has just been + // processed. This means that until we handle an update_XXX() function + // the data is stale with old navigation information. + void set_data_is_stale(void) override; + + // return true if a new waypoint has been processed by mission + // controller but the navigation controller still has old stale data + // from previous waypoint navigation handling. This gets cleared on + // every update_XXXXXX() call. + bool data_is_stale(void) const override; + + void set_reverse(bool reverse) override; +}; From f034e0da6adb47fb744fdd4a1c4c9da270b43dcf Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 19 Dec 2023 16:51:22 +0000 Subject: [PATCH 05/19] Plane: include AP_NPFG library Signed-off-by: Rhys Mainwaring --- ArduPlane/wscript | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduPlane/wscript b/ArduPlane/wscript index 4dd670ccc34a5..76dcc33f1adcd 100644 --- a/ArduPlane/wscript +++ b/ArduPlane/wscript @@ -14,6 +14,7 @@ def build(bld): 'AP_Camera', 'AP_L1_Control', 'AP_Navigation', + 'AP_NPFG', 'AP_RCMapper', 'AP_TECS', 'AP_InertialNav', From c8ce840f4837d6d612e19e7adc2bb21f2ead8811 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 19 Dec 2023 17:29:53 +0000 Subject: [PATCH 06/19] AP_NPFG: add npfg controller - Copy npfg implementation from PX4 - Port npfg to ArduPilot - Replace tabs in npfg with whitespace - Change suffix in npfg header - Add NPFG instance to AP_Navigation adapter - Add parameters - Define static const variables in cpp - Update param group name Signed-off-by: Rhys Mainwaring --- libraries/AP_NPFG/AP_NPFG.cpp | 123 +++++++ libraries/AP_NPFG/AP_NPFG.h | 41 ++- libraries/AP_NPFG/npfg.cpp | 490 +++++++++++++++++++++++++++ libraries/AP_NPFG/npfg.h | 614 ++++++++++++++++++++++++++++++++++ 4 files changed, 1267 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_NPFG/npfg.cpp create mode 100644 libraries/AP_NPFG/npfg.h diff --git a/libraries/AP_NPFG/AP_NPFG.cpp b/libraries/AP_NPFG/AP_NPFG.cpp index 5358acc211a0a..8fcaae95256b8 100644 --- a/libraries/AP_NPFG/AP_NPFG.cpp +++ b/libraries/AP_NPFG/AP_NPFG.cpp @@ -4,6 +4,85 @@ extern const AP_HAL::HAL& hal; +// table of parameters +const AP_Param::GroupInfo AP_NPFG::var_info[] = { + // @Param: PERIOD + // @DisplayName: NPFG control period. + // @Description: Period in seconds of NPFG controller. + // @Units: s + // @User: Standard + AP_GROUPINFO("PERIOD", 0, AP_NPFG, _npfg_period, 10.0f), + + // @Param: DAMPING + // @DisplayName: NPFG control damping ratio. + // @Description: Damping ratio for NPFG controller. + // @User: Advanced + AP_GROUPINFO("DAMPING", 1, AP_NPFG, _npfg_damping, 0.707f), + + // @Param: LB_PERIOD + // @DisplayName: NPFG enable control period lower bound constraints. + // @Description: Enable automatic lower bounding of the user set controller period. + // @User: Advanced + AP_GROUPINFO("LB_PERIOD", 2, AP_NPFG, _npfg_en_period_lb, 1), + + // @Param: UB_PERIOD + // @DisplayName: NPFG enable control period upper bound constraints. + // @Description: Enable automatic adaptation of the user set controller period for track keeping performance. + // @User: Advanced + AP_GROUPINFO("UB_PERIOD", 3, AP_NPFG, _npfg_en_period_ub, 1), + + // @Param: TRACK_KEEP + // @DisplayName: NPFG enable track keeping. + // @Description: The airspeed reference is incremented to return to the track and sustain zero ground velocity until excess wind subsides. + // @User: Advanced + AP_GROUPINFO("TRACK_KEEP", 4, AP_NPFG, _npfg_en_track_keeping, 0), + + // @Param: EN_MIN_GSP + // @DisplayName: NPFG enable min ground speed. + // @Description: The airspeed reference is incremented to sustain a user defined minimum forward ground speed. + // @User: Advanced + AP_GROUPINFO("EN_MIN_GSP", 5, AP_NPFG, _npfg_en_min_gsp, 1), + + // @Param: WIND_REG + // @DisplayName: NPFG enable excess wind regulation. + // @Description: The airspeed reference is incremented to regulate the excess wind, but not overcome it. Disabling this parameter disables all other excess wind handling options, using only the nominal airspeed for reference. + // @User: Advanced + AP_GROUPINFO("WIND_REG", 6, AP_NPFG, _npfg_en_wind_reg, 1), + + // @Param: GSP_MAX_TK + // @DisplayName: NPFG track keeping max, min. + // @Description: Maximum, minimum forward ground speed demand from track keeping logic. + // @User: Advanced + AP_GROUPINFO("GSP_MAX_TK", 7, AP_NPFG, _npfg_track_keeping_gsp_max, 5.0f), + + // @Param: ROLL_TC + // @DisplayName: NPFG roll time constant. + // @Description: Autopilot roll response time constant. + // @User: Advanced + AP_GROUPINFO("ROLL_TC", 8, AP_NPFG, _npfg_roll_time_const, 0.0f), + + // @Param: SW_DST_MLT + // @DisplayName: NPFG switch distance multiplier. + // @Description: A value multiplied by the track error boundary resulting in a lower switch distance as the bearing angle changes quadratically (instead of linearly as in L1), the time constant (automatically calculated for on track stability) proportional track error boundary typically over estimates the required switching distance. + // @User: Advanced + AP_GROUPINFO("SW_DST_MLT", 9, AP_NPFG, _npfg_switch_distance_multiplier, 0.32f), + + // @Param: PERIOD_SF + // @DisplayName: NPFG period safety factor. + // @Description: Multiplied by the minimum period for conservative lower bound. + // @User: Advanced + AP_GROUPINFO("PERIOD_SF", 10, AP_NPFG, _npfg_period_safety_factor, 1.5f), + + AP_GROUPEND +}; + +AP_NPFG::AP_NPFG(AP_AHRS &ahrs, const AP_TECS *tecs) + : _ahrs(ahrs) + , _tecs(tecs) +{ + update_parameters(); +} + int32_t AP_NPFG::nav_roll_cd(void) const { return 0; } @@ -48,6 +127,30 @@ void AP_NPFG::update_waypoint(const class Location &prev_WP, const class Locatio } void AP_NPFG::update_loiter(const class Location ¢er_WP, float radius, int8_t loiter_direction) { + // check for parameter updates + update_parameters(); + + //! @todo - obtain current position from _ahrs in local cartesian coords + Vector2f curr_pos_local; + + //! @todo - obtain ground vel from _ahrs in local cartesian coords + Vector2f ground_vel; + + //! @todo - obtain wind vel from _ahrs in local cartesian coords + Vector2f wind_vel; + + //! @todo - unit path tangent is the normalised setpoint velocity + Vector2f unit_path_tangent; + + //! @todo - the setpoint position in local cartesian coords + Vector2f position_on_path; + + //! @todo calculate from loiter radius and direction + //! @todo check loiter radius is not zero or infinite + float path_curvature = -1.0 * float(loiter_direction) / radius; + + _npfg.guideToPath(curr_pos_local, ground_vel, wind_vel, + unit_path_tangent, position_on_path, path_curvature); } void AP_NPFG::update_heading_hold(int32_t navigation_heading_cd) { @@ -69,3 +172,23 @@ bool AP_NPFG::data_is_stale(void) const { void AP_NPFG::set_reverse(bool reverse) { } + +void AP_NPFG::update_parameters() { + + //! @todo populate all parameters + + _npfg.setPeriod(_npfg_period); + _npfg.setDamping(_npfg_damping); + _npfg.enablePeriodLB(_npfg_en_period_lb); + _npfg.enablePeriodUB(_npfg_en_period_ub); + _npfg.enableMinGroundSpeed(_npfg_en_min_gsp); + _npfg.enableTrackKeeping(_npfg_en_track_keeping); + _npfg.enableWindExcessRegulation(_npfg_en_wind_reg); + // _npfg.setMinGroundSpeed(_param_fw_gnd_spd_min); + _npfg.setMaxTrackKeepingMinGroundSpeed(_npfg_track_keeping_gsp_max); + _npfg.setRollTimeConst(_npfg_roll_time_const); + _npfg.setSwitchDistanceMultiplier(_npfg_switch_distance_multiplier); + // _npfg.setRollLimit(radians(_param_fw_r_lim)); + // _npfg.setRollSlewRate(radians(_param_fw_pn_r_slew_max)); + _npfg.setPeriodSafetyFactor(_npfg_period_safety_factor); +} diff --git a/libraries/AP_NPFG/AP_NPFG.h b/libraries/AP_NPFG/AP_NPFG.h index b3e542dd11caf..2eb5d61c4bf58 100644 --- a/libraries/AP_NPFG/AP_NPFG.h +++ b/libraries/AP_NPFG/AP_NPFG.h @@ -3,11 +3,22 @@ #pragma once -#include +#include +#include #include +#include +#include +#include + +#include "AP_NPFG/npfg.h" class AP_NPFG : public AP_Navigation { public: + AP_NPFG(AP_AHRS &ahrs, const AP_TECS *tecs); + + // do not permit copies + CLASS_NO_COPY(AP_NPFG); + // return the desired roll angle in centi-degrees to move towards // the target waypoint int32_t nav_roll_cd(void) const override; @@ -110,4 +121,32 @@ class AP_NPFG : public AP_Navigation { bool data_is_stale(void) const override; void set_reverse(bool reverse) override; + + // store the NPFG_* user settable parameters + static const struct AP_Param::GroupInfo var_info[]; + +private: + void update_parameters(); + + // reference to the AHRS object + AP_AHRS &_ahrs; + + // pointer to the SpdHgtControl object + const AP_TECS *_tecs; + + // nonlinear path following guidance implementation + NPFG _npfg; + + // parameters + AP_Int8 _npfg_en_period_lb; + AP_Int8 _npfg_en_period_ub; + AP_Int8 _npfg_en_track_keeping; + AP_Int8 _npfg_en_min_gsp; + AP_Int8 _npfg_en_wind_reg; + AP_Float _npfg_period; + AP_Float _npfg_damping; + AP_Float _npfg_track_keeping_gsp_max; + AP_Float _npfg_roll_time_const; + AP_Float _npfg_switch_distance_multiplier; + AP_Float _npfg_period_safety_factor; }; diff --git a/libraries/AP_NPFG/npfg.cpp b/libraries/AP_NPFG/npfg.cpp new file mode 100644 index 0000000000000..7fa064e70f345 --- /dev/null +++ b/libraries/AP_NPFG/npfg.cpp @@ -0,0 +1,490 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 npfg.cpp + * Implementation of a lateral-directional nonlinear path following guidance + * law with excess wind handling. Commands lateral acceleration and airspeed. + * + * Authors and acknowledgements in header. + */ + +#include "npfg.h" + +#include + +static inline constexpr bool ISFINITE(float x) { return __builtin_isfinite(x); } + +const float NPFG::NPFG_EPSILON = 1.0e-6; // small number *bigger than machine epsilon +const float NPFG::MIN_RADIUS = 0.5f; // minimum effective radius (avoid singularities) [m] +const float NPFG::NTE_FRACTION = 0.5f; // normalized track error fraction (must be > 0) +const float NPFG::AIRSPEED_BUFFER = 1.5f; // airspeed buffer [m/s] (must be > 0) + +void NPFG::guideToPath(const Vector2f &curr_pos_local, const Vector2f &ground_vel, const Vector2f &wind_vel, + const Vector2f &unit_path_tangent, + const Vector2f &position_on_path, const float path_curvature) +{ + const float ground_speed = ground_vel.length(); + + const Vector2f air_vel = ground_vel - wind_vel; + const float airspeed = air_vel.length(); + + const float wind_speed = wind_vel.length(); + + const Vector2f path_pos_to_vehicle{curr_pos_local - position_on_path}; + signed_track_error_ = unit_path_tangent % path_pos_to_vehicle; + + // on-track wind triangle projections + const float wind_cross_upt = wind_vel % unit_path_tangent; + const float wind_dot_upt = wind_vel.dot(unit_path_tangent); + + // calculate the bearing feasibility on the track at the current closest point + feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed); + + const float track_error = fabsf(signed_track_error_); + + // update control parameters considering upper and lower stability bounds (if enabled) + // must be called before trackErrorBound() as it updates time_const_ + adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, track_error, + path_curvature, wind_vel, unit_path_tangent, feas_on_track_); + p_gain_ = pGain(adapted_period_, damping_); + time_const_ = timeConst(adapted_period_, damping_); + + // track error bound is dynamic depending on ground speed + track_error_bound_ = trackErrorBound(ground_speed, time_const_); + const float normalized_track_error = normalizedTrackError(track_error, track_error_bound_); + + // look ahead angle based solely on track proximity + const float look_ahead_ang = lookAheadAngle(normalized_track_error); + + track_proximity_ = trackProximity(look_ahead_ang); + + bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error_); + + // wind triangle projections + const float wind_cross_bearing = wind_vel % bearing_vec_; + const float wind_dot_bearing = wind_vel.dot(bearing_vec_); + + // continuous representation of the bearing feasibility + feas_ = bearingFeasibility(wind_cross_bearing, wind_dot_bearing, airspeed, wind_speed); + + // we consider feasibility of both the current bearing as well as that on the track at the current closest point + const float feas_combined = feas_ * feas_on_track_; + + min_ground_speed_ref_ = minGroundSpeed(normalized_track_error, feas_combined); + + // reference air velocity with directional feedforward effect for following + // curvature in wind and magnitude incrementation depending on minimum ground + // speed violations and/or high wind conditions in general + air_vel_ref_ = refAirVelocity(wind_vel, bearing_vec_, wind_cross_bearing, + wind_dot_bearing, wind_speed, min_ground_speed_ref_); + airspeed_ref_ = air_vel_ref_.length(); + + // lateral acceleration demand based on heading error + const float lateral_accel = lateralAccel(air_vel, air_vel_ref_, airspeed); + + // lateral acceleration needed to stay on curved track (assuming no heading error) + lateral_accel_ff_ = lateralAccelFF(unit_path_tangent, ground_vel, wind_dot_upt, + wind_cross_upt, airspeed, wind_speed, signed_track_error_, path_curvature); + + // total lateral acceleration to drive aircaft towards track as well as account + // for path curvature. The full effect of the feed-forward acceleration is smoothly + // ramped in as the vehicle approaches the track and is further smoothly + // zeroed out as the bearing becomes infeasible. + lateral_accel_ = lateral_accel + feas_combined * track_proximity_ * lateral_accel_ff_; + + updateRollSetpoint(); +} // guideToPath + +float NPFG::adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed, + const float track_error, const float path_curvature, const Vector2f &wind_vel, + const Vector2f &unit_path_tangent, const float feas_on_track) const +{ + float period = period_; + const float air_turn_rate = fabsf(path_curvature * airspeed); + const float wind_factor = windFactor(airspeed, wind_speed); + + if (en_period_lb_ && roll_time_const_ > NPFG_EPSILON) { + // lower bound for period not considering path curvature + const float period_lb_zero_curvature = periodLowerBound(0.0f, wind_factor, feas_on_track) * period_safety_factor_; + + // lower bound for period *considering path curvature + float period_lb = periodLowerBound(air_turn_rate, wind_factor, feas_on_track) * period_safety_factor_; + + // calculate the time constant and track error bound considering the zero + // curvature, lower-bounded period and subsequently recalculate the normalized + // track error + const float time_const = timeConst(period_lb_zero_curvature, damping_); + const float track_error_bound = trackErrorBound(ground_speed, time_const); + const float normalized_track_error = normalizedTrackError(track_error, track_error_bound); + + // calculate nominal track proximity with lower bounded time constant + // (only a numerical solution can find corresponding track proximity + // and adapted gains simultaneously) + const float look_ahead_ang = lookAheadAngle(normalized_track_error); + const float track_proximity = trackProximity(look_ahead_ang); + + // linearly ramp in curvature dependent lower bound with track proximity + period_lb = period_lb * track_proximity + (1.0f - track_proximity) * period_lb_zero_curvature; + + // lower bounded period + period = MAX(period_lb, period); + + // only allow upper bounding ONLY if lower bounding is enabled (is otherwise + // dangerous to allow period decrements without stability checks). + // NOTE: if the roll time constant is not accurately known, lower-bound + // checks may be too optimistic and reducing the period can still destabilize + // the system! enable this feature at your own risk. + if (en_period_ub_) { + + const float period_ub = periodUpperBound(air_turn_rate, wind_factor, feas_on_track); + + if (en_period_ub_ && ISFINITE(period_ub) && period > period_ub) { + // NOTE: if the roll time constant is not accurately known, reducing + // the period here can destabilize the system! + // enable this feature at your own risk! + + // upper bound the period (for track keeping stability), prefer lower bound if violated + const float period_adapted = MAX(period_lb, period_ub); + + // transition from the nominal period to the adapted period as we get + // closer to the track + period = period_adapted * track_proximity + (1.0f - track_proximity) * period; + } + } + } + + return period; +} // adaptPeriod + +float NPFG::normalizedTrackError(const float track_error, const float track_error_bound) const +{ + return constrain_value(track_error / track_error_bound, 0.0f, 1.0f); +} + +float NPFG::windFactor(const float airspeed, const float wind_speed) const +{ + // See [TODO: include citation] for definition/elaboration of this approximation. + if (wind_speed > airspeed || airspeed < NPFG_EPSILON) { + return 2.0f; + + } else { + return 2.0f * (1.0f - sqrtf(1.0f - MIN(1.0f, wind_speed / airspeed))); + } +} // windFactor + +float NPFG::periodUpperBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const +{ + if (air_turn_rate * wind_factor > NPFG_EPSILON) { + // multiply air turn rate by feasibility on track to zero out when we anyway + // should not consider the curvature + return 4.0f * M_PI * damping_ / (air_turn_rate * wind_factor * feas_on_track); + } + + return INFINITY; +} // periodUB + +float NPFG::periodLowerBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const +{ + // this method considers a "conservative" lower period bound, i.e. a constant + // worst case bound for any wind ratio, airspeed, and path curvature + + // the lower bound for zero curvature and no wind OR damping ratio < 0.5 + const float period_lb = M_PI * roll_time_const_ / damping_; + + if (air_turn_rate * wind_factor < NPFG_EPSILON || damping_ < 0.5f) { + return period_lb; + + } else { + // the lower bound for tracking a curved path in wind with damping ratio > 0.5 + const float period_windy_curved_damped = 4.0f * M_PI * roll_time_const_ * damping_; + + // blend the two together as the bearing on track becomes less feasible + return period_windy_curved_damped * feas_on_track + (1.0f - feas_on_track) * period_lb; + } +} // periodLB + +float NPFG::trackProximity(const float look_ahead_ang) const +{ + const float sin_look_ahead_ang = sinf(look_ahead_ang); + return sin_look_ahead_ang * sin_look_ahead_ang; +} // trackProximity + +float NPFG::trackErrorBound(const float ground_speed, const float time_const) const +{ + if (ground_speed > 1.0f) { + return ground_speed * time_const; + + } else { + // limit bound to some minimum ground speed to avoid singularities in track + // error normalization. the following equation assumes ground speed minimum = 1.0 + return 0.5f * time_const * (ground_speed * ground_speed + 1.0f); + } +} // trackErrorBound + +float NPFG::pGain(const float period, const float damping) const +{ + return 4.0f * M_PI * damping / period; +} // pGain + +float NPFG::timeConst(const float period, const float damping) const +{ + return period * damping; +} // timeConst + +float NPFG::lookAheadAngle(const float normalized_track_error) const +{ + return M_PI_2 * (normalized_track_error - 1.0f) * (normalized_track_error - 1.0f); +} // lookAheadAngle + +Vector2f NPFG::bearingVec(const Vector2f &unit_path_tangent, const float look_ahead_ang, + const float signed_track_error) const +{ + const float cos_look_ahead_ang = cosf(look_ahead_ang); + const float sin_look_ahead_ang = sinf(look_ahead_ang); + + Vector2f unit_path_normal(-unit_path_tangent[1], unit_path_tangent[0]); // right handed 90 deg (clockwise) turn + Vector2f unit_track_error = unit_path_normal * -1.0 * ((signed_track_error < 0.0f) ? -1.0f : 1.0f); + + return unit_track_error * cos_look_ahead_ang + unit_path_tangent * sin_look_ahead_ang; +} // bearingVec + +float NPFG::minGroundSpeed(const float normalized_track_error, const float feas) +{ + // minimum ground speed demand from track keeping logic + min_gsp_track_keeping_ = 0.0f; + + if (en_track_keeping_ && en_wind_excess_regulation_) { + // zero out track keeping speed increment when bearing is feasible + // maximum track keeping speed increment is applied until we are within + // a user defined fraction of the normalized track error + min_gsp_track_keeping_ = (1.0f - feas) * min_gsp_track_keeping_max_ * constrain_value( + normalized_track_error / NTE_FRACTION, 0.0f, + 1.0f); + } + + // minimum ground speed demand from minimum forward ground speed user setting + float min_gsp_desired = 0.0f; + + if (en_min_ground_speed_ && en_wind_excess_regulation_) { + min_gsp_desired = min_gsp_desired_; + } + + return MAX(min_gsp_track_keeping_, min_gsp_desired); +} // minGroundSpeed + +Vector2f NPFG::refAirVelocity(const Vector2f &wind_vel, const Vector2f &bearing_vec, + const float wind_cross_bearing, const float wind_dot_bearing, const float wind_speed, + const float min_ground_speed) const +{ + Vector2f air_vel_ref; + + if (min_ground_speed > wind_dot_bearing && (en_min_ground_speed_ || en_track_keeping_) && en_wind_excess_regulation_) { + // minimum ground speed and/or track keeping + + // airspeed required to achieve minimum ground speed along bearing vector + const float airspeed_min = sqrtf((min_ground_speed - wind_dot_bearing) * (min_ground_speed - wind_dot_bearing) + + wind_cross_bearing * wind_cross_bearing); + + if (airspeed_min > airspeed_max_) { + if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_max_, wind_speed)) { + // we will not maintain the minimum ground speed, but can still achieve the bearing at maximum airspeed + const float airsp_dot_bearing = projectAirspOnBearing(airspeed_max_, wind_cross_bearing); + air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec); + + } else { + // bearing is maximally infeasible, employ mitigation law + air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vec, wind_speed, airspeed_max_); + } + + } else if (airspeed_min > airspeed_nom_) { + // the minimum ground speed is achievable within the nom - max airspeed range + // solve wind triangle with for air velocity reference with minimum airspeed + const float airsp_dot_bearing = projectAirspOnBearing(airspeed_min, wind_cross_bearing); + air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec); + + } else { + // the minimum required airspeed is less than nominal, so we can track the bearing and minimum + // ground speed with our nominal airspeed reference + const float airsp_dot_bearing = projectAirspOnBearing(airspeed_nom_, wind_cross_bearing); + air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec); + } + + } else { + // wind excess regulation and/or mitigation + + if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_nom_, wind_speed)) { + // bearing is nominally feasible, solve wind triangle for air velocity reference using nominal airspeed + const float airsp_dot_bearing = projectAirspOnBearing(airspeed_nom_, wind_cross_bearing); + air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec); + + } else if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_max_, wind_speed) + && en_wind_excess_regulation_) { + // bearing is maximally feasible + if (wind_dot_bearing <= 0.0f) { + // we only increment the airspeed to regulate, but not overcome, excess wind + // NOTE: in the terminal condition, this will result in a zero ground velocity configuration + air_vel_ref = wind_vel; + + } else { + // the bearing is achievable within the nom - max airspeed range + // solve wind triangle with for air velocity reference with minimum airspeed + const float airsp_dot_bearing = 0.0f; // right angle to the bearing line gives minimal airspeed usage + air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec); + } + + } else { + // bearing is maximally infeasible, employ mitigation law + const float airspeed_input = (en_wind_excess_regulation_) ? airspeed_max_ : airspeed_nom_; + air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vec, wind_speed, airspeed_input); + } + } + + return air_vel_ref; +} // refAirVelocity + +float NPFG::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(MAX(airspeed * airspeed - wind_cross_bearing * wind_cross_bearing, 0.0f)); +} // projectAirspOnBearing + +int NPFG::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)); +} // bearingIsFeasible + +Vector2f NPFG::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]}; +} // solveWindTriangle + +Vector2f NPFG::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 = bearing_vec * sqrtf(MAX(wind_speed * wind_speed - airspeed * airspeed, 0.0f)) - wind_vel; + return air_vel_ref.normalized() * airspeed; +} // infeasibleAirVelRef + +float NPFG::bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, + const float wind_speed) const +{ + if (wind_dot_bearing < 0.0f) { + wind_cross_bearing = wind_speed; + + } else { + wind_cross_bearing = fabsf(wind_cross_bearing); + } + + float sin_arg = sinf(M_PI * 0.5f * constrain_value((airspeed - wind_cross_bearing) / AIRSPEED_BUFFER, 0.0f, 1.0f)); + return sin_arg * sin_arg; +} // bearingFeasibility + +float NPFG::lateralAccelFF(const Vector2f &unit_path_tangent, const Vector2f &ground_vel, + const float wind_dot_upt, const float wind_cross_upt, const float airspeed, + const float wind_speed, const float signed_track_error, const float path_curvature) const +{ + // NOTE: all calculations within this function take place at the closet point + // on the path, as if the aircraft were already tracking the given path at + // this point with zero angular error. this allows us to evaluate curvature + // induced requirements for lateral acceleration incrementation and ramp them + // in with the track proximity and further consider the bearing feasibility + // in excess wind conditions (this is done external to this method). + + // path frame curvature is the instantaneous curvature at our current distance + // from the actual path (considering e.g. concentric circles emanating outward/inward) + const float path_frame_curvature = path_curvature / MAX(1.0f - path_curvature * signed_track_error, + fabsf(path_curvature) * MIN_RADIUS); + + // limit tangent ground speed to along track (forward moving) direction + const float tangent_ground_speed = MAX(ground_vel.dot(unit_path_tangent), 0.0f); + + const float path_frame_rate = path_frame_curvature * tangent_ground_speed; + + // speed ratio = projection of ground vel on track / projection of air velocity + // on track + const float speed_ratio = (1.0f + wind_dot_upt / MAX(projectAirspOnBearing(airspeed, wind_cross_upt), + NPFG_EPSILON)); + + // note the use of airspeed * speed_ratio as oppose to ground_speed^2 here -- + // the prior considers that we command lateral acceleration in the air mass + // relative frame while the latter does not + return airspeed * speed_ratio * path_frame_rate; +} // lateralAccelFF + +float NPFG::lateralAccel(const Vector2f &air_vel, const Vector2f &air_vel_ref, const float airspeed) const +{ + // lateral acceleration demand only from the heading error + + const float dot_air_vel_err = air_vel.dot(air_vel_ref); + const float cross_air_vel_err = air_vel % air_vel_ref; + + 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 / airspeed_ref_; + } +} // lateralAccel + +float NPFG::switchDistance(float wp_radius) const +{ + return MIN(wp_radius, track_error_bound_ * switch_distance_multiplier_); +} // switchDistance + +void NPFG::updateRollSetpoint() +{ + float roll_new = atanf(lateral_accel_ * 1.0f / GRAVITY_MSS); + roll_new = constrain_value(roll_new, -roll_lim_rad_, roll_lim_rad_); + + if (dt_ > 0.0f && roll_slew_rate_ > 0.0f) { + // slew rate limiting active + roll_new = constrain_value(roll_new, roll_setpoint_ - roll_slew_rate_ * dt_, roll_setpoint_ + roll_slew_rate_ * dt_); + } + + if (ISFINITE(roll_new)) { + roll_setpoint_ = roll_new; + } +} // updateRollSetpoint diff --git a/libraries/AP_NPFG/npfg.h b/libraries/AP_NPFG/npfg.h new file mode 100644 index 0000000000000..abd1d90a83d44 --- /dev/null +++ b/libraries/AP_NPFG/npfg.h @@ -0,0 +1,614 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 npfg.hpp + * Implementation of a lateral-directional nonlinear path following guidance + * law with excess wind handling. Commands lateral acceleration and airspeed. + * + * @author Thomas Stastny + * + * 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 NPFG_H_ +#define NPFG_H_ + +#include + +/* + * NPFG + * Lateral-directional nonlinear path following guidance logic with excess wind handling + */ +class NPFG +{ + +public: + /* + * Computes the lateral acceleration and airspeed references necessary to track + * a path in wind (including excess wind conditions). + * + * @param[in] curr_pos_local Current horizontal vehicle position in local coordinates [m] + * @param[in] ground_vel Vehicle ground velocity vector [m/s] + * @param[in] wind_vel Wind velocity vector [m/s] + * @param[in] unit_path_tangent Unit vector tangent to path at closest point + * in direction of path + * @param[in] position_on_path Horizontal point on the path to track described in local coordinates [m] + * @param[in] path_curvature Path curvature at closest point on track [m^-1] + */ + void guideToPath(const Vector2f &curr_pos_local, const Vector2f &ground_vel, + const Vector2f &wind_vel, + const Vector2f &unit_path_tangent, const Vector2f &position_on_path, + const float path_curvature); + + /* + * Set the nominal controller period [s]. + */ + void setPeriod(float period) { period_ = MAX(period, NPFG_EPSILON); } + + /* + * Set the nominal controller damping ratio. + */ + void setDamping(float damping) { damping_ = constrain_value(damping, NPFG_EPSILON, 1.0f); } + + /* + * Enable automatic lower bounding of the user set controller period. + */ + void enablePeriodLB(const bool en) { en_period_lb_ = en; } + + /* + * Enable automatic adaptation of the user set controller period for track keeping + * performance. + */ + void enablePeriodUB(const bool en) { en_period_ub_ = en; } + + /* + * Enable minimum forward ground speed maintenance logic. + */ + void enableMinGroundSpeed(const bool en) { en_min_ground_speed_ = en; } + + /* + * Enable track keeping logic in excess wind conditions. + */ + void enableTrackKeeping(const bool en) { en_track_keeping_ = en; } + + /* + * Enable wind excess regulation. Disabling this param disables all airspeed + * reference incrementaion (airspeed reference will always be nominal). + */ + void enableWindExcessRegulation(const bool en) { en_wind_excess_regulation_ = en; } + + /* + * Set the minimum allowed forward ground speed [m/s]. + */ + void setMinGroundSpeed(float min_gsp) { min_gsp_desired_ = MAX(min_gsp, 0.0f); } + + /* + * Set the maximum value of the minimum forward ground speed command for track + * keeping (occurs at the track error boundary) [m/s]. + */ + void setMaxTrackKeepingMinGroundSpeed(float min_gsp) { min_gsp_track_keeping_max_ = MAX(min_gsp, 0.0f); } + + /* + * Set the nominal airspeed reference [m/s]. + */ + void setAirspeedNom(float airsp) { airspeed_nom_ = MAX(airsp, 0.1f); } + + /* + * Set the maximum airspeed reference [m/s]. + */ + void setAirspeedMax(float airsp) { airspeed_max_ = MAX(airsp, 0.1f); } + + /* + * Set the autopilot roll response time constant [s]. + */ + void setRollTimeConst(float tc) { roll_time_const_ = tc; } + + /* + * Set the switch distance multiplier. + */ + void setSwitchDistanceMultiplier(float mult) { switch_distance_multiplier_ = MAX(mult, 0.1f); } + + /* + * Set the period safety factor. + */ + void setPeriodSafetyFactor(float sf) { period_safety_factor_ = MAX(sf, 1.0f); } + + /* + * @return Controller proportional gain [rad/s] + */ + float getPGain() const { return p_gain_; } + + /* + * @return Controller time constant [s] + */ + float getTimeConst() const { return time_const_; } + + /* + * @return Adapted controller period [s] + */ + float getAdaptedPeriod() const { return adapted_period_; } + + /* + * @return Track error boundary [m] + */ + float getTrackErrorBound() const { return track_error_bound_; } + + /* + * @return Signed track error [m] + */ + float getTrackError() const { return signed_track_error_; } + + /* + * @return Airspeed reference [m/s] + */ + float getAirspeedRef() const { return airspeed_ref_; } + + /* + * @return Heading angle reference [rad] + */ + float getHeadingRef() const { return atan2f(air_vel_ref_[1], air_vel_ref_[0]); } + + /* + * @return Bearing angle [rad] + */ + float getBearing() const { return atan2f(bearing_vec_[1], bearing_vec_[0]); } + + /* + * @return Lateral acceleration command [m/s^2] + */ + float getLateralAccel() const { return lateral_accel_; } + + /* + * @return Feed-forward lateral acceleration command increment for tracking + * path curvature [m/s^2] + */ + float getLateralAccelFF() const { return lateral_accel_ff_; } + + /* + * @return Bearing feasibility [0, 1] + */ + float getBearingFeas() const { return feas_; } + + /* + * @return On-track bearing feasibility [0, 1] + */ + float getOnTrackBearingFeas() const { return feas_on_track_; } + + /* + * @return Minimum forward ground speed reference [m/s] + */ + float getMinGroundSpeedRef() const { return min_ground_speed_ref_; } + + /* + * [Copied directly from ECL_L1_Pos_Controller] + * + * Set the maximum roll angle output in radians + */ + void setRollLimit(float roll_lim_rad) { roll_lim_rad_ = roll_lim_rad; } + + /* + * [Copied directly from ECL_L1_Pos_Controller] + * + * Set roll angle slew rate. Set to zero to deactivate. + */ + void setRollSlewRate(float roll_slew_rate) { roll_slew_rate_ = roll_slew_rate; } + + /* + * [Copied directly from ECL_L1_Pos_Controller] + * + * Set control loop dt. The value will be used to apply roll angle setpoint slew rate limiting. + */ + void setDt(const float dt) { dt_ = dt; } + + /* + * [Copied directly from ECL_L1_Pos_Controller] + * + * Get the switch distance + * + * This is the distance at which the system will switch to the next waypoint. + * This depends on the period and damping + * + * @param[in] wp_radius The switching radius the waypoint has set. + */ + float switchDistance(float wp_radius) const; + + /* + * [Copied directly from ECL_L1_Pos_Controller] + * + * Get roll angle setpoint for fixed wing. + * + * @return Roll angle (in NED frame) + */ + float getRollSetpoint() { return roll_setpoint_; } + +private: + + static const float NPFG_EPSILON; // small number *bigger than machine epsilon + static const float MIN_RADIUS; // minimum effective radius (avoid singularities) [m] + static const float NTE_FRACTION; // normalized track error fraction (must be > 0) + // ^determines at what fraction of the normalized track error the maximum track keeping forward ground speed demand is reached + static const float AIRSPEED_BUFFER; // airspeed buffer [m/s] (must be > 0) + // ^the size of the feasibility transition region at cross wind angle >= 90 deg. + // This must be non-zero to avoid jumpy airspeed incrementation while using wind + // excess handling logic. Similarly used as buffer region around zero airspeed. + + /* + * tuning + */ + + float period_{10.0f}; // nominal (desired) period -- user defined [s] + float damping_{0.7071f}; // nominal (desired) damping ratio -- user defined + float p_gain_{0.8885f}; // proportional gain (computed from period_ and damping_) [rad/s] + float time_const_{7.071f}; // time constant (computed from period_ and damping_) [s] + float adapted_period_{10.0f}; // auto-adapted period (if stability bounds enabled) [s] + + /* + * user defined guidance settings + */ + + // guidance options + bool en_period_lb_{true}; // enables automatic lower bound constraints on controller period + bool en_period_ub_{true}; // enables automatic upper bound constraints on controller period (remains disabled if lower bound is disabled) + bool en_min_ground_speed_{true}; // the airspeed reference is incremented to sustain a user defined minimum forward ground speed + bool en_track_keeping_{false}; // the airspeed reference is incremented to return to the track and sustain zero ground velocity until excess wind subsides + bool en_wind_excess_regulation_{true}; // the airspeed reference is incremented to regulate the excess wind, but not overcome it ... + // ^disabling this parameter disables all other excess wind handling options, using only the nominal airspeed for reference + + // guidance settings + float airspeed_nom_{15.0f}; // nominal (desired) airspeed reference (generally equivalent to cruise optimized airspeed) [m/s] + float airspeed_max_{20.0f}; // maximum airspeed reference - the maximum achievable/allowed airspeed reference [m/s] + float roll_time_const_{0.0f}; // autopilot roll response time constant [s] + float min_gsp_desired_{0.0f}; // user defined miminum desired forward ground speed [m/s] + float min_gsp_track_keeping_max_{5.0f}; // maximum, minimum forward ground speed demand from track keeping logic [m/s] + + // guidance parameters + float switch_distance_multiplier_{0.32f}; // a value multiplied by the track error boundary resulting in a lower switch distance + // ^as the bearing angle changes quadratically (instead of linearly as in L1), the time constant (automatically calculated for on track stability) proportional track error boundary typically over estimates the required switching distance + float period_safety_factor_{1.5f}; // multiplied by the minimum period for conservative lower bound + + /* + * internal guidance states + */ + + // speeds + float min_gsp_track_keeping_{0.0f}; // minimum forward ground speed demand from track keeping logic [m/s] + float min_ground_speed_ref_{0.0f}; // resultant minimum forward ground speed reference considering all active guidance logic [m/s] + + //bearing feasibility + float feas_{1.0f}; // continous representation of bearing feasibility in [0,1] (0=infeasible, 1=feasible) + float feas_on_track_{1.0f}; // continuous bearing feasibility "on track" + + // track proximity + float track_error_bound_{212.13f}; // the current ground speed dependent track error bound [m] + float track_proximity_{0.0f}; // value in [0,1] indicating proximity to track, 0 = at track error boundary or beyond, 1 = on track + + // path following states + float signed_track_error_{0.0f}; // signed track error [m] + Vector2f bearing_vec_{Vector2f{1.0f, 0.0f}}; // bearing unit vector + + /* + * guidance outputs + */ + + float airspeed_ref_{15.0f}; // airspeed reference [m/s] + Vector2f air_vel_ref_{Vector2f{15.0f, 0.0f}}; // air velocity reference vector [m/s] + float lateral_accel_{0.0f}; // lateral acceleration reference [m/s^2] + float lateral_accel_ff_{0.0f}; // lateral acceleration demand to maintain path curvature [m/s^2] + + /* + * ECL_L1_Pos_Controller functionality + */ + + float dt_{0}; // control loop time [s] + float roll_lim_rad_{radians(30.0f)}; // maximum roll angle [rad] + float roll_setpoint_{0.0f}; // current roll angle setpoint [rad] + float roll_slew_rate_{0.0f}; // roll angle setpoint slew rate limit [rad/s] + + /* + * Adapts the controller period considering user defined inputs, current flight + * condition, path properties, and stability bounds. + * + * @param[in] ground_speed Vehicle ground speed [m/s] + * @param[in] airspeed Vehicle airspeed [m/s] + * @param[in] wind_speed Wind speed [m/s] + * @param[in] track_error Track error (magnitude) [m] + * @param[in] path_curvature Path curvature at closest point on track [m^-1] + * @param[in] wind_vel Wind velocity vector in inertial frame [m/s] + * @param[in] unit_path_tangent Unit vector tangent to path at closest point + * in direction of path + * @param[in] feas_on_track Bearing feasibility on track at the closest point + * @return Adapted period [s] + */ + float adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed, + const float track_error, const float path_curvature, const Vector2f &wind_vel, + const Vector2f &unit_path_tangent, const float feas_on_track) const; + + /* + * Returns normalized (unitless) and constrained track error [0,1]. + * + * @param[in] track_error Track error (magnitude) [m] + * @param[in] track_error_bound Track error boundary [m] + * @return Normalized track error + */ + float normalizedTrackError(const float track_error, const float track_error_bound) const; + + /* + * Cacluates an approximation of the wind factor (see [TODO: include citation]). + * + * @param[in] airspeed Vehicle airspeed [m/s] + * @param[in] wind_speed Wind speed [m/s] + * @return Non-dimensional wind factor approximation + */ + float windFactor(const float airspeed, const float wind_speed) const; + + /* + * Calculates a theoretical upper bound on the user defined period to maintain + * track keeping stability. + * + * @param[in] air_turn_rate The turn rate required to track the current path + * curvature at the current airspeed, in a no-wind condition [rad/s] + * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation]) + * @return Period upper bound [s] + */ + float periodUpperBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const; + + /* + * Calculates a theoretical lower bound on the user defined period to avoid + * limit cycle oscillations considering an acceleration actuation delay (e.g. + * roll response delay). Note this lower bound defines *marginal stability, + * and a safety factor should be applied in addition to the returned value. + * + * @param[in] air_turn_rate The turn rate required to track the current path + * curvature at the current airspeed, in a no-wind condition [rad/s] + * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation]) + * @return Period lower bound [s] + */ + float periodLowerBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const; + + /* + * Computes a continous non-dimensional track proximity [0,1] - 0 when the + * vehicle is at the track error boundary, and 1 when on track. + * + * @param[in] look_ahead_ang The angle at which the bearing vector deviates + * from the unit track error vector [rad] + * @return Track proximity + */ + float trackProximity(const float look_ahead_ang) const; + + /* + * Calculates a ground speed modulated track error bound under which the + * look ahead angle is quadratically transitioned from alignment with the + * track error vector to that of the path tangent vector. + * + * @param[in] ground_speed Vehicle ground speed [m/s] + * @param[in] time_const Controller time constant [s] + * @return Track error boundary [m] + */ + float trackErrorBound(const float ground_speed, const float time_const) const; + + /* + * Calculates the required controller proportional gain to achieve the desired + * system period and damping ratio. NOTE: actual period and damping will vary + * when following paths with curvature in wind. + * + * @param[in] period Desired system period [s] + * @param[in] damping Desired system damping ratio + * @return Proportional gain [rad/s] + */ + float pGain(const float period, const float damping) const; + + /* + * Calculates the required controller time constant to achieve the desired + * system period and damping ratio. NOTE: actual period and damping will vary + * when following paths with curvature in wind. + * + * @param[in] period Desired system period [s] + * @param[in] damping Desired system damping ratio + * @return Time constant [s] + */ + float timeConst(const float period, const float damping) const; + + /* + * Cacluates the look ahead angle as a quadratic function of the normalized + * track error. + * + * @param[in] normalized_track_error Normalized track error (track error / track error boundary) + * @return Look ahead angle [rad] + */ + float lookAheadAngle(const float normalized_track_error) const; + + /* + * Calculates the bearing vector and track proximity transitioning variable + * from the look-ahead angle mapping. + * + * @param[in] unit_path_tangent Unit vector tangent to path at closest point + * in direction of path + * @param[in] look_ahead_ang The bearing vector lies at this angle from + * the path normal vector [rad] + * @param[in] signed_track_error Signed error to track at closest point (sign + * determined by path normal direction) [m] + * @return Unit bearing vector + */ + Vector2f bearingVec(const Vector2f &unit_path_tangent, const float look_ahead_ang, + const float signed_track_error) const; + + /* + * Calculates the minimum forward ground speed demand for minimum forward + * ground speed maintanence as well as track keeping logic. + * + * @param[in] normalized_track_error Normalized track error (track error / track error boundary) + * @param[in] feas Bearing feasibility + * @return Minimum forward ground speed demand [m/s] + */ + float minGroundSpeed(const float normalized_track_error, const float feas); + + /* + * Determines a reference air velocity *without curvature compensation, but + * including "optimal" airspeed reference compensation in excess wind conditions. + * Nominal and maximum airspeed member variables must be set before using this method. + * + * @param[in] wind_vel Wind velocity vector [m/s] + * @param[in] bearing_vec Bearing vector + * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] + * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s] + * @param[in] wind_speed Wind speed [m/s] + * @param[in] min_ground_speed Minimum commanded forward ground speed [m/s] + * @return Air velocity vector [m/s] + */ + Vector2f refAirVelocity(const Vector2f &wind_vel, const Vector2f &bearing_vec, + const float wind_cross_bearing, const float wind_dot_bearing, const float wind_speed, + const float min_ground_speed) const; + + /* + * Projection of the air velocity vector onto the bearing line considering + * a connected wind triangle. + * + * @param[in] airspeed Vehicle airspeed [m/s] + * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] + * @return Projection of air velocity vector on bearing vector [m/s] + */ + float projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const; + + /* + * Check for binary bearing feasibility. + * + * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] + * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s] + * @param[in] airspeed Vehicle airspeed [m/s] + * @param[in] wind_speed Wind speed [m/s] + * @return Binary bearing feasibility: 1 if feasible, 0 if infeasible + */ + int bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, + const float wind_speed) const; + + /* + * Air velocity solution for a given wind velocity and bearing vector assuming + * a "high speed" (not backwards) solution in the excess wind case. + * + * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] + * @param[in] airsp_dot_bearing 2D dot product of air velocity (solution) and bearing vector [m/s] + * @param[in] bearing_vec Bearing vector + * @return Air velocity vector [m/s] + */ + Vector2f solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing, + const Vector2f &bearing_vec) const; + + + /* + * Air velocity solution for an infeasible bearing. + * + * @param[in] wind_vel Wind velocity vector [m/s] + * @param[in] bearing_vec Bearing vector + * @param[in] wind_speed Wind speed [m/s] + * @param[in] airspeed Vehicle airspeed [m/s] + * @return Air velocity vector [m/s] + */ + Vector2f infeasibleAirVelRef(const Vector2f &wind_vel, const Vector2f &bearing_vec, + const float wind_speed, const float airspeed) const; + + + /* + * Cacluates a continuous representation of the bearing feasibility from [0,1]. + * 0 = infeasible, 1 = fully feasible, partial feasibility in between. + * + * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] + * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s] + * @param[in] airspeed Vehicle airspeed [m/s] + * @param[in] wind_speed Wind speed [m/s] + * @return bearing feasibility + */ + float bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, + const float wind_speed) const; + + /* + * Calculates an additional feed-forward lateral acceleration demand considering + * the path curvature. + * + * @param[in] unit_path_tangent Unit vector tangent to path at closest point + * in direction of path + * @param[in] ground_vel Vehicle ground velocity vector [m/s] + * @param[in] wind_vel Wind velocity vector [m/s] + * @param[in] airspeed Vehicle airspeed [m/s] + * @param[in] wind_speed Wind speed [m/s] + * @param[in] signed_track_error Signed error to track at closest point (sign + * determined by path normal direction) [m] + * @param[in] path_curvature Path curvature at closest point on track [m^-1] + * @return Feed-forward lateral acceleration command [m/s^2] + */ + float lateralAccelFF(const Vector2f &unit_path_tangent, const Vector2f &ground_vel, + const float wind_dot_upt, const float wind_cross_upt, const float airspeed, + const float wind_speed, const float signed_track_error, const float path_curvature) const; + + /* + * Calculates a lateral acceleration demand from the heading error. + * (does not consider path curvature) + * + * @param[in] air_vel Vechile air velocity vector [m/s] + * @param[in] air_vel_ref Reference air velocity vector [m/s] + * @param[in] airspeed Vehicle airspeed [m/s] + * @return Lateral acceleration demand [m/s^2] + */ + float lateralAccel(const Vector2f &air_vel, const Vector2f &air_vel_ref, + const float airspeed) const; + + /******************************************************************************* + * PX4 POSITION SETPOINT INTERFACE FUNCTIONS + */ + + /** + * [Copied directly from ECL_L1_Pos_Controller] + * + * Update roll angle setpoint. This will also apply slew rate limits if set. + */ + void updateRollSetpoint(); + +}; // class NPFG + +#endif // NPFG_H_ From 946db974ac52343b7c2a6fe6023ce9d0316ff3cb Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Sat, 17 Feb 2024 23:20:48 +0000 Subject: [PATCH 07/19] Plane: add terrain navigation mode - Add NPFG parameters - Add NPFG controller - Partially implement terrain navigation mode - Update set_position_target_global_int to use terrain navigation Signed-off-by: Rhys Mainwaring --- ArduPlane/GCS_Mavlink.cpp | 40 +++++++++------- ArduPlane/Parameters.cpp | 4 ++ ArduPlane/Parameters.h | 3 ++ ArduPlane/Plane.h | 2 + ArduPlane/mode.h | 8 ++++ ArduPlane/mode_terrain_navigation.cpp | 68 ++++++++++++++++++++++++--- 6 files changed, 103 insertions(+), 22 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 2797d6bb99cd6..2c24d232948f8 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1351,7 +1351,8 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess // in modes such as RTL, CIRCLE, etc. Specifying ONLY one mode // for companion computer control is more safe (provided // one uses the FENCE_ACTION = 4 (RTL) for geofence failures). - if (plane.control_mode != &plane.mode_guided) { + if (!(plane.control_mode == &plane.mode_guided || + plane.control_mode == &plane.mode_terrain_navigation)) { //don't screw up failsafes return; } @@ -1394,10 +1395,13 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess } } // end if alt_mask + // terrain_navigation //! @todo(srmainwaring) handle mode general control... //! check the ignore flags //! check for valid inputs if (pos_target.type_mask == 0) { + // switch mode to TERRAIN_NAVIGATION + plane.set_mode(plane.mode_terrain_navigation, ModeReason::GCS_COMMAND); cmd.content.location.lat = pos_target.lat_int; cmd.content.location.lng = pos_target.lon_int; @@ -1456,21 +1460,25 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess // circular motion: a = v^2 / r. Vector2f vel(vx, vy); Vector2f accel(ax, ay); - if (!vel.is_zero() && !accel.is_zero()) { - Vector2f vel_unit = vel.normalized(); - Vector2f accel_normal = accel - vel_unit * accel.dot(vel); - Vector2f accel_normal_unit = accel_normal.normalized(); - // % is cross product, -1 converts to body frame with z-dn - // See: ModeGuided::set_radius_and_direction - // plane.loiter.direction = direction_is_ccw ? -1 : 1; - float dir = -1.0 * (vel_unit % accel_normal_unit); - float radius = vel.length_squared() / accel_normal.length(); - bool dir_is_ccw = dir < 0.0; - plane.mode_guided.set_radius_and_direction(radius, dir_is_ccw); - - // //! @todo(srmainwaring) remove - used for debugging - // gcs().send_text(MAV_SEVERITY_DEBUG, - // "radius: %f, dir: %f", radius, dir); + if (!vel.is_zero()) { + Vector2f vel_unit = vel.normalized(); + plane.mode_terrain_navigation.set_path_tangent(vel_unit); + + if (!accel.is_zero()) { + Vector2f accel_normal = accel - vel_unit * accel.dot(vel); + Vector2f accel_normal_unit = accel_normal.normalized(); + // % is cross product, -1 converts to body frame with z-dn + // See: ModeGuided::set_radius_and_direction + // plane.loiter.direction = direction_is_ccw ? -1 : 1; + float dir = -1.0 * (vel_unit % accel_normal_unit); + float radius = vel.length_squared() / accel_normal.length(); + bool dir_is_ccw = dir < 0.0; + plane.mode_terrain_navigation.set_radius_and_direction(radius, dir_is_ccw); + + // //! @todo(srmainwaring) remove - used for debugging + // gcs().send_text(MAV_SEVERITY_DEBUG, + // "radius: %f, dir: %f", radius, dir); + } } } diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index ea467883d8c19..631e68f8917c9 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -900,6 +900,10 @@ const AP_Param::Info Plane::var_info[] = { // @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp GOBJECT(L1_controller, "NAVL1_", AP_L1_Control), + // @Group: NPFG_ + // @Path: ../libraries/AP_NPFG/AP_NPFG.cpp + GOBJECT(NPFG_controller, "NPFG_", AP_NPFG), + // @Group: TECS_ // @Path: ../libraries/AP_TECS/AP_TECS.cpp GOBJECT(TECS_controller, "TECS_", AP_TECS), diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 49f3822f83d90..f342ee7d6a4c5 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -356,6 +356,9 @@ class Parameters { k_param_acro_yaw_rate, k_param_takeoff_throttle_max_t, k_param_autotune_options, + + k_param_NPFG_controller, + }; AP_Int16 format_version; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 5adcf0c77f9a0..2bffd58a72b56 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -58,6 +58,7 @@ #include #include +#include #include // RC input mapping library #include @@ -209,6 +210,7 @@ class Plane : public AP_Vehicle { AP_TECS TECS_controller{ahrs, aparm, landing, MASK_LOG_TECS}; AP_L1_Control L1_controller{ahrs, &TECS_controller}; + AP_NPFG NPFG_controller{ahrs, &TECS_controller}; // Attitude to servo controllers AP_RollController rollController{aparm}; diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 76267ed8f7d93..6dbe102de3257 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -330,11 +330,19 @@ class ModeTerrainNavigation : public ModeGuided void set_radius_and_direction(const float radius, const bool direction_is_ccw); + void set_path_tangent(Vector2f unit_path_tangent); + void update_target_altitude() override; protected: bool _enter() override; + + void _exit() override; + +private: + float _radius_m; + Vector2f _unit_path_tangent; }; class ModeCircle: public Mode diff --git a/ArduPlane/mode_terrain_navigation.cpp b/ArduPlane/mode_terrain_navigation.cpp index f31bd1d3dae07..bed11fea04e1a 100644 --- a/ArduPlane/mode_terrain_navigation.cpp +++ b/ArduPlane/mode_terrain_navigation.cpp @@ -1,33 +1,89 @@ #include "mode.h" #include "Plane.h" +//! @note using ModeCruise as template + bool ModeTerrainNavigation::_enter() { - gcs().send_text(MAV_SEVERITY_DEBUG, "Entering ModeTerrainNavigation"); - return ModeGuided::_enter(); + gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::_enter"); + // return ModeGuided::_enter(); + + // switch to NPFG nav controller + plane.nav_controller = &plane.NPFG_controller; + + return true; +} + +void ModeTerrainNavigation::_exit() +{ + gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::_exit"); + + // restore default nav controller + plane.nav_controller = &plane.L1_controller; } void ModeTerrainNavigation::update() { - ModeGuided::update(); + gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::update"); + // ModeGuided::update(); + + plane.calc_nav_roll(); + + plane.calc_nav_pitch(); + + // Throttle output + + // TECS control + plane.calc_throttle(); } void ModeTerrainNavigation::navigate() { - ModeGuided::navigate(); + gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::navigate"); + // ModeGuided::navigate(); + + plane.NPFG_controller.update_path_tangent(_unit_path_tangent); + plane.nav_controller->update_loiter(plane.next_WP_loc, _radius_m, + plane.loiter.direction); } bool ModeTerrainNavigation::handle_guided_request(Location target_loc) { - return ModeGuided::handle_guided_request(target_loc); + gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::handle_guided_request"); + // return ModeGuided::handle_guided_request(target_loc); + + // add home alt if needed + if (target_loc.relative_alt) { + target_loc.alt += plane.home.alt; + target_loc.relative_alt = 0; + } + + // see: Plane::set_guided_WP + + // copy the current location into the OldWP slot + plane.prev_WP_loc = plane.current_loc; + + // Load the next_WP slot + plane.next_WP_loc = target_loc; + + return true; } void ModeTerrainNavigation::set_radius_and_direction(const float radius, const bool direction_is_ccw) { - ModeGuided::set_radius_and_direction(radius, direction_is_ccw); + // gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::set_radius_and_direction"); + // ModeGuided::set_radius_and_direction(radius, direction_is_ccw); + + _radius_m = radius; + plane.loiter.direction = direction_is_ccw ? -1 : 1; +} + +void ModeTerrainNavigation::set_path_tangent(Vector2f unit_path_tangent) { + _unit_path_tangent = unit_path_tangent; } void ModeTerrainNavigation::update_target_altitude() { + // gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::update_target_altitude"); ModeGuided::update_target_altitude(); } From ce81b91a1c28d006a7ee6131bb9cb9eeecfb20af Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 19 Feb 2024 07:54:47 +0000 Subject: [PATCH 08/19] AP_NPFG: add method to set path tangent - Make getRollSetpoint const - Add method to set path tangent and implement some getters Signed-off-by: Rhys Mainwaring --- libraries/AP_NPFG/AP_NPFG.cpp | 112 +++++++++++++++++++++++++++------- libraries/AP_NPFG/AP_NPFG.h | 10 +++ libraries/AP_NPFG/npfg.h | 2 +- 3 files changed, 100 insertions(+), 24 deletions(-) diff --git a/libraries/AP_NPFG/AP_NPFG.cpp b/libraries/AP_NPFG/AP_NPFG.cpp index 8fcaae95256b8..7164e243353d4 100644 --- a/libraries/AP_NPFG/AP_NPFG.cpp +++ b/libraries/AP_NPFG/AP_NPFG.cpp @@ -73,6 +73,24 @@ const AP_Param::GroupInfo AP_NPFG::var_info[] = { // @User: Advanced AP_GROUPINFO("PERIOD_SF", 10, AP_NPFG, _npfg_period_safety_factor, 1.5f), + // // @Param: FW_GND_SPD_MIN + // // @DisplayName: NPFG forward ground speed minimum (m/s). + // // @Description: NPFG forward ground speed minimum (m/s). + // // @User: Advanced + // AP_GROUPINFO("FW_GND_SPD_MIN", 11, AP_NPFG, _param_fw_gnd_spd_min, 15.0f), + + // // @Param: FW_R_LIM + // // @DisplayName: NPFG forward roll limit (deg). + // // @Description: NPFG forward roll limit (deg). + // // @User: Advanced + // AP_GROUPINFO("FW_R_LIM", 12, AP_NPFG, _param_fw_r_lim, 30.0f), + + // // @Param: FW_PN_R_SLEW_MAX + // // @DisplayName: NPFG forward roll slew limit (deg/s). + // // @Description: NPFG forward roll slew limit (deg/s). + // // @User: Advanced + // AP_GROUPINFO("FW_PN_R_SLEW_MAX", 13, AP_NPFG, _param_fw_pn_r_slew_max, 0.0f), + AP_GROUPEND }; @@ -84,18 +102,23 @@ AP_NPFG::AP_NPFG(AP_AHRS &ahrs, const AP_TECS *tecs) } int32_t AP_NPFG::nav_roll_cd(void) const { - return 0; + float roll_setpoint_rad = _npfg.getRollSetpoint(); + int32_t roll_setpoint_cd = int32_t(100 * degrees(roll_setpoint_rad)); + return roll_setpoint_cd; } float AP_NPFG::lateral_acceleration(void) const { - return 0.0; + return _npfg.getLateralAccel(); } int32_t AP_NPFG::nav_bearing_cd(void) const { - return 0; + float bearing_rad = _npfg.getBearing(); + int32_t bearing_cd = int32_t(100 * degrees(bearing_rad)); + return bearing_cd; } int32_t AP_NPFG::bearing_error_cd(void) const { + // not implemented return 0; } @@ -104,73 +127,104 @@ int32_t AP_NPFG::target_bearing_cd(void) const { } float AP_NPFG::crosstrack_error(void) const { - return 0.0; + return _npfg.getTrackError(); } float AP_NPFG::crosstrack_error_integrator(void) const { + // not implemented return 0.0; } float AP_NPFG::turn_distance(float wp_radius) const { + // not implemented return 0.0; } float AP_NPFG::turn_distance(float wp_radius, float turn_angle) const { + // not implemented return 0.0; } float AP_NPFG::loiter_radius(const float radius) const { + // not implemented return 0.0; } void AP_NPFG::update_waypoint(const class Location &prev_WP, const class Location &next_WP, float dist_min) { + // not implemented } void AP_NPFG::update_loiter(const class Location ¢er_WP, float radius, int8_t loiter_direction) { // check for parameter updates update_parameters(); - //! @todo - obtain current position from _ahrs in local cartesian coords - Vector2f curr_pos_local; - - //! @todo - obtain ground vel from _ahrs in local cartesian coords - Vector2f ground_vel; - - //! @todo - obtain wind vel from _ahrs in local cartesian coords - Vector2f wind_vel; - - //! @todo - unit path tangent is the normalised setpoint velocity - Vector2f unit_path_tangent; - - //! @todo - the setpoint position in local cartesian coords - Vector2f position_on_path; + // get current position and velocity + Location current_loc; + if (_ahrs.get_location(current_loc) == false) { + //! @todo if no GPS loc available, maintain last nav/target_bearing + // _data_is_stale = true; + // return; + } + + //! @todo - obtain current position from _ahrs in local cartesian coords (m) + Vector2f curr_pos_local_cm; + if (current_loc.get_vector_xy_from_origin_NE(curr_pos_local_cm) == false) { + //! @todo add error handling + } + Vector2f curr_pos_local(0.01 * curr_pos_local_cm.x, 0.01 * curr_pos_local_cm.y); + + //! @todo - obtain ground vel from _ahrs in local cartesian coords (m/s) + Vector2f ground_vel = _ahrs.groundspeed_vector(); + + //! @todo - obtain wind vel from _ahrs in local cartesian coords (m/s) + Vector3f wind_estimate = _ahrs.wind_estimate(); + Vector2f wind_vel(wind_estimate.x, wind_estimate.y); + + //! @todo - unit path tangent is the normalised setpoint velocity (m/s) + // Vector2f unit_path_tangent(0.0f, 0.0f); + + //! @todo - the setpoint position in local cartesian coords (m) + Vector2f position_on_path_cm; + if (center_WP.get_vector_xy_from_origin_NE(position_on_path_cm) == false) { + //! @todo add error handling + } + Vector2f position_on_path(0.01 * position_on_path_cm.x, 0.01 * position_on_path_cm.y); //! @todo calculate from loiter radius and direction //! @todo check loiter radius is not zero or infinite - float path_curvature = -1.0 * float(loiter_direction) / radius; + float path_curvature = 0.0; + if (is_positive(radius)) { + path_curvature = -1.0 * float(loiter_direction) / radius; + } _npfg.guideToPath(curr_pos_local, ground_vel, wind_vel, - unit_path_tangent, position_on_path, path_curvature); + _unit_path_tangent, position_on_path, path_curvature); } void AP_NPFG::update_heading_hold(int32_t navigation_heading_cd) { + // not implemented } void AP_NPFG::update_level_flight(void) { + // not implemented } bool AP_NPFG::reached_loiter_target(void) { + // not implemented return false; } void AP_NPFG::set_data_is_stale(void) { + // not implemented } bool AP_NPFG::data_is_stale(void) const { + // not implemented return false; } void AP_NPFG::set_reverse(bool reverse) { + // not implemented } void AP_NPFG::update_parameters() { @@ -183,12 +237,24 @@ void AP_NPFG::update_parameters() { _npfg.enablePeriodUB(_npfg_en_period_ub); _npfg.enableMinGroundSpeed(_npfg_en_min_gsp); _npfg.enableTrackKeeping(_npfg_en_track_keeping); - _npfg.enableWindExcessRegulation(_npfg_en_wind_reg); - // _npfg.setMinGroundSpeed(_param_fw_gnd_spd_min); + // _npfg.enableWindExcessRegulation(_npfg_en_wind_reg); + _npfg.enableWindExcessRegulation(false); _npfg.setMaxTrackKeepingMinGroundSpeed(_npfg_track_keeping_gsp_max); _npfg.setRollTimeConst(_npfg_roll_time_const); _npfg.setSwitchDistanceMultiplier(_npfg_switch_distance_multiplier); + _npfg.setPeriodSafetyFactor(_npfg_period_safety_factor); + + // _npfg.setMinGroundSpeed(_param_fw_gnd_spd_min); // _npfg.setRollLimit(radians(_param_fw_r_lim)); // _npfg.setRollSlewRate(radians(_param_fw_pn_r_slew_max)); - _npfg.setPeriodSafetyFactor(_npfg_period_safety_factor); + _npfg.setMinGroundSpeed(13.0f); + _npfg.setRollLimit(radians(30.0f)); + _npfg.setRollSlewRate(radians(0.0f)); + + _npfg.setAirspeedNom(25.0f); + _npfg.setAirspeedMax(35.0f); +} + +void AP_NPFG::set_path_tangent(Vector2f unit_path_tangent) { + _unit_path_tangent = unit_path_tangent; } diff --git a/libraries/AP_NPFG/AP_NPFG.h b/libraries/AP_NPFG/AP_NPFG.h index 2eb5d61c4bf58..c7ccb76d53637 100644 --- a/libraries/AP_NPFG/AP_NPFG.h +++ b/libraries/AP_NPFG/AP_NPFG.h @@ -125,6 +125,10 @@ class AP_NPFG : public AP_Navigation { // store the NPFG_* user settable parameters static const struct AP_Param::GroupInfo var_info[]; + + //! @todo replace by updating an existing interface + void set_path_tangent(Vector2f unit_path_tangent); + private: void update_parameters(); @@ -149,4 +153,10 @@ class AP_NPFG : public AP_Navigation { AP_Float _npfg_roll_time_const; AP_Float _npfg_switch_distance_multiplier; AP_Float _npfg_period_safety_factor; + + // AP_Float _param_fw_gnd_spd_min; + // AP_Float _param_fw_r_lim; + // AP_Float _param_fw_pn_r_slew_max; + + Vector2f _unit_path_tangent; }; diff --git a/libraries/AP_NPFG/npfg.h b/libraries/AP_NPFG/npfg.h index abd1d90a83d44..3296545398e9b 100644 --- a/libraries/AP_NPFG/npfg.h +++ b/libraries/AP_NPFG/npfg.h @@ -266,7 +266,7 @@ class NPFG * * @return Roll angle (in NED frame) */ - float getRollSetpoint() { return roll_setpoint_; } + float getRollSetpoint() const { return roll_setpoint_; } private: From b4f0c177cdc11ed5a88571315b0868f415f74a84 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 19 Feb 2024 07:57:07 +0000 Subject: [PATCH 09/19] Plane: set desired path tangent in terrain navigation mode Signed-off-by: Rhys Mainwaring --- ArduPlane/mode_terrain_navigation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/mode_terrain_navigation.cpp b/ArduPlane/mode_terrain_navigation.cpp index bed11fea04e1a..5faab205b6d77 100644 --- a/ArduPlane/mode_terrain_navigation.cpp +++ b/ArduPlane/mode_terrain_navigation.cpp @@ -42,7 +42,7 @@ void ModeTerrainNavigation::navigate() gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::navigate"); // ModeGuided::navigate(); - plane.NPFG_controller.update_path_tangent(_unit_path_tangent); + plane.NPFG_controller.set_path_tangent(_unit_path_tangent); plane.nav_controller->update_loiter(plane.next_WP_loc, _radius_m, plane.loiter.direction); } From d89f59eb1613ad5a08834c8cd8e25976f1e0100e Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 19 Feb 2024 16:40:45 +0000 Subject: [PATCH 10/19] AP_NPFG: move airspeed updates to update_loiter Signed-off-by: Rhys Mainwaring --- libraries/AP_NPFG/AP_NPFG.cpp | 22 ++++++++++------------ 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/libraries/AP_NPFG/AP_NPFG.cpp b/libraries/AP_NPFG/AP_NPFG.cpp index 7164e243353d4..707a19db14a41 100644 --- a/libraries/AP_NPFG/AP_NPFG.cpp +++ b/libraries/AP_NPFG/AP_NPFG.cpp @@ -158,6 +158,12 @@ void AP_NPFG::update_loiter(const class Location ¢er_WP, float radius, int8_ // check for parameter updates update_parameters(); + + //! @todo remove hardcoding + _npfg.setAirspeedNom(15.0f); + _npfg.setAirspeedMax(20.0f); + + // get current position and velocity Location current_loc; if (_ahrs.get_location(current_loc) == false) { @@ -237,22 +243,14 @@ void AP_NPFG::update_parameters() { _npfg.enablePeriodUB(_npfg_en_period_ub); _npfg.enableMinGroundSpeed(_npfg_en_min_gsp); _npfg.enableTrackKeeping(_npfg_en_track_keeping); - // _npfg.enableWindExcessRegulation(_npfg_en_wind_reg); - _npfg.enableWindExcessRegulation(false); + _npfg.enableWindExcessRegulation(false /*_npfg_en_wind_reg*/); + _npfg.setMinGroundSpeed(0.0 /*_param_fw_gnd_spd_min*/); _npfg.setMaxTrackKeepingMinGroundSpeed(_npfg_track_keeping_gsp_max); _npfg.setRollTimeConst(_npfg_roll_time_const); _npfg.setSwitchDistanceMultiplier(_npfg_switch_distance_multiplier); + _npfg.setRollLimit(radians(30.0 /*_param_fw_r_lim*/)); + _npfg.setRollSlewRate(radians(0.0 /*_param_fw_pn_r_slew_max*/)); _npfg.setPeriodSafetyFactor(_npfg_period_safety_factor); - - // _npfg.setMinGroundSpeed(_param_fw_gnd_spd_min); - // _npfg.setRollLimit(radians(_param_fw_r_lim)); - // _npfg.setRollSlewRate(radians(_param_fw_pn_r_slew_max)); - _npfg.setMinGroundSpeed(13.0f); - _npfg.setRollLimit(radians(30.0f)); - _npfg.setRollSlewRate(radians(0.0f)); - - _npfg.setAirspeedNom(25.0f); - _npfg.setAirspeedMax(35.0f); } void AP_NPFG::set_path_tangent(Vector2f unit_path_tangent) { From 88405cc19a78d2fa58d4eef04b2d916502b24ac0 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 19 Feb 2024 16:42:49 +0000 Subject: [PATCH 11/19] Plane: set radius and path tangent before handle guided request Signed-off-by: Rhys Mainwaring --- ArduPlane/GCS_Mavlink.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 2c24d232948f8..86122bdedf500 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1442,8 +1442,6 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess } } - handle_guided_request(cmd); - { //! @todo(srmainwaring) add checks that velocity and accel //! are present and valid. @@ -1476,12 +1474,13 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess plane.mode_terrain_navigation.set_radius_and_direction(radius, dir_is_ccw); // //! @todo(srmainwaring) remove - used for debugging - // gcs().send_text(MAV_SEVERITY_DEBUG, - // "radius: %f, dir: %f", radius, dir); + // gcs().send_text(MAV_SEVERITY_DEBUG, "radius: %f, dir: %f", radius, dir); } } } + handle_guided_request(cmd); + // update adjust_altitude_target immediately rather than wait // for the scheduler. See also: Plane::set_next_WP plane.adjust_altitude_target(); From 01db379a30eff2b0b3132d34dd4cfc0467cbc273 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 22 Feb 2024 16:29:05 +0000 Subject: [PATCH 12/19] AP_NPFG: set control interval and enable roll slew rate - Enable roll slew rate - Disable gain scaling to avoid div zero error - Correct control interval calc - Set default track keeping to enable - Update slew rate to 90 deg / s Signed-off-by: Rhys Mainwaring --- libraries/AP_NPFG/AP_NPFG.cpp | 14 +++++++++++--- libraries/AP_NPFG/AP_NPFG.h | 1 + libraries/AP_NPFG/npfg.cpp | 4 +++- 3 files changed, 15 insertions(+), 4 deletions(-) diff --git a/libraries/AP_NPFG/AP_NPFG.cpp b/libraries/AP_NPFG/AP_NPFG.cpp index 707a19db14a41..e80a7f5f9e480 100644 --- a/libraries/AP_NPFG/AP_NPFG.cpp +++ b/libraries/AP_NPFG/AP_NPFG.cpp @@ -35,7 +35,7 @@ const AP_Param::GroupInfo AP_NPFG::var_info[] = { // @DisplayName: NPFG enable track keeping. // @Description: The airspeed reference is incremented to return to the track and sustain zero ground velocity until excess wind subsides. // @User: Advanced - AP_GROUPINFO("TRACK_KEEP", 4, AP_NPFG, _npfg_en_track_keeping, 0), + AP_GROUPINFO("TRACK_KEEP", 4, AP_NPFG, _npfg_en_track_keeping, 1), // @Param: EN_MIN_GSP // @DisplayName: NPFG enable min ground speed. @@ -158,12 +158,20 @@ void AP_NPFG::update_loiter(const class Location ¢er_WP, float radius, int8_ // check for parameter updates update_parameters(); + // calculate control interval + uint32_t now_ms = AP_HAL::millis(); + uint32_t control_interval_ms = now_ms - _last_update_ms; + _last_update_ms = now_ms; + + const float control_interval_s = float(control_interval_ms) * 1.0E-3; + + // update control time step for slew rates + _npfg.setDt(control_interval_s); //! @todo remove hardcoding _npfg.setAirspeedNom(15.0f); _npfg.setAirspeedMax(20.0f); - // get current position and velocity Location current_loc; if (_ahrs.get_location(current_loc) == false) { @@ -249,7 +257,7 @@ void AP_NPFG::update_parameters() { _npfg.setRollTimeConst(_npfg_roll_time_const); _npfg.setSwitchDistanceMultiplier(_npfg_switch_distance_multiplier); _npfg.setRollLimit(radians(30.0 /*_param_fw_r_lim*/)); - _npfg.setRollSlewRate(radians(0.0 /*_param_fw_pn_r_slew_max*/)); + _npfg.setRollSlewRate(radians(90.0 /*_param_fw_pn_r_slew_max*/)); _npfg.setPeriodSafetyFactor(_npfg_period_safety_factor); } diff --git a/libraries/AP_NPFG/AP_NPFG.h b/libraries/AP_NPFG/AP_NPFG.h index c7ccb76d53637..fe8f0b00627f3 100644 --- a/libraries/AP_NPFG/AP_NPFG.h +++ b/libraries/AP_NPFG/AP_NPFG.h @@ -158,5 +158,6 @@ class AP_NPFG : public AP_Navigation { // AP_Float _param_fw_r_lim; // AP_Float _param_fw_pn_r_slew_max; + uint32_t _last_update_ms; Vector2f _unit_path_tangent; }; diff --git a/libraries/AP_NPFG/npfg.cpp b/libraries/AP_NPFG/npfg.cpp index 7fa064e70f345..f9f4551f62f22 100644 --- a/libraries/AP_NPFG/npfg.cpp +++ b/libraries/AP_NPFG/npfg.cpp @@ -465,7 +465,9 @@ float NPFG::lateralAccel(const Vector2f &air_vel, const Vector2f &air_vel_ref, c } 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 / airspeed_ref_; + //! @todo(srmainwaring) - reenable gain scaling + // return p_gain_ * cross_air_vel_err / airspeed_ref_; + return p_gain_; } } // lateralAccel From 80956f6e716cfc0f2cd8ca2a9095170bb50c9c53 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 22 Feb 2024 22:49:15 +0000 Subject: [PATCH 13/19] Plane: adapt L1 controller for path following - Ensure radius is reset in terrain navigation mode - Co-opt L1 nav controller for path following in terrain navigation mode - Correct position of loiter centre - Update curvature calculation - Correct tangent vector Signed-off-by: Rhys Mainwaring --- ArduPlane/GCS_Mavlink.cpp | 23 +++++---- ArduPlane/mode.h | 1 + ArduPlane/mode_terrain_navigation.cpp | 74 ++++++++++++++++++--------- 3 files changed, 64 insertions(+), 34 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 86122bdedf500..c2fed50c19089 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1463,18 +1463,21 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess plane.mode_terrain_navigation.set_path_tangent(vel_unit); if (!accel.is_zero()) { - Vector2f accel_normal = accel - vel_unit * accel.dot(vel); - Vector2f accel_normal_unit = accel_normal.normalized(); - // % is cross product, -1 converts to body frame with z-dn - // See: ModeGuided::set_radius_and_direction - // plane.loiter.direction = direction_is_ccw ? -1 : 1; - float dir = -1.0 * (vel_unit % accel_normal_unit); - float radius = vel.length_squared() / accel_normal.length(); + //! @note this should be zero for terrain planning, + // as we expect the acceleration to be normal to the + // velocity vector (unit_tangent). + float accel_proj = accel.dot(vel_unit); + Vector2f accel_lat = accel - vel_unit * accel_proj; + Vector2f accel_lat_unit = accel_lat.normalized(); + // % is cross product, direction: cw:= 1, ccw:= -1 + float dir = accel_lat_unit % vel_unit; + float radius = vel.length_squared() / accel_lat.length(); + // float curvature = accel_lat.length(); + // float radius = 1.0 / curvature; bool dir_is_ccw = dir < 0.0; plane.mode_terrain_navigation.set_radius_and_direction(radius, dir_is_ccw); - - // //! @todo(srmainwaring) remove - used for debugging - // gcs().send_text(MAV_SEVERITY_DEBUG, "radius: %f, dir: %f", radius, dir); + } else { + plane.mode_terrain_navigation.set_radius_and_direction(0.0, true); } } } diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 6dbe102de3257..2e216ae8bf6e9 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -343,6 +343,7 @@ class ModeTerrainNavigation : public ModeGuided private: float _radius_m; Vector2f _unit_path_tangent; + enum { NAV_LOITER=0, NAV_WAYPOINT=1 } _nav_mode{NAV_LOITER}; }; class ModeCircle: public Mode diff --git a/ArduPlane/mode_terrain_navigation.cpp b/ArduPlane/mode_terrain_navigation.cpp index 5faab205b6d77..6b6458755a9b8 100644 --- a/ArduPlane/mode_terrain_navigation.cpp +++ b/ArduPlane/mode_terrain_navigation.cpp @@ -1,65 +1,94 @@ #include "mode.h" #include "Plane.h" -//! @note using ModeCruise as template +#define ENABLE_NPFG_CONTROLLER 0 bool ModeTerrainNavigation::_enter() { - gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::_enter"); - // return ModeGuided::_enter(); - +#if ENABLE_NPFG_CONTROLLER // switch to NPFG nav controller plane.nav_controller = &plane.NPFG_controller; +#else +#endif return true; } void ModeTerrainNavigation::_exit() { - gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::_exit"); - +#if ENABLE_NPFG_CONTROLLER // restore default nav controller plane.nav_controller = &plane.L1_controller; +#else +#endif } void ModeTerrainNavigation::update() { - gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::update"); - // ModeGuided::update(); - plane.calc_nav_roll(); plane.calc_nav_pitch(); - // Throttle output - - // TECS control plane.calc_throttle(); } void ModeTerrainNavigation::navigate() { - gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::navigate"); - // ModeGuided::navigate(); - +#if ENABLE_NPFG_CONTROLLER plane.NPFG_controller.set_path_tangent(_unit_path_tangent); - plane.nav_controller->update_loiter(plane.next_WP_loc, _radius_m, + plane.NPFG_controller.update_loiter(plane.next_WP_loc, _radius_m, plane.loiter.direction); +#else + if (_radius_m > 0.0) { + // moving along arc of circle - loiter about wp located at + // centre of curvature. + auto center_wp = plane.next_WP_loc; + Vector3p tangent_ned(_unit_path_tangent.x, _unit_path_tangent.y, 0.0); + Vector3p dn_ned(0.0, 0.0, 1.0); + auto ofs_ned = dn_ned.cross(tangent_ned) + * _radius_m * plane.loiter.direction; + center_wp.offset(ofs_ned); + + plane.nav_controller->update_loiter(center_wp, _radius_m, + plane.loiter.direction); + + // notify when we switch mode + if (_nav_mode == NAV_WAYPOINT) { + gcs().send_text(MAV_SEVERITY_INFO, + "NAV_LOITER: radius: %f, direction: %d", _radius_m, + plane.loiter.direction); + } + _nav_mode = NAV_LOITER; + } else { + // moving along a line segment - navigate to wp ahead of closest point + // in direction of path tangent + float ofs_m = 50.0; + Vector3p ofs_ned(ofs_m * _unit_path_tangent.x, + ofs_m * _unit_path_tangent.y, 0.0); + auto prev_wp = plane.next_WP_loc; + auto next_wp = plane.next_WP_loc; + next_wp.offset(ofs_ned); + + plane.nav_controller->update_waypoint(prev_wp, next_wp); + + // notify when we switch mode + if (_nav_mode == NAV_LOITER) { + gcs().send_text(MAV_SEVERITY_INFO, "NAV_WAYPOINT: offset: %f", ofs_m); + } + _nav_mode = NAV_WAYPOINT; + } + +#endif } bool ModeTerrainNavigation::handle_guided_request(Location target_loc) { - gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::handle_guided_request"); - // return ModeGuided::handle_guided_request(target_loc); - // add home alt if needed if (target_loc.relative_alt) { target_loc.alt += plane.home.alt; target_loc.relative_alt = 0; } - // see: Plane::set_guided_WP - // copy the current location into the OldWP slot plane.prev_WP_loc = plane.current_loc; @@ -71,9 +100,6 @@ bool ModeTerrainNavigation::handle_guided_request(Location target_loc) void ModeTerrainNavigation::set_radius_and_direction(const float radius, const bool direction_is_ccw) { - // gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::set_radius_and_direction"); - // ModeGuided::set_radius_and_direction(radius, direction_is_ccw); - _radius_m = radius; plane.loiter.direction = direction_is_ccw ? -1 : 1; } From 6bc89303c75e46664d51e8d9d0dda18295f285c8 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 26 Feb 2024 10:51:45 +0000 Subject: [PATCH 14/19] Plane: check SET_POSITION_TARGET_GLOBAL_INT bit mask for path following Signed-off-by: Rhys Mainwaring --- ArduPlane/GCS_Mavlink.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index c2fed50c19089..60ccad3d46a30 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1362,8 +1362,11 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess // Unexpectedly, the mask is expecting "ones" for dimensions that should // be IGNORNED rather than INCLUDED. See mavlink documentation of the // SET_POSITION_TARGET_GLOBAL_INT message, type_mask field. - const uint16_t alt_mask = 0b1111111111111011; // (z mask at bit 3) - + const uint16_t alt_mask = 0b1111111111111011; // (z mask at bit 3) + + // bit mask for path following: ignore force_set, yaw, yaw_rate + const uint16_t path_mask = 0b1111111000000000; + bool msg_valid = true; AP_Mission::Mission_Command cmd = {0}; @@ -1395,11 +1398,8 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess } } // end if alt_mask - // terrain_navigation - //! @todo(srmainwaring) handle mode general control... - //! check the ignore flags - //! check for valid inputs - if (pos_target.type_mask == 0) { + // terrain_navigation / path following + if (pos_target.type_mask & path_mask) { // switch mode to TERRAIN_NAVIGATION plane.set_mode(plane.mode_terrain_navigation, ModeReason::GCS_COMMAND); From 0e7f307330abf733e4618c7c1942770da012e1bf Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 26 Feb 2024 11:29:20 +0000 Subject: [PATCH 15/19] AP_Navigation: add method update_path Signed-off-by: Rhys Mainwaring --- libraries/AP_Navigation/AP_Navigation.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/libraries/AP_Navigation/AP_Navigation.h b/libraries/AP_Navigation/AP_Navigation.h index 26d25ad19a8c8..c12f4e4dbff43 100644 --- a/libraries/AP_Navigation/AP_Navigation.h +++ b/libraries/AP_Navigation/AP_Navigation.h @@ -95,6 +95,16 @@ class AP_Navigation { // attitude/steering. virtual void update_level_flight(void) = 0; + // update the internal state of the navigation controller when + // the vehicle has been commanded with a path following setpoint, which + // includes the closest point on the path, the unit tangent to the path, + // and the curvature. This is the step function for navigation control when + // path following. This function is called at regular intervals + // (typically 10Hz). The main flight code will call an output function + // (such as nav_roll_cd()) after this function to ask for the new required + // navigation attitude/steering. + virtual void update_path(const class Location &position_on_path, Vector2f unit_path_tangent, float path_curvature, int8_t direction) {} + // return true if we have reached the target loiter location. This // may be a fuzzy decision, depending on internal navigation // parameters. For example the controller may return true only From 348185a1fd1df42046e65fdfddde8a1385e40db6 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 26 Feb 2024 11:30:51 +0000 Subject: [PATCH 16/19] AP_L1_Control: add method update_path - Implement method update_path - Update comment Signed-off-by: Rhys Mainwaring --- libraries/AP_L1_Control/AP_L1_Control.cpp | 28 +++++++++++++++++++++++ libraries/AP_L1_Control/AP_L1_Control.h | 1 + 2 files changed, 29 insertions(+) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index 2750c232298d0..39128eac8b77a 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -515,3 +515,31 @@ void AP_L1_Control::update_level_flight(void) _data_is_stale = false; // status are correctly updated with current waypoint data } + +// update L1 control for path following +void AP_L1_Control::update_path(const class Location &position_on_path, Vector2f unit_path_tangent, float path_curvature, int8_t direction) { + //! @note initial implementation uses existing functions + if (!is_zero(path_curvature)) { + // moving along arc of circle - loiter about wp located at + // centre of curvature. + float radius_m = 1.0 / path_curvature; + auto center_wp = position_on_path; + Vector3p tangent_ned(unit_path_tangent.x, unit_path_tangent.y, 0.0); + Vector3p dn_ned(0.0, 0.0, 1.0); + auto ofs_ned = dn_ned.cross(tangent_ned) * radius_m * direction; + center_wp.offset(ofs_ned); + + update_loiter(center_wp, radius_m, direction); + } else { + // moving along a line segment - navigate to wp ahead of closest point + // in direction of path tangent + float ofs_m = 1000.0; + Vector3p ofs_ned(ofs_m * unit_path_tangent.x, + ofs_m * unit_path_tangent.y, 0.0); + auto prev_wp = position_on_path; + auto next_wp = position_on_path; + next_wp.offset(ofs_ned); + + update_waypoint(prev_wp, next_wp); + } +} diff --git a/libraries/AP_L1_Control/AP_L1_Control.h b/libraries/AP_L1_Control/AP_L1_Control.h index b775e5800a5a8..b47fc07b7a5cc 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.h +++ b/libraries/AP_L1_Control/AP_L1_Control.h @@ -54,6 +54,7 @@ class AP_L1_Control : public AP_Navigation { void update_loiter(const class Location ¢er_WP, float radius, int8_t loiter_direction) override; void update_heading_hold(int32_t navigation_heading_cd) override; void update_level_flight(void) override; + void update_path(const class Location &position_on_path, Vector2f unit_path_tangent, float path_curvature, int8_t direction) override; bool reached_loiter_target(void) override; // set the default NAVL1_PERIOD From 10fe4bfcef107f91bd8f72c4e3113e8d15e18545 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 26 Feb 2024 11:31:15 +0000 Subject: [PATCH 17/19] AP_NPFG: add method update_path Signed-off-by: Rhys Mainwaring --- libraries/AP_NPFG/AP_NPFG.cpp | 10 ++++++++++ libraries/AP_NPFG/AP_NPFG.h | 10 ++++++++++ 2 files changed, 20 insertions(+) diff --git a/libraries/AP_NPFG/AP_NPFG.cpp b/libraries/AP_NPFG/AP_NPFG.cpp index e80a7f5f9e480..96820e7fb7120 100644 --- a/libraries/AP_NPFG/AP_NPFG.cpp +++ b/libraries/AP_NPFG/AP_NPFG.cpp @@ -223,6 +223,16 @@ void AP_NPFG::update_level_flight(void) { // not implemented } +void AP_NPFG::update_path(const class Location &position_on_path, Vector2f unit_path_tangent, float path_curvature, int8_t direction) { + //! @note initial implementation uses existing functions + float radius_m = 0.0; + if (!is_zero(path_curvature)) { + radius_m = 1.0 / path_curvature; + } + set_path_tangent(_unit_path_tangent); + update_loiter(position_on_path, radius_m, direction); +} + bool AP_NPFG::reached_loiter_target(void) { // not implemented return false; diff --git a/libraries/AP_NPFG/AP_NPFG.h b/libraries/AP_NPFG/AP_NPFG.h index fe8f0b00627f3..00d948d2b24b6 100644 --- a/libraries/AP_NPFG/AP_NPFG.h +++ b/libraries/AP_NPFG/AP_NPFG.h @@ -101,6 +101,16 @@ class AP_NPFG : public AP_Navigation { // attitude/steering. void update_level_flight(void) override; + // update the internal state of the navigation controller when + // the vehicle has been commanded with a path following setpoint, which + // includes the closest point on the path, the unit tangent to the path, + // and the curvature. This is the step function for navigation control when + // path following. This function is called at regular intervals + // (typically 10Hz). The main flight code will call an output function + // (such as nav_roll_cd()) after this function to ask for the new required + // navigation attitude/steering. + void update_path(const class Location &position_on_path, Vector2f unit_path_tangent, float path_curvature, int8_t direction) override; + // return true if we have reached the target loiter location. This // may be a fuzzy decision, depending on internal navigation // parameters. For example the controller may return true only From 70ae8dabe44af2ccf700569295547e779dae8fd9 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 26 Feb 2024 12:16:39 +0000 Subject: [PATCH 18/19] Plane: terrain_navigation: use method nav_controller->update_path - Call ModeGuided _enter and _exit methods. - Use ModeGuided update method. - Store commanded curvature rather than radius. Signed-off-by: Rhys Mainwaring --- ArduPlane/mode.h | 3 +- ArduPlane/mode_terrain_navigation.cpp | 67 ++++----------------------- 2 files changed, 11 insertions(+), 59 deletions(-) diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 2e216ae8bf6e9..17edd6e452696 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -341,9 +341,8 @@ class ModeTerrainNavigation : public ModeGuided void _exit() override; private: - float _radius_m; + float _curvature; Vector2f _unit_path_tangent; - enum { NAV_LOITER=0, NAV_WAYPOINT=1 } _nav_mode{NAV_LOITER}; }; class ModeCircle: public Mode diff --git a/ArduPlane/mode_terrain_navigation.cpp b/ArduPlane/mode_terrain_navigation.cpp index 6b6458755a9b8..88085c840ade7 100644 --- a/ArduPlane/mode_terrain_navigation.cpp +++ b/ArduPlane/mode_terrain_navigation.cpp @@ -8,10 +8,8 @@ bool ModeTerrainNavigation::_enter() #if ENABLE_NPFG_CONTROLLER // switch to NPFG nav controller plane.nav_controller = &plane.NPFG_controller; -#else #endif - - return true; + return ModeGuided::_enter(); } void ModeTerrainNavigation::_exit() @@ -19,66 +17,19 @@ void ModeTerrainNavigation::_exit() #if ENABLE_NPFG_CONTROLLER // restore default nav controller plane.nav_controller = &plane.L1_controller; -#else #endif + ModeGuided::_exit(); } void ModeTerrainNavigation::update() { - plane.calc_nav_roll(); - - plane.calc_nav_pitch(); - - plane.calc_throttle(); + ModeGuided::update(); } void ModeTerrainNavigation::navigate() { -#if ENABLE_NPFG_CONTROLLER - plane.NPFG_controller.set_path_tangent(_unit_path_tangent); - plane.NPFG_controller.update_loiter(plane.next_WP_loc, _radius_m, - plane.loiter.direction); -#else - if (_radius_m > 0.0) { - // moving along arc of circle - loiter about wp located at - // centre of curvature. - auto center_wp = plane.next_WP_loc; - Vector3p tangent_ned(_unit_path_tangent.x, _unit_path_tangent.y, 0.0); - Vector3p dn_ned(0.0, 0.0, 1.0); - auto ofs_ned = dn_ned.cross(tangent_ned) - * _radius_m * plane.loiter.direction; - center_wp.offset(ofs_ned); - - plane.nav_controller->update_loiter(center_wp, _radius_m, - plane.loiter.direction); - - // notify when we switch mode - if (_nav_mode == NAV_WAYPOINT) { - gcs().send_text(MAV_SEVERITY_INFO, - "NAV_LOITER: radius: %f, direction: %d", _radius_m, - plane.loiter.direction); - } - _nav_mode = NAV_LOITER; - } else { - // moving along a line segment - navigate to wp ahead of closest point - // in direction of path tangent - float ofs_m = 50.0; - Vector3p ofs_ned(ofs_m * _unit_path_tangent.x, - ofs_m * _unit_path_tangent.y, 0.0); - auto prev_wp = plane.next_WP_loc; - auto next_wp = plane.next_WP_loc; - next_wp.offset(ofs_ned); - - plane.nav_controller->update_waypoint(prev_wp, next_wp); - - // notify when we switch mode - if (_nav_mode == NAV_LOITER) { - gcs().send_text(MAV_SEVERITY_INFO, "NAV_WAYPOINT: offset: %f", ofs_m); - } - _nav_mode = NAV_WAYPOINT; - } - -#endif + plane.nav_controller->update_path(plane.next_WP_loc, _unit_path_tangent, + _curvature, plane.loiter.direction); } bool ModeTerrainNavigation::handle_guided_request(Location target_loc) @@ -100,8 +51,11 @@ bool ModeTerrainNavigation::handle_guided_request(Location target_loc) void ModeTerrainNavigation::set_radius_and_direction(const float radius, const bool direction_is_ccw) { - _radius_m = radius; - plane.loiter.direction = direction_is_ccw ? -1 : 1; + _curvature = 0.0; + if (!is_zero(radius)) { + _curvature = 1.0 / radius; + } + ModeGuided::set_radius_and_direction(radius, direction_is_ccw); } void ModeTerrainNavigation::set_path_tangent(Vector2f unit_path_tangent) { @@ -110,6 +64,5 @@ void ModeTerrainNavigation::set_path_tangent(Vector2f unit_path_tangent) { void ModeTerrainNavigation::update_target_altitude() { - // gcs().send_text(MAV_SEVERITY_DEBUG, "ModeTerrainNavigation::update_target_altitude"); ModeGuided::update_target_altitude(); } From 427f6cd75e1825379ffa29f320d54f927af23738 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 26 Feb 2024 13:26:33 +0000 Subject: [PATCH 19/19] AP_NPFG: move update code from update_loiter to update_path Signed-off-by: Rhys Mainwaring --- libraries/AP_NPFG/AP_NPFG.cpp | 74 +++++++++++++---------------------- libraries/AP_NPFG/AP_NPFG.h | 5 --- 2 files changed, 27 insertions(+), 52 deletions(-) diff --git a/libraries/AP_NPFG/AP_NPFG.cpp b/libraries/AP_NPFG/AP_NPFG.cpp index 96820e7fb7120..beca2cabbe873 100644 --- a/libraries/AP_NPFG/AP_NPFG.cpp +++ b/libraries/AP_NPFG/AP_NPFG.cpp @@ -155,7 +155,18 @@ void AP_NPFG::update_waypoint(const class Location &prev_WP, const class Locatio } void AP_NPFG::update_loiter(const class Location ¢er_WP, float radius, int8_t loiter_direction) { - // check for parameter updates + // not implemented +} + +void AP_NPFG::update_heading_hold(int32_t navigation_heading_cd) { + // not implemented +} + +void AP_NPFG::update_level_flight(void) { + // not implemented +} + +void AP_NPFG::update_path(const class Location &position_on_path, Vector2f unit_path_tangent, float path_curvature, int8_t direction) { update_parameters(); // calculate control interval @@ -168,69 +179,42 @@ void AP_NPFG::update_loiter(const class Location ¢er_WP, float radius, int8_ // update control time step for slew rates _npfg.setDt(control_interval_s); - //! @todo remove hardcoding - _npfg.setAirspeedNom(15.0f); - _npfg.setAirspeedMax(20.0f); + //! @todo(srmainwaring) remove hardcoding + _npfg.setAirspeedNom(25.0f); + _npfg.setAirspeedMax(35.0f); // get current position and velocity Location current_loc; if (_ahrs.get_location(current_loc) == false) { - //! @todo if no GPS loc available, maintain last nav/target_bearing + //! @todo(srmainwaring) if no GPS loc available, maintain last nav/target_bearing // _data_is_stale = true; // return; } - //! @todo - obtain current position from _ahrs in local cartesian coords (m) + // current position from _ahrs in local cartesian coords (m) Vector2f curr_pos_local_cm; if (current_loc.get_vector_xy_from_origin_NE(curr_pos_local_cm) == false) { - //! @todo add error handling + //! @todo(srmainwaring) add error handling } Vector2f curr_pos_local(0.01 * curr_pos_local_cm.x, 0.01 * curr_pos_local_cm.y); - //! @todo - obtain ground vel from _ahrs in local cartesian coords (m/s) + // ground vel from _ahrs in local cartesian coords (m/s) Vector2f ground_vel = _ahrs.groundspeed_vector(); - //! @todo - obtain wind vel from _ahrs in local cartesian coords (m/s) + // wind estimate from _ahrs in local cartesian coords (m/s) Vector3f wind_estimate = _ahrs.wind_estimate(); Vector2f wind_vel(wind_estimate.x, wind_estimate.y); - //! @todo - unit path tangent is the normalised setpoint velocity (m/s) - // Vector2f unit_path_tangent(0.0f, 0.0f); - - //! @todo - the setpoint position in local cartesian coords (m) - Vector2f position_on_path_cm; - if (center_WP.get_vector_xy_from_origin_NE(position_on_path_cm) == false) { - //! @todo add error handling - } - Vector2f position_on_path(0.01 * position_on_path_cm.x, 0.01 * position_on_path_cm.y); - - //! @todo calculate from loiter radius and direction - //! @todo check loiter radius is not zero or infinite - float path_curvature = 0.0; - if (is_positive(radius)) { - path_curvature = -1.0 * float(loiter_direction) / radius; + // setpoint position in local cartesian coords (m) + Vector2f position_on_path_ned_cm; + if (position_on_path.get_vector_xy_from_origin_NE(position_on_path_ned_cm) == false) { + //! @todo(srmainwaring) add error handling } + Vector2f position_on_path_ned(0.01 * position_on_path_ned_cm.x, 0.01 * position_on_path_ned_cm.y); _npfg.guideToPath(curr_pos_local, ground_vel, wind_vel, - _unit_path_tangent, position_on_path, path_curvature); -} - -void AP_NPFG::update_heading_hold(int32_t navigation_heading_cd) { - // not implemented -} - -void AP_NPFG::update_level_flight(void) { - // not implemented -} - -void AP_NPFG::update_path(const class Location &position_on_path, Vector2f unit_path_tangent, float path_curvature, int8_t direction) { - //! @note initial implementation uses existing functions - float radius_m = 0.0; - if (!is_zero(path_curvature)) { - radius_m = 1.0 / path_curvature; - } - set_path_tangent(_unit_path_tangent); - update_loiter(position_on_path, radius_m, direction); + unit_path_tangent, position_on_path_ned, + path_curvature * float(direction)); } bool AP_NPFG::reached_loiter_target(void) { @@ -270,7 +254,3 @@ void AP_NPFG::update_parameters() { _npfg.setRollSlewRate(radians(90.0 /*_param_fw_pn_r_slew_max*/)); _npfg.setPeriodSafetyFactor(_npfg_period_safety_factor); } - -void AP_NPFG::set_path_tangent(Vector2f unit_path_tangent) { - _unit_path_tangent = unit_path_tangent; -} diff --git a/libraries/AP_NPFG/AP_NPFG.h b/libraries/AP_NPFG/AP_NPFG.h index 00d948d2b24b6..211377cfdee76 100644 --- a/libraries/AP_NPFG/AP_NPFG.h +++ b/libraries/AP_NPFG/AP_NPFG.h @@ -135,10 +135,6 @@ class AP_NPFG : public AP_Navigation { // store the NPFG_* user settable parameters static const struct AP_Param::GroupInfo var_info[]; - - //! @todo replace by updating an existing interface - void set_path_tangent(Vector2f unit_path_tangent); - private: void update_parameters(); @@ -169,5 +165,4 @@ class AP_NPFG : public AP_Navigation { // AP_Float _param_fw_pn_r_slew_max; uint32_t _last_update_ms; - Vector2f _unit_path_tangent; };