Plane: add guided path navigation

- Add guided path sub-mode - Extend handling of SET_POSITION_TARGET_GLOBAL_INT to include path guidance - Check for non-zero velocity and acceleration - Calculate path curvature and direction - Force update of adjust_altitude_target. Signed-off-by: Rhys Mainwaring <[email protected]>
ArduPilot · Feb 27, 2024 · bdad990 · bdad990
1 parent c68af3f
commit bdad990
Show file tree

Hide file tree

Showing 3 changed files with 119 additions and 4 deletions.
diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
@@ -1361,10 +1361,13 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess
         // 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)
-
+
+        // 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 +1394,66 @@ 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 & 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) {
+                Vector2f vel(pos_target.vx, pos_target.vy);
+                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;
+                    }
+                }
+
+                // only accept position updates when in GUIDED mode
+                if (!plane.control_mode->is_guided_mode()) {
+                    return;
+                }
+                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)

diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
@@ -296,17 +296,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(Location position_on_path, 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

diff --git a/ArduPlane/mode_guided.cpp b/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->update_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,33 @@ 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(Location position_on_path, Vector2f unit_path_tangent, const float path_curvature, const bool direction_is_ccw)
+{
+    // add home alt if needed
+    if (position_on_path.relative_alt) {
+        position_on_path.alt += plane.home.alt;
+        position_on_path.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 = position_on_path;
+
+    _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;
 }