DDS: add guided path external control and implement for plane

ArduPlane/AP_ExternalControl_Plane.cpp

@@ -19,4 +19,24 @@ bool AP_ExternalControl_Plane::set_global_position(const Location& loc)
     return plane.set_target_location(loc);
 }
 
+bool AP_ExternalControl_Plane::set_path_position_tangent_and_curvature(const Location &position_on_path,
+    const Vector2f unit_path_tangent, const float path_curvature, const bool direction_is_ccw)
+{
+    //! @todo(srmainwaring) consolidate duplicate code in GCS_MAVLINK_Plane::handle_set_position_target_global_int.
+
+    // only accept position updates when in GUIDED mode.
+    if (!plane.control_mode->is_guided_mode()) {
+        return false;
+    }
+
+    // update guided path 
+    plane.mode_guided.handle_guided_path_request(
+        position_on_path, unit_path_tangent, path_curvature, direction_is_ccw);
+
+    // update adjust_altitude_target immediately rather than wait for the scheduler.
+    plane.adjust_altitude_target();
+
+    return true;
+}
+
 #endif // AP_EXTERNAL_CONTROL_ENABLED

ArduPlane/AP_ExternalControl_Plane.h

@@ -16,6 +16,13 @@ class AP_ExternalControl_Plane : public AP_ExternalControl {
         Sets the target global position for a loiter point.
     */
     bool set_global_position(const Location& loc) override WARN_IF_UNUSED;
+
+    /*
+        Set the target setpoint for path guidance: the closest position on the
+        path, the unit tangent to the path, and path curvature and direction.
+    */
+    bool set_path_position_tangent_and_curvature(const Location &position_on_path,
+        Vector2f unit_path_tangent, const float path_curvature, const bool direction_is_ccw) override WARN_IF_UNUSED;
 };
 
 #endif // AP_EXTERNAL_CONTROL_ENABLED

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,63 @@ 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) {
+                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;
+                    }
+                }
+
+                // 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(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

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;
 }
 

libraries/AP_DDS/AP_DDS_ExternalControl.cpp

@@ -9,11 +9,24 @@
 #include <AP_ExternalControl/AP_ExternalControl.h>
 
 // These are the Goal Interface constants. Because microxrceddsgen does not expose
-// them in the generated code, they are manually maintained
-// Position ignore flags
-static constexpr uint16_t TYPE_MASK_IGNORE_LATITUDE = 1;
-static constexpr uint16_t TYPE_MASK_IGNORE_LONGITUDE = 2;
-static constexpr uint16_t TYPE_MASK_IGNORE_ALTITUDE = 4;
+// them in the generated code, they are manually maintained.
+
+//! @todo(srmainwaring) make an enum class and use templates to enable as bitfield
+// see: https://accu.org/journals/overload/24/132/williams_2228/
+typedef enum AP_DDS_PositionTargetTypeMask {
+    TYPE_MASK_IGNORE_LATITUDE = 1,
+    TYPE_MASK_IGNORE_LONGITUDE = 2,
+    TYPE_MASK_IGNORE_ALTITUDE = 4,
+    TYPE_MASK_IGNORE_VX = 8,
+    TYPE_MASK_IGNORE_VY = 16,
+    TYPE_MASK_IGNORE_VZ = 32,
+    TYPE_MASK_IGNORE_AFX = 64,
+    TYPE_MASK_IGNORE_AFY = 128,
+    TYPE_MASK_IGNORE_AFZ = 256,
+    TYPE_MASK_FORCE_SET = 512,
+    TYPE_MASK_YAW_IGNORE = 1024,
+    TYPE_MASK_YAW_RATE_IGNORE = 2048,
+} AP_DDS_PositionTargetTypeMask;
 
 bool AP_DDS_External_Control::handle_global_position_control(ardupilot_msgs_msg_GlobalPosition& cmd_pos)
 {
@@ -31,19 +44,68 @@ bool AP_DDS_External_Control::handle_global_position_control(ardupilot_msgs_msg_
             return false;
         }
 
-        constexpr uint32_t MASK_POS_IGNORE =
-            TYPE_MASK_IGNORE_LATITUDE |
-            TYPE_MASK_IGNORE_LONGITUDE |
-            TYPE_MASK_IGNORE_ALTITUDE;
+        const uint16_t TYPE_MASK_LAST_BYTE = 0xF000;
 
-        if (!(cmd_pos.type_mask & MASK_POS_IGNORE)) {
+        // position setpoint mask
+        const uint16_t position_mask =
+            (TYPE_MASK_IGNORE_VX | TYPE_MASK_IGNORE_VY | TYPE_MASK_IGNORE_VZ |
+             TYPE_MASK_IGNORE_AFX | TYPE_MASK_IGNORE_AFY | TYPE_MASK_IGNORE_AFZ |
+             TYPE_MASK_YAW_IGNORE | TYPE_MASK_YAW_RATE_IGNORE |
+             TYPE_MASK_LAST_BYTE) ^ 0xFFFF;
+
+        // path setpoint mask
+        const uint16_t path_mask =
+            (TYPE_MASK_YAW_IGNORE | TYPE_MASK_YAW_RATE_IGNORE |
+             TYPE_MASK_LAST_BYTE) ^ 0xFFFF;
+
+        // position control
+        if (((cmd_pos.type_mask | TYPE_MASK_LAST_BYTE) ^ 0xFFFF) == position_mask) {
             Location loc(cmd_pos.latitude * 1E7, cmd_pos.longitude * 1E7, alt_cm, alt_frame);
             if (!external_control->set_global_position(loc)) {
                 return false; // Don't try sending other commands if this fails
             }
         }
 
-        // TODO add velocity and accel handling
+        // path guidance
+        if (((cmd_pos.type_mask | TYPE_MASK_LAST_BYTE) ^ 0xFFFF) == path_mask) {
+            Location position_on_path(cmd_pos.latitude * 1E7, cmd_pos.longitude * 1E7, alt_cm, alt_frame);
+
+            // convert velocty and acceleration setpoints from ENU to NED
+            Vector2f vel(
+                float(cmd_pos.velocity.linear.y),
+                float(cmd_pos.velocity.linear.x));
+            Vector2f accel(
+                float(cmd_pos.acceleration_or_force.linear.y),
+                float(cmd_pos.acceleration_or_force.linear.x));
+            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;
+                }
+            }
+
+            if (external_control->set_path_position_tangent_and_curvature(
+                    position_on_path, unit_vel, path_curvature, dir_is_ccw)) {
+                return false;
+            }
+        }
 
         return true;
     }
@@ -105,5 +167,4 @@ bool AP_DDS_External_Control::convert_alt_frame(const uint8_t frame_in,  Locatio
     return true;
 }
 
-
 #endif // AP_DDS_ENABLED

libraries/AP_ExternalControl/AP_ExternalControl.h

@@ -32,6 +32,15 @@ class AP_ExternalControl
         return false;
     }
 
+    /*
+        Set the target setpoint for path guidance: the closest position on the
+        path, the unit tangent to the path, and path curvature and direction.
+    */
+    virtual bool set_path_position_tangent_and_curvature(const Location &position_on_path,
+            Vector2f unit_path_tangent, const float path_curvature, const bool direction_is_ccw) WARN_IF_UNUSED {
+        return false;
+    }
+
     static AP_ExternalControl *get_singleton(void) WARN_IF_UNUSED {
         return singleton;
     }
@@ -42,7 +51,6 @@ class AP_ExternalControl
     static AP_ExternalControl *singleton;
 };
 
-
 namespace AP
 {
 AP_ExternalControl *externalcontrol();