Plane: handle guided path requests in GUIDED mode

ArduPlane/GCS_Mavlink.cpp

@@ -1372,15 +1372,28 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess
 
         mavlink_set_position_target_global_int_t pos_target;
         mavlink_msg_set_position_target_global_int_decode(&msg, &pos_target);
+
+        const uint16_t POSITION_TARGET_TYPEMASK_LAST_BYTE = 0xF000;
+
         // 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 = (
+            POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | POSITION_TARGET_TYPEMASK_Z_IGNORE |
+            POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE |
+            POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE |
+            POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE |
+            POSITION_TARGET_TYPEMASK_LAST_BYTE) ^ 0xFFFF;
+
+        // bit mask for path following
+        const uint16_t path_mask = (
+            POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE |
+            POSITION_TARGET_TYPEMASK_LAST_BYTE) ^ 0xFFFF;
+
         bool msg_valid = true;
         AP_Mission::Mission_Command cmd = {0};
-        
-        if (pos_target.type_mask & alt_mask)
+
+        if (((pos_target.type_mask | POSITION_TARGET_TYPEMASK_LAST_BYTE) ^ 0xFFFF) == alt_mask)
         {
             cmd.content.location.alt = pos_target.alt * 100;
             cmd.content.location.relative_alt = false;
@@ -1408,7 +1421,65 @@ 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
+
+        // guided path following
+        if (((pos_target.type_mask | POSITION_TARGET_TYPEMASK_LAST_BYTE) ^ 0xFFFF) == path_mask) {
+            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) {
+                return;
+            }
+
+            const Vector2f vel{pos_target.vx, pos_target.vy};
+            const Vector2f accel{pos_target.afx, pos_target.afy};
+            Vector2f unit_vel;
+
+            float path_curvature = 0.0;
+            bool dir_is_ccw = false;
+
+            if (!vel.is_zero()) {
+                unit_vel = vel.normalized();
+
+                if (!accel.is_zero()) {
+                    // curvature is determined from the acceleration normal
+                    // to the planar velocity and the equation for uniform
+                    // circular motion: a = v^2 / r.
+                    float accel_proj = accel.dot(unit_vel);
+                    Vector2f accel_lat = accel - unit_vel * accel_proj;
+                    Vector2f accel_lat_unit = accel_lat.normalized();
+
+                    path_curvature = accel_lat.length() / vel.length_squared();
+
+                    // % is cross product, direction: cw:= 1, ccw:= -1
+                    float dir = accel_lat_unit % unit_vel;
+                    dir_is_ccw = dir < 0.0;
+                }
+            }
+
+            // update path guidance
+            plane.mode_guided.handle_guided_path_request(
+                cmd.content.location, unit_vel, path_curvature, dir_is_ccw);
+
+            // update adjust_altitude_target immediately rather than wait
+            // for the scheduler.
+            plane.adjust_altitude_target();
+        }
     }
 
 MAV_RESULT GCS_MAVLINK_Plane::handle_command_do_set_mission_current(const mavlink_command_int_t &packet)

ArduPlane/mode.h

@@ -314,17 +314,32 @@ class ModeGuided : public Mode
     // handle a guided target request from GCS
     bool handle_guided_request(Location target_loc) override;
 
+    // handle a guided path following request
+    bool handle_guided_path_request(const Location& location_on_path, const Vector2f& unit_path_tangent, const float path_curvature, const bool direction_is_ccw);
+
     void set_radius_and_direction(const float radius, const bool direction_is_ccw);
 
     void update_target_altitude() override;
 
 protected:
 
+    enum class SubMode: uint8_t {
+        WP,
+        Path
+    };
+
     bool _enter() override;
     bool _pre_arm_checks(size_t buflen, char *buffer) const override { return true; }
 
 private:
+
+    SubMode _guided_mode;
+
     float active_radius_m;
+
+    // used in path mode
+    float _path_curvature;
+    Vector2f _unit_path_tangent;
 };
 
 class ModeCircle: public Mode

