Rover: guided integrates wpnav's support of fast updates

also guided implements wp_bearing, nav_bearing, crosstrack_error and
get_desired_lat_accel

Rover/Parameters.cpp

@@ -637,8 +637,8 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_GROUPINFO("LOIT_SPEED_GAIN", 47, ParametersG2, loiter_speed_gain, 0.5f),
 
     // @Param: FS_OPTIONS
-    // @DisplayName: Rover Failsafe Options
-    // @Description: Bitmask to enable Rover failsafe options
+    // @DisplayName: Failsafe Options
+    // @Description: Bitmask to enable failsafe options
     // @Values: 0:None,1:Failsafe enabled in Hold mode
     // @Bitmask: 0:Failsafe enabled in Hold mode
     // @User: Advanced
@@ -660,6 +660,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: ../libraries/APM_Control/AR_PosControl.cpp
     AP_SUBGROUPINFO(pos_control, "PSC", 51, ParametersG2, AR_PosControl),
 
+    // @Param: GUID_OPTIONS
+    // @DisplayName: Guided mode options
+    // @Description: Options that can be applied to change guided mode behaviour
+    // @Bitmask: 6:SCurves used for navigation
+    // @User: Advanced
+    AP_GROUPINFO("GUID_OPTIONS", 52, ParametersG2, guided_options, 0),
+
     AP_GROUPEND
 };
 

Rover/Parameters.h

@@ -414,6 +414,9 @@ public:
 
     // position controller
     AR_PosControl pos_control;
+
+    // guided options bitmask
+    AP_Int32 guided_options;
 };
 
 extern const AP_Param::Info var_info[];

Rover/mode.h

@@ -406,6 +406,12 @@ public:
     // return if external control is allowed in this mode (Guided or Guided-within-Auto)
     bool in_guided_mode() const override { return true; }
 
+    // return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
+    float wp_bearing() const override;
+    float nav_bearing() const override;
+    float crosstrack_error() const override;
+    float get_desired_lat_accel() const override;
+
     // return distance (in meters) to destination
     float get_distance_to_destination() const override;
 
@@ -452,8 +458,17 @@ protected:
         Guided_Stop
     };
 
+    // enum for GUID_OPTIONS parameter
+    enum class Options : int32_t {
+        SCurvesUsedForNavigation = (1U << 6)
+    };
+
     bool _enter() override;
 
+    // returns true if GUID_OPTIONS bit set to use scurve navigation instead of position controller input shaping
+    // scurves provide path planning and object avoidance but cannot handle fast updates to the destination (for fast updates use position controller input shaping)
+    bool use_scurves_for_navigation() const;
+
     GuidedMode _guided_mode;    // stores which GUIDED mode the vehicle is in
 
     // attitude control

Rover/mode_guided.cpp

@@ -142,6 +142,83 @@ void ModeGuided::update()
     }
 }
 
+// return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
+float ModeGuided::wp_bearing() const
+{
+    switch (_guided_mode) {
+    case Guided_WP:
+        return g2.wp_nav.wp_bearing_cd() * 0.01f;
+    case Guided_HeadingAndSpeed:
+    case Guided_TurnRateAndSpeed:
+        return 0.0f;
+    case Guided_Loiter:
+        return rover.mode_loiter.wp_bearing();
+    case Guided_SteeringAndThrottle:
+    case Guided_Stop:
+        return 0.0f;
+    }
+
+    // we should never reach here but just in case, return 0
+    return 0.0f;
+}
+
+float ModeGuided::nav_bearing() const
+{
+    switch (_guided_mode) {
+    case Guided_WP:
+        return g2.wp_nav.nav_bearing_cd() * 0.01f;
+    case Guided_HeadingAndSpeed:
+    case Guided_TurnRateAndSpeed:
+        return 0.0f;
+    case Guided_Loiter:
+        return rover.mode_loiter.nav_bearing();
+    case Guided_SteeringAndThrottle:
+    case Guided_Stop:
+        return 0.0f;
+    }
+
+    // we should never reach here but just in case, return 0
+    return 0.0f;
+}
+
+float ModeGuided::crosstrack_error() const
+{
+    switch (_guided_mode) {
+    case Guided_WP:
+        return g2.wp_nav.crosstrack_error();
+    case Guided_HeadingAndSpeed:
+    case Guided_TurnRateAndSpeed:
+        return 0.0f;
+    case Guided_Loiter:
+        return rover.mode_loiter.crosstrack_error();
+    case Guided_SteeringAndThrottle:
+    case Guided_Stop:
+        return 0.0f;
+    }
+
+    // we should never reach here but just in case, return 0
+    return 0.0f;
+}
+
+float ModeGuided::get_desired_lat_accel() const
+{
+    switch (_guided_mode) {
+    case Guided_WP:
+        return g2.wp_nav.get_lat_accel();
+    case Guided_HeadingAndSpeed:
+    case Guided_TurnRateAndSpeed:
+        return 0.0f;
+    case Guided_Loiter:
+        return rover.mode_loiter.get_desired_lat_accel();
+    case Guided_SteeringAndThrottle:
+    case Guided_Stop:
+        return 0.0f;
+    }
+
+    // we should never reach here but just in case, return 0
+    return 0.0f;
+}
+
 // return distance (in meters) to destination
 float ModeGuided::get_distance_to_destination() const
 {
@@ -234,15 +311,24 @@ bool ModeGuided::get_desired_location(Location& destination) const
 // set desired location
 bool ModeGuided::set_desired_location(const Location &destination, Location next_destination)
 {
-    if (g2.wp_nav.set_desired_location(destination, next_destination)) {
-
-        // handle guided specific initialisation and logging
-        _guided_mode = ModeGuided::Guided_WP;
-        send_notification = true;
-        rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_desired_speed(), 0.0f, 0.0f));
-        return true;
+    if (use_scurves_for_navigation()) {
+        // use scurves for navigation
+        if (!g2.wp_nav.set_desired_location(destination, next_destination)) {
+            return false;
+        }
+    } else {
+        // use position controller input shaping for navigation
+        // this does not support object avoidance but does allow faster updates of the target
+        if (!g2.wp_nav.set_desired_location_expect_fast_update(destination)) {
+            return false;
+        }
     }
-    return false;
+
+    // handle guided specific initialisation and logging
+    _guided_mode = ModeGuided::Guided_WP;
+    send_notification = true;
+    rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_desired_speed(), 0.0f, 0.0f));
+    return true;
 }
 
 // set desired attitude
@@ -353,3 +439,10 @@ bool ModeGuided::limit_breached() const
     // if we got this far we must be within limits
     return false;
 }
+
+// returns true if GUID_OPTIONS bit set to use scurve navigation instead of position controller input shaping
+// scurves provide path planning and object avoidance but cannot handle fast updates to the destination (for fast updates use position controller input shaping)
+bool ModeGuided::use_scurves_for_navigation() const
+{
+    return ((rover.g2.guided_options.get() & uint32_t(Options::SCurvesUsedForNavigation)) != 0);
+}