AP_DDS: initial version of external guided path control

Signed-off-by: Rhys Mainwaring <[email protected]>

libraries/AP_DDS/AP_DDS_ExternalControl.cpp

@@ -11,9 +11,24 @@
 // 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;
+enum class AP_DDS_PositionTargetTypeMask : uint16_t {
+    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,
+};
+
+// 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;
 
 bool AP_DDS_External_Control::handle_global_position_control(ardupilot_msgs_msg_GlobalPosition& cmd_pos)
 {
@@ -31,6 +46,8 @@ bool AP_DDS_External_Control::handle_global_position_control(ardupilot_msgs_msg_
             return false;
         }
 
+//! @todo(srmainwaring) rework the case evaluation logic
+#if 0
         constexpr uint32_t MASK_POS_IGNORE =
             TYPE_MASK_IGNORE_LATITUDE |
             TYPE_MASK_IGNORE_LONGITUDE |
@@ -42,8 +59,49 @@ bool AP_DDS_External_Control::handle_global_position_control(ardupilot_msgs_msg_
                 return false; // Don't try sending other commands if this fails
             }
         }
+#endif
+
+        // path guidance
+        //! @todo(srmainwaring) consolidate duplicate code in GCS_MAVLINK_Plane::handle_set_position_target_global_int.
+        {
+            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;
+                }
+            }
 
-        // TODO add velocity and accel handling
+            if (external_control->set_path_position_tangent_and_curvature(
+                position_on_path, unit_vel, path_curvature, dir_is_ccw)) {
+                return false;
+            }
+        }
 
         return true;
     }