ArduPlane/mode_guided.cpp

@@ -103,7 +103,18 @@ void ModeGuided::update()
 
 void ModeGuided::navigate()
 {
-    plane.update_loiter(active_radius_m);
+    switch (_guided_mode) {
+    case SubMode::WP:
+        plane.update_loiter(active_radius_m);
+        break;
+    case SubMode::Path:
+        plane.nav_controller->follow_path(plane.next_WP_loc,
+            _unit_path_tangent, _path_curvature, plane.loiter.direction);
+        break;
+    default:
+        gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
+        break;
+    }
 }
 
 bool ModeGuided::handle_guided_request(Location target_loc)
@@ -116,6 +127,35 @@ bool ModeGuided::handle_guided_request(Location target_loc)
 
     plane.set_guided_WP(target_loc);
 
+    // use waypoint navigation sub-mode
+    _guided_mode = SubMode::WP;
+
+    return true;
+}
+
+bool ModeGuided::handle_guided_path_request(const Location& location_on_path, const Vector2f& unit_path_tangent, const float path_curvature, const bool direction_is_ccw)
+{
+    Location location_on_path_abs_alt = location_on_path;
+
+    // add home alt if needed
+    if (location_on_path_abs_alt.relative_alt) {
+        location_on_path_abs_alt.alt += plane.home.alt;
+        location_on_path_abs_alt.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 = location_on_path_abs_alt;
+
+    _unit_path_tangent = unit_path_tangent;
+    _path_curvature = path_curvature;
+    plane.loiter.direction = direction_is_ccw ? -1 : 1;
+
+    // use path navigation sub-mode
+    _guided_mode = SubMode::Path;
+
     return true;
 }
 

libraries/AP_L1_Control/AP_L1_Control.cpp

@@ -147,6 +147,10 @@ float AP_L1_Control::turn_distance(float wp_radius, float turn_angle) const
 
 float AP_L1_Control::loiter_radius(const float radius) const
 {
+    if (_disable_loiter_radius_scaling) {
+        return radius;
+    }
+
     // prevent an insane loiter bank limit
     float sanitized_bank_limit = constrain_float(_loiter_bank_limit, 0.0f, 89.0f);
     float lateral_accel_sea_level = tanf(radians(sanitized_bank_limit)) * GRAVITY_MSS;
@@ -545,3 +549,34 @@ 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::follow_path(const Location &location_on_path, const Vector2f& unit_path_tangent, float path_curvature, int8_t direction) {
+    //! @note initial implementation uses existing functions
+    if (is_positive(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 = location_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);
+
+        // disable loiter radius scaling while updating loiter
+        _disable_loiter_radius_scaling = true;
+        update_loiter(center_wp, radius_m, direction);
+        _disable_loiter_radius_scaling = false;
+    } else {
+        // moving along a line segment - navigate to wp ahead of closest point
+        // in direction of path tangent 
+        float ofs_m = 100.0;
+        Vector3p ofs_ned(ofs_m * unit_path_tangent.x,
+            ofs_m * unit_path_tangent.y, 0.0);
+        auto prev_wp = location_on_path;
+        auto next_wp = location_on_path;
+        next_wp.offset(ofs_ned);
+
+        update_waypoint(prev_wp, next_wp);
+    }
+}

libraries/AP_L1_Control/AP_L1_Control.h

@@ -53,6 +53,7 @@ class AP_L1_Control : public AP_Navigation {
     void update_loiter(const class Location &center_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 follow_path(const Location &location_on_path, const Vector2f& unit_path_tangent, float path_curvature, int8_t direction) override;
     bool reached_loiter_target(void) override;
 
     // set the default NAVL1_PERIOD
@@ -135,4 +136,7 @@ class AP_L1_Control : public AP_Navigation {
     bool _reverse = false;
     float get_yaw() const;
     int32_t get_yaw_sensor() const;
+
+    // allow loiter radius scaling to be disabled (in path guidance).
+    bool _disable_loiter_radius_scaling;
 };

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 follow_path(const Location &location_on_path, const 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