Plane: handle guided path requests in GUIDED mode #26328

srmainwaring · 2024-02-26T18:50:41Z

Add a new method handle_guided_path_request to the plane GUIDED mode and extra update function update_path in AP_Navigation. A preliminary implementation is provided for AP_L1_Control. The guided path mode is used to handle SET_POSITION_TARGET_GLOBAL_INT mavlink commands.

This is a simplified version of:

Plane: support off-board terrain navigation #26262

It excludes the NPFG controller and modifies the existing ModeGuided rather than introduce a new mode.

Details

The implementation follows the approach used in Rover's ModeGuided which supports sub-modes.
Two modes are provided: WP for the existing handle_guided_request, and Path for the new handled_guided_path_request.
A new virtual update method update_path is added to AP_Navigation.
An implementation in AP_L1_Control uses the existing update_waypoint and update_loiter functions.
Add a flag to disable loiter radius scaling. The scaling causes the vehicle to overshoot turns and loiter at a larger radius than commanded by the off-board planner. For terrain planning this may take the vehicle outside the safe regions.

Testing

See the tests using the terrain_navigation package in:

Plane: support off-board terrain navigation #26262

Tasks

Issues to resolve following further testing:

Rework the handling of the SET_POSITION_TARGET_GLOBAL_INT type_mask as the logic is not correct.
Use the POSITION_TARGET_XXX enum from mavlink common.h to check the bit mask.
Add CI test suite.
Must make sure that MAVProxy (changealt) and MissionPlanner can still change the vehicle's altitude

libraries/AP_L1_Control/AP_L1_Control.h

libraries/AP_L1_Control/AP_L1_Control.cpp

ArduPlane/mode.h

tridge

needs a CI test suite

tridge · 2024-03-13T07:17:00Z

ArduPlane/mode_guided.cpp

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


this should use change_alt_frame()

refuse -ve curvature?

tridge · 2024-03-13T07:21:35Z

libraries/AP_L1_Control/AP_L1_Control.cpp

@@ -515,3 +519,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::update_path(const class Location &position_on_path, Vector2f unit_path_tangent, float path_curvature, int8_t direction) {


add comment on units, check sign of curvature
explain what zero means
explain what direction means

tridge · 2024-03-13T07:21:37Z

libraries/AP_L1_Control/AP_L1_Control.cpp

+    if (!is_zero(path_curvature)) {
+        // moving along arc of circle - loiter about wp located at
+        // centre of curvature.
+        float radius_m = 1.0 / path_curvature;


The curvature is computed as path_curvature = accel_lat.length() / vel.length_squared(); which is always positive.
Should we add a comment that curvature is assumed to be positive in the header?

libraries/AP_L1_Control/AP_L1_Control.cpp

rmackay9 · 2024-03-13T07:23:47Z

ArduPlane/GCS_Mavlink.cpp

+            cmd.content.location.terrain_alt = false;
+            switch (pos_target.coordinate_frame) 
+            {
+                case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:


Sometimes we move these types of checks right to the top of the function. This just gets it out of the way and allows us to remove some indenting lower down.

peterbarker

Must make sure that MAVProxy (changealt) and MissionPlanner can still change the vehicle's altitude!

peterbarker · 2024-03-13T07:22:56Z

ArduPlane/GCS_Mavlink.cpp

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


Try mavlink_coordinate_frame_to_location_alt_frame

i.e. extract as much useful stuff from our COMMAND_INT / COMMAND_LONG parsing...

Rebase on top of #28479 once that's merged.

ArduPlane/GCS_Mavlink.cpp

rmackay9 · 2024-03-13T07:31:52Z

ArduPlane/mode.h

    bool _enter() override;
    bool _pre_arm_checks(size_t buflen, char *buffer) const override { return true; }

 private:
+
+    SubMode _guided_mode;


can you check that the vehicle always ends up in the WP sub mode when it is first switched to Guided mode? So it probably always does the first time the vehicle is switched to Guided but it may not if the vehicle is in Path sub mode (in Guided), then switched to another mode, then back to Guided.

Do you think it's desireable for guided mode to preserve any state when you switch out of it and back into guided?

For example, if you had sent a heading command, switch to RTL for 0.5 seconds, and back to guided, it seems the vehicle should chose the default WP sub mode and drop a point where it enters guided rather than go back to the old stale heading command.

Do you think it's desireable for guided mode to preserve any state when you switch out of it and back into guided?

This is why I added a HEADING_TYPE_DEFAULT mavlink option in #27428 - so you can explicitly switch to GUIDED_HEADING_NONE, because it should definitely not preserve state when switching out of the mode and then back again.

rmackay9 · 2024-03-13T07:32:53Z

ArduPlane/mode_guided.cpp

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


There are various altitude types so it might be best to instead use the Location class's method to change the altitude type.

ArduPlane/GCS_Mavlink.cpp

libraries/AP_L1_Control/AP_L1_Control.h

tridge · 2024-03-13T07:40:28Z

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


how does this variable get unset?

Ryanf55 · 2024-10-24T14:05:03Z

ArduPlane/mode_guided.cpp

+    Location location_on_path_abs_alt = location_on_path;
+
+    // add home alt if needed
+    if (location_on_path_abs_alt.relative_alt) {


Instead of these bools, just call the altitude getter with the right frame.

libraries/AP_L1_Control/AP_L1_Control.cpp

ArduPlane/mode.h

timtuxworth · 2024-10-24T21:34:42Z

ArduPlane/mode_guided.cpp

+    default:
+        gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
+        break;
+    }


Guided Mode already (logically) has another "sub mode" which is "heading".

ArduPilot/ardupilot_wiki#6096

timtuxworth · 2024-10-24T21:35:36Z

ArduPlane/mode.h

    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


Also "None"

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

- Implement method update_path - Update comment - Reduce look-ahead distance in update_path Signed-off-by: Rhys Mainwaring <[email protected]>

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

- 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. - Remove duplicate check for guided mode. Signed-off-by: Rhys Mainwaring <[email protected]>

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

* Save flash Signed-off-by: Ryan Friedman <[email protected]>

Signed-off-by: Ryan Friedman <[email protected]>

Avoid !is_zero

srmainwaring requested review from tridge and IamPete1 February 26, 2024 18:52

Ryanf55 self-requested a review February 26, 2024 18:52

Ryanf55 added the ROS label Feb 26, 2024

srmainwaring force-pushed the prs/pr-plane-guided-path branch from b811f87 to 119ca3b Compare February 27, 2024 08:50

This was referenced Feb 27, 2024

Plane: support off-board terrain navigation #26262

Draft

DDS: add guided path external control and implement for plane #26339

Open

srmainwaring added the Plane label Feb 27, 2024

srmainwaring force-pushed the prs/pr-plane-guided-path branch 3 times, most recently from 64bba35 to 4d07d90 Compare March 6, 2024 11:05

srmainwaring mentioned this pull request Mar 6, 2024

AP_DDS: Add trajectory tracking control for Plane #25382

Open

srmainwaring force-pushed the prs/pr-plane-guided-path branch 3 times, most recently from b445ae6 to 72f40b4 Compare March 12, 2024 21:00

srmainwaring added the DevCallEU label Mar 12, 2024