Plane: support off-board terrain navigation

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:

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:
@@ -1350,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;
         }
@@ -1360,11 +1362,14 @@ 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};
-        
+
         if (pos_target.type_mask & alt_mask)
         {
             cmd.content.location.alt = pos_target.alt * 100;
@@ -1391,7 +1396,99 @@ 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
+
+        // 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);
+
+            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);
+                    }
+                }
+
+                {
+                    //! @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()) {
+                        Vector2f vel_unit = vel.normalized();
+                        plane.mode_terrain_navigation.set_path_tangent(vel_unit);
+
+                        if (!accel.is_zero()) {
+                            //! @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);
+                        }  else {
+                            plane.mode_terrain_navigation.set_radius_and_direction(0.0, true);
+                        }
+                    }
+                }
+
+                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();
+            }
+        }
     }
 
 MAV_RESULT GCS_MAVLINK_Plane::handle_command_do_set_mission_current(const mavlink_command_int_t &packet)

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:

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),
 
@@ -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),

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;

ArduPlane/Plane.h

@@ -58,6 +58,7 @@
 
 #include <AP_Navigation/AP_Navigation.h>
 #include <AP_L1_Control/AP_L1_Control.h>
+#include <AP_NPFG/AP_NPFG.h>
 #include <AP_RCMapper/AP_RCMapper.h>        // RC input mapping library
 
 #include <AP_Vehicle/AP_Vehicle.h>
@@ -152,6 +153,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;
@@ -208,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};
@@ -273,6 +276,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;

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;

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

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;

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) {

ArduPlane/mode.cpp

@@ -12,6 +12,8 @@ Mode::Mode() :
 {
 }
 
+Mode::~Mode() = default;
+
 void Mode::exit()
 {
     // call sub-classes exit

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,39 @@ 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 set_path_tangent(Vector2f unit_path_tangent);
+
+    void update_target_altitude() override;
+
+protected:
+
+    bool _enter() override;
+
+    void _exit() override;
+
+private:
+    float _curvature;
+    Vector2f _unit_path_tangent;
+};
+
 class ModeCircle: public Mode
 {
 public:

ArduPlane/mode_terrain_navigation.cpp

@@ -0,0 +1,68 @@
+#include "mode.h"
+#include "Plane.h"
+
+#define ENABLE_NPFG_CONTROLLER 0
+
+bool ModeTerrainNavigation::_enter()
+{
+#if ENABLE_NPFG_CONTROLLER
+    // switch to NPFG nav controller
+    plane.nav_controller = &plane.NPFG_controller;
+#endif
+    return ModeGuided::_enter();
+}
+
+void ModeTerrainNavigation::_exit()
+{
+#if ENABLE_NPFG_CONTROLLER
+    // restore default nav controller
+    plane.nav_controller = &plane.L1_controller;
+#endif
+    ModeGuided::_exit();
+}
+
+void ModeTerrainNavigation::update()
+{
+    ModeGuided::update();
+}
+
+void ModeTerrainNavigation::navigate()
+{
+    plane.nav_controller->update_path(plane.next_WP_loc, _unit_path_tangent,
+        _curvature, plane.loiter.direction);
+}
+
+bool ModeTerrainNavigation::handle_guided_request(Location target_loc)
+{
+    // add home alt if needed
+    if (target_loc.relative_alt) {
+        target_loc.alt += plane.home.alt;
+        target_loc.relative_alt = 0;
+    }
+
+    // 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)
+{
+    _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) {
+    _unit_path_tangent = unit_path_tangent;
+}
+
+void ModeTerrainNavigation::update_target_altitude()
+{
+    ModeGuided::update_target_altitude();
+}