Rover: integrate navigation library

APMrover2/APMrover2.cpp

@@ -291,6 +291,9 @@ void Rover::one_second_loop(void)
     // need to set "likely flying" when armed to allow for compass
     // learning to run
     ahrs.set_likely_flying(hal.util->get_soft_armed());
+
+    // send latest param values to wp_nav
+    g2.wp_nav.set_turn_params(g.turn_max_g, g2.turn_radius, g2.motors.have_skid_steering());
 }
 
 void Rover::update_GPS(void)

APMrover2/Log.cpp

@@ -130,10 +130,10 @@ void Rover::Log_Write_Nav_Tuning()
         LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
         time_us             : AP_HAL::micros64(),
         wp_distance         : control_mode->get_distance_to_destination(),
-        wp_bearing_cd       : (uint16_t)wrap_360_cd(nav_controller->target_bearing_cd()),
+        wp_bearing_cd       : (uint16_t)wrap_360_cd(control_mode->wp_bearing() * 100),
-        nav_bearing_cd      : (uint16_t)wrap_360_cd(nav_controller->nav_bearing_cd()),
+        nav_bearing_cd      : (uint16_t)wrap_360_cd(control_mode->nav_bearing() * 100),
         yaw                 : (uint16_t)ahrs.yaw_sensor,
-        xtrack_error        : nav_controller->crosstrack_error()
+        xtrack_error        : control_mode->crosstrack_error()
     };
     logger.WriteBlock(&pkt, sizeof(pkt));
 }

APMrover2/Parameters.cpp

@@ -93,14 +93,6 @@ const AP_Param::Info Rover::var_info[] = {
     // @User: Standard
     GSCALAR(speed_cruise,        "CRUISE_SPEED",    CRUISE_SPEED),
 
-    // @Param: PIVOT_TURN_ANGLE
-    // @DisplayName: Pivot turn angle
-    // @Description: Navigation angle threshold in degrees to switch to pivot steering when SKID_STEER_OUT is 1. This allows you to setup a skid steering rover to turn on the spot in auto mode when the angle it needs to turn it greater than this angle. An angle of zero means to disable pivot turning. Note that you will probably also want to set a low value for WP_RADIUS to get neat turns.
-    // @Units: deg
-    // @Range: 0 360
-    // @Increment: 1
-    // @User: Standard
-    GSCALAR(pivot_turn_angle,   "PIVOT_TURN_ANGLE",  60),
 
     // @Param: CRUISE_THROTTLE
     // @DisplayName: Base throttle percentage in auto
@@ -260,24 +252,6 @@ const AP_Param::Info Rover::var_info[] = {
     // @User: Standard
     GSCALAR(mode6,           "MODE6",         Mode::Number::MANUAL),
 
-    // @Param: WP_RADIUS
-    // @DisplayName: Waypoint radius
-    // @Description: The distance in meters from a waypoint when we consider the waypoint has been reached. This determines when the rover will turn along the next waypoint path.
-    // @Units: m
-    // @Range: 0 1000
-    // @Increment: 0.1
-    // @User: Standard
-    GSCALAR(waypoint_radius,        "WP_RADIUS",        2.0f),
-
-    // @Param: WP_OVERSHOOT
-    // @DisplayName: Waypoint overshoot maximum
-    // @Description: Waypoint overshoot maximum in meters.  The vehicle will attempt to stay within this many meters of the track as it completes one waypoint and moves to the next.
-    // @Units: m
-    // @Range: 0 10
-    // @Increment: 0.1
-    // @User: Standard
-    GSCALAR(waypoint_overshoot,     "WP_OVERSHOOT", 2.0f),
-
     // @Param: TURN_MAX_G
     // @DisplayName: Turning maximum G force
     // @Description: The maximum turning acceleration (in units of gravities) that the rover can handle while remaining stable. The navigation code will keep the lateral acceleration below this level to avoid rolling over or slipping the wheels in turns
@@ -507,14 +481,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: ../libraries/AP_SmartRTL/AP_SmartRTL.cpp
     AP_SUBGROUPINFO(smart_rtl, "SRTL_", 13, ParametersG2, AP_SmartRTL),
 
-    // @Param: WP_SPEED
+    // 14 was WP_SPEED and should not be re-used
-    // @DisplayName: Waypoint speed default
-    // @Description: Waypoint speed default.  If zero use CRUISE_SPEED.
-    // @Units: m/s
-    // @Range: 0 100
-    // @Increment: 0.1
-    // @User: Standard
-    AP_GROUPINFO("WP_SPEED", 14, ParametersG2, wp_speed, 0.0f),
 
     // @Param: RTL_SPEED
     // @DisplayName: Return-to-Launch speed default
@@ -544,14 +511,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp
     AP_SUBGROUPINFO(avoid, "AVOID_", 19, ParametersG2, AC_Avoid),
 
-    // @Param: PIVOT_TURN_RATE
+    // 20 was PIVOT_TURN_RATE and should not be re-used
-    // @DisplayName: Pivot turn rate
-    // @Description: Desired pivot turn rate in deg/s.
-    // @Units: deg/s
-    // @Range: 0 360
-    // @Increment: 1
-    // @User: Standard
-    AP_GROUPINFO("PIVOT_TURN_RATE", 20, ParametersG2, pivot_turn_rate, 90),
 
     // @Param: BAL_PITCH_MAX
     // @DisplayName: BalanceBot Maximum Pitch
@@ -708,6 +668,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("STICK_MIXING", 42, ParametersG2, stick_mixing, 0),
 
+    // @Group: WP_
+    // @Path: ../libraries/AR_WPNav/AR_WPNav.cpp
+    AP_SUBGROUPINFO(wp_nav, "WP_", 43, ParametersG2, AR_WPNav),
+
     AP_GROUPEND
 };
 
@@ -725,6 +689,46 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
 // @Description: RC Channel to use for auxiliary functions including saving waypoints
 // @User: Advanced
 
+// @Param: PIVOT_TURN_ANGLE
+// @DisplayName: Pivot turn angle
+// @Description: Navigation angle threshold in degrees to switch to pivot steering when SKID_STEER_OUT is 1. This allows you to setup a skid steering rover to turn on the spot in auto mode when the angle it needs to turn it greater than this angle. An angle of zero means to disable pivot turning. Note that you will probably also want to set a low value for WP_RADIUS to get neat turns.
+// @Units: deg
+// @Range: 0 360
+// @Increment: 1
+// @User: Standard
+
+// @Param: WP_RADIUS
+// @DisplayName: Waypoint radius
+// @Description: The distance in meters from a waypoint when we consider the waypoint has been reached. This determines when the rover will turn along the next waypoint path.
+// @Units: m
+// @Range: 0 1000
+// @Increment: 0.1
+// @User: Standard
+
+// @Param: WP_OVERSHOOT
+// @DisplayName: Waypoint overshoot maximum
+// @Description: Waypoint overshoot maximum in meters.  The vehicle will attempt to stay within this many meters of the track as it completes one waypoint and moves to the next.
+// @Units: m
+// @Range: 0 10
+// @Increment: 0.1
+// @User: Standard
+
+// @Param: WP_SPEED
+// @DisplayName: Waypoint speed default
+// @Description: Waypoint speed default.  If zero use CRUISE_SPEED.
+// @Units: m/s
+// @Range: 0 100
+// @Increment: 0.1
+// @User: Standard
+
+// @Param: PIVOT_TURN_RATE
+// @DisplayName: Pivot turn rate
+// @Description: Desired pivot turn rate in deg/s.
+// @Units: deg/s
+// @Range: 0 360
+// @Increment: 1
+// @User: Standard
+
 ParametersG2::ParametersG2(void)
     :
 #if ADVANCED_FAILSAFE == ENABLED
@@ -739,7 +743,8 @@ ParametersG2::ParametersG2(void)
     avoid(rover.ahrs, fence, rover.g2.proximity, &rover.g2.beacon),
     follow(),
     windvane(),
-    airspeed()
+    airspeed(),
+    wp_nav(attitude_control, rover.L1_controller)
 {
     AP_Param::setup_object_defaults(this, var_info);
 }
@@ -772,6 +777,11 @@ const AP_Param::ConversionInfo conversion_table[] = {
     { Parameters::k_param_throttle_max_old,   0,      AP_PARAM_INT8,  "MOT_THR_MAX" },
 
     { Parameters::k_param_compass_enabled_deprecated,       0,      AP_PARAM_INT8, "COMPASS_ENABLE" },
+    { Parameters::k_param_pivot_turn_angle,   0,      AP_PARAM_INT16,  "WP_PIVOT_ANGLE" },
+    { Parameters::k_param_waypoint_radius,    0,      AP_PARAM_FLOAT,  "WP_RADIUS" },
+    { Parameters::k_param_waypoint_overshoot, 0,      AP_PARAM_FLOAT,  "WP_OVERSHOOT" },
+    { Parameters::k_param_g2,                20,      AP_PARAM_INT16,  "WP_PIVOT_RATE" },
+
 };
 
 void Rover::load_parameters(void)
@@ -812,7 +822,7 @@ void Rover::load_parameters(void)
         g2.crash_angle.set_default(0);
         g2.loit_type.set_default(1);
         g2.loit_radius.set_default(5);
-        g.waypoint_overshoot.set_default(10);
+        g2.wp_nav.set_default_overshoot(10);
     }
 
     const uint8_t old_rc_keys[14] = { Parameters::k_param_rc_1_old,  Parameters::k_param_rc_2_old,
@@ -840,6 +850,18 @@ void Rover::load_parameters(void)
         }
     }
 
+    // set AR_WPNav's WP_SPEED to be old WP_SPEED (if set) or CRUISE_SPEED (if set)
+    const AP_Param::ConversionInfo wp_speed_old_info = { Parameters::k_param_g2, 14, AP_PARAM_FLOAT, "WP_SPEED" };
+    const AP_Param::ConversionInfo cruise_speed_info = { Parameters::k_param_speed_cruise, 0, AP_PARAM_FLOAT, "WP_SPEED" };
+    AP_Float wp_speed_old;
+    if (AP_Param::find_old_parameter(&wp_speed_old_info, &wp_speed_old)) {
+        // old WP_SPEED parameter value was set so copy to new WP_SPEED
+        AP_Param::convert_old_parameter(&wp_speed_old_info, 1.0f);
+    } else {
+        // copy CRUISE_SPEED to new WP_SPEED
+        AP_Param::convert_old_parameter(&cruise_speed_info, 1.0f);
+    }
+
     // configure safety switch to allow stopping the motors while armed
 #if HAL_HAVE_SAFETY_SWITCH
     AP_Param::set_default_by_name("BRD_SAFETYOPTION", AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF|

APMrover2/Parameters.h

@@ -36,7 +36,7 @@ public:
         k_param_scheduler,
         k_param_relay,
         k_param_BoardConfig,
-        k_param_pivot_turn_angle,
+        k_param_pivot_turn_angle,  // unused
         k_param_rc_13_old,  // unused
         k_param_rc_14_old,  // unused
 
@@ -173,8 +173,8 @@ public:
         //
         k_param_command_total = 220,    // unused
         k_param_command_index,          // unused
-        k_param_waypoint_radius,
+        k_param_waypoint_radius,        // unused
-        k_param_waypoint_overshoot,
+        k_param_waypoint_overshoot,     // unused
 
         //
         // camera control
@@ -234,7 +234,6 @@ public:
     AP_Int8     auto_trigger_pin;
     AP_Float    auto_kickstart;
     AP_Float    turn_max_g;
-    AP_Int16    pivot_turn_angle;
     AP_Int16    gcs_pid_mask;
 
     // Throttle
@@ -269,11 +268,6 @@ public:
     AP_Int8     mode5;
     AP_Int8     mode6;
 
-    // Waypoints
-    //
-    AP_Float    waypoint_radius;
-    AP_Float    waypoint_overshoot;
-
     Parameters() {}
 };
 
@@ -330,8 +324,7 @@ public:
     // Safe RTL library
     AP_SmartRTL smart_rtl;
 
-    // default speeds for auto, rtl
+    // default speeds for rtl
-    AP_Float wp_speed;
     AP_Float rtl_speed;
 
     // frame class for vehicle
@@ -346,9 +339,6 @@ public:
     // avoidance library
     AC_Avoid avoid;
 
-    // pivot turn rate
-    AP_Int16 pivot_turn_rate;
-
     // pitch angle at 100% throttle
     AP_Float bal_pitch_max;
 
@@ -404,6 +394,8 @@ public:
     AP_Scripting scripting;
 #endif // ENABLE_SCRIPTING
 
+    // waypoint navigation
+    AR_WPNav wp_nav;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/Rover.cpp

@@ -29,7 +29,6 @@ Rover::Rover(void) :
     channel_lateral(nullptr),
     logger{g.log_bitmask},
     modes(&g.mode1),
-    nav_controller(&L1_controller),
     control_mode(&mode_initializing),
     G_Dt(0.02f)
 {

APMrover2/Rover.h

@@ -65,6 +65,7 @@
 #include <AP_WheelEncoder/AP_WheelEncoder.h>
 #include <AP_WheelEncoder/AP_WheelRateControl.h>
 #include <APM_Control/AR_AttitudeControl.h>
+#include <AR_WPNav/AR_WPNav.h>
 #include <AP_SmartRTL/AP_SmartRTL.h>
 #include <AP_Logger/AP_Logger.h>
 #include <Filter/AverageFilter.h>                   // Mode Filter from Filter library
@@ -198,9 +199,6 @@ private:
 
     AP_L1_Control L1_controller{ahrs, nullptr};
 
-    // selected navigation controller
-    AP_Navigation *nav_controller;
-
 #if AP_AHRS_NAVEKF_AVAILABLE
     OpticalFlow optflow;
 #endif
@@ -307,9 +305,6 @@ private:
     // flyforward timer
     uint32_t flyforward_start_ms;
 
-    // true if pivoting (set by use_pivot_steering)
-    bool pivot_steering_active;
-
     static const AP_Scheduler::Task scheduler_tasks[];
 
     static const AP_Param::Info var_info[];
@@ -474,8 +469,6 @@ private:
     void rpm_update(void);
 
     // Steering.cpp
-    bool use_pivot_steering_at_next_WP(float yaw_error_cd);
-    bool use_pivot_steering(float yaw_error_cd);
     void set_servos(void);
 
     // system.cpp

APMrover2/Steering.cpp

@@ -1,53 +1,5 @@
 #include "Rover.h"
 
-/*
-    work out if we are going to use pivot steering at next waypoint
-*/
-bool Rover::use_pivot_steering_at_next_WP(float yaw_error_cd)
-{
-    // check cases where we clearly cannot use pivot steering
-    if (!g2.motors.have_skid_steering() || g.pivot_turn_angle <= 0) {
-        return false;
-    }
-
-    // if error is larger than pivot_turn_angle then use pivot steering at next WP
-    if (fabsf(yaw_error_cd) * 0.01f > g.pivot_turn_angle) {
-        return true;
-    }
-
-    return false;
-}
-
-/*
-    work out if we are going to use pivot steering
-*/
-bool Rover::use_pivot_steering(float yaw_error_cd)
-{
-    // check cases where we clearly cannot use pivot steering
-    if (!g2.motors.have_skid_steering() || g.pivot_turn_angle <= 0) {
-        pivot_steering_active = false;
-        return false;
-    }
-
-    // calc bearing error
-    const float yaw_error = fabsf(yaw_error_cd) * 0.01f;
-
-    // if error is larger than pivot_turn_angle start pivot steering
-    if (yaw_error > g.pivot_turn_angle) {
-        pivot_steering_active = true;
-        return true;
-    }
-
-    // if within 10 degrees of the target heading, exit pivot steering
-    if (yaw_error < 10.0f) {
-        pivot_steering_active = false;
-        return false;
-    }
-
-    // by default stay in
-    return pivot_steering_active;
-}
-
 /*****************************************
     Set the flight control servos based on the current calculated values
 *****************************************/

APMrover2/mode.cpp

@@ -165,7 +165,7 @@ float Mode::wp_bearing() const
     if (!is_autopilot_mode()) {
         return 0.0f;
     }
-    return rover.nav_controller->target_bearing_cd() * 0.01f;
+    return g2.wp_nav.wp_bearing_cd() * 0.01f;
 }
 
 // return short-term target heading in degrees (i.e. target heading back to line between waypoints)
@@ -174,7 +174,7 @@ float Mode::nav_bearing() const
     if (!is_autopilot_mode()) {
         return 0.0f;
     }
-    return rover.nav_controller->nav_bearing_cd() * 0.01f;
+    return g2.wp_nav.nav_bearing_cd() * 0.01f;
 }
 
 // return cross track error (i.e. vehicle's distance from the line between waypoints)
@@ -183,57 +183,17 @@ float Mode::crosstrack_error() const
     if (!is_autopilot_mode()) {
         return 0.0f;
     }
-    return rover.nav_controller->crosstrack_error();
+    return g2.wp_nav.crosstrack_error();
 }
 
 // set desired location
 void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
 {
-    // set origin to last destination if waypoint controller active
+    g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd);
-    if ((AP_HAL::millis() - last_steer_to_wp_ms < 100) && _reached_destination) {
-        _origin = _destination;
-    } else {
-        // otherwise use reasonable stopping point
-        calc_stopping_location(_origin);
-    }
-    _destination = destination;
 
     // initialise distance
-    _distance_to_destination = _origin.get_distance(_destination);
+    _distance_to_destination = g2.wp_nav.get_distance_to_destination();
     _reached_destination = false;
-
-    // set final desired speed
-    _desired_speed_final = 0.0f;
-    if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) {
-        const float curr_leg_bearing_cd = _origin.get_bearing_to(_destination);
-        const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);
-        if (fabsf(turn_angle_cd) < 10.0f) {
-            // if turning less than 0.1 degrees vehicle can continue at full speed
-            // we use 0.1 degrees instead of zero to avoid divide by zero in calcs below
-            _desired_speed_final = _desired_speed;
-        } else if (rover.use_pivot_steering_at_next_WP(turn_angle_cd)) {
-            // pivoting so we will stop
-            _desired_speed_final = 0.0f;
-        } else {
-            // calculate maximum speed that keeps overshoot within bounds
-            const float radius_m = fabsf(g.waypoint_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f));
-            _desired_speed_final = MIN(_desired_speed, safe_sqrt(g.turn_max_g * GRAVITY_MSS * radius_m));
-        }
-    }
-}
-
-// set desired location as an offset from the EKF origin in NED frame
-bool Mode::set_desired_location_NED(const Vector3f& destination, float next_leg_bearing_cd)
-{
-    Location destination_ned;
-    // initialise destination to ekf origin
-    if (!ahrs.get_origin(destination_ned)) {
-        return false;
-    }
-    // apply offset
-    destination_ned.offset(destination.x, destination.y);
-    set_desired_location(destination_ned, next_leg_bearing_cd);
-    return true;
 }
 
 // set desired heading and speed
@@ -247,19 +207,17 @@ void Mode::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
     _desired_speed = target_speed;
 }
 
-// get default speed for this mode (held in (CRUISE_SPEED, WP_SPEED or RTL_SPEED)
+// get default speed for this mode (held in (WP_SPEED or RTL_SPEED)
 float Mode::get_speed_default(bool rtl) const
 {
     if (rtl && is_positive(g2.rtl_speed)) {
         return g2.rtl_speed;
-    } else if (is_positive(g2.wp_speed)) {
-        return g2.wp_speed;
-    } else {
-        return g.speed_cruise;
     }
+
+    return g2.wp_nav.get_default_speed();
 }
 
-// restore desired speed to default from parameter values (CRUISE_SPEED or WP_SPEED)
+// restore desired speed to default from parameter values (WP_SPEED)
 void Mode::set_desired_speed_to_default(bool rtl)
 {
     _desired_speed = get_speed_default(rtl);
@@ -278,7 +236,7 @@ bool Mode::set_desired_speed(float speed)
 // execute the mission in reverse (i.e. backing up)
 void Mode::set_reversed(bool value)
 {
-    _reversed = value;
+    g2.wp_nav.set_reversed(value);
 }
 
 // handle tacking request (from auxiliary switch) in sailboats
@@ -415,81 +373,48 @@ float Mode::calc_speed_nudge(float target_speed, float cruise_speed, float cruis
     return target_speed + speed_nudge;
 }
 
-// calculated a reduced speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint
+// high level call to navigate to waypoint
-// should be called after calc_lateral_acceleration and before calc_throttle
+// uses wp_nav to calculate turn rate and speed to drive along the path from origin to destination
-// relies on these internal members being updated: _yaw_error_cd, _distance_to_destination
+// this function updates _distance_to_destination and _yaw_error_cd
-float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed)
+void Mode::navigate_to_waypoint()
 {
-    // reduce speed to zero during pivot turns
+    // update navigation controller
-    if (rover.use_pivot_steering(_yaw_error_cd)) {
+    g2.wp_nav.update(rover.G_Dt);
-        return 0.0f;
+    _distance_to_destination = g2.wp_nav.get_distance_to_destination();
-    }
 
-    // reduce speed to limit overshoot from line between origin and destination
+    // pass speed to throttle controller
-    // calculate number of degrees vehicle must turn to face waypoint
+    const float desired_speed = g2.wp_nav.get_speed();
-    const float heading_cd = is_negative(desired_speed) ? wrap_180_cd(ahrs.yaw_sensor + 18000) : ahrs.yaw_sensor;
+    calc_throttle(desired_speed, true, true);
-    const float wp_yaw_diff = wrap_180_cd(rover.nav_controller->target_bearing_cd() - heading_cd);
-    const float turn_angle_rad = fabsf(radians(wp_yaw_diff * 0.01f));
-
-    // calculate distance from vehicle to line + wp_overshoot
-    const float line_yaw_diff = wrap_180_cd(_origin.get_bearing_to(_destination) - heading_cd);
-    const float xtrack_error = rover.nav_controller->crosstrack_error();
-    const float dist_from_line = fabsf(xtrack_error);
-    const bool heading_away = is_positive(line_yaw_diff) == is_positive(xtrack_error);
-    const float wp_overshoot_adj = heading_away ? -dist_from_line : dist_from_line;
-
-    // calculate radius of circle that touches vehicle's current position and heading and target position and heading
-    float radius_m = 999.0f;
-    float radius_calc_denom = fabsf(1.0f - cosf(turn_angle_rad));
-    if (!is_zero(radius_calc_denom)) {
-        radius_m = MAX(0.0f, rover.g.waypoint_overshoot + wp_overshoot_adj) / radius_calc_denom;
-    }
 
-    // calculate and limit speed to allow vehicle to stay on circle
+    float desired_heading_cd = g2.wp_nav.wp_bearing_cd();
-    float overshoot_speed_max = safe_sqrt(g.turn_max_g * GRAVITY_MSS * MAX(g2.turn_radius, radius_m));
+    _yaw_error_cd = wrap_180_cd(desired_heading_cd - ahrs.yaw_sensor);
-    float speed_max = constrain_float(desired_speed, -overshoot_speed_max, overshoot_speed_max);
 
-    // limit speed based on distance to waypoint and max acceleration/deceleration
+    if (rover.sailboat_use_indirect_route(desired_heading_cd)) {
-    if (is_positive(_distance_to_destination) && is_positive(attitude_control.get_decel_max())) {
+        // sailboats use heading controller when tacking upwind
-        const float dist_speed_max = safe_sqrt(2.0f * _distance_to_destination * attitude_control.get_decel_max() + sq(_desired_speed_final));
+        desired_heading_cd = rover.sailboat_calc_heading(desired_heading_cd);
-        speed_max = constrain_float(speed_max, -dist_speed_max, dist_speed_max);
+        calc_steering_to_heading(desired_heading_cd, g2.wp_nav.get_pivot_rate());
+    } else {
+        // call turn rate steering controller
+        calc_steering_from_turn_rate(g2.wp_nav.get_turn_rate_rads(), desired_speed, g2.wp_nav.get_reversed());
     }
-
-    // return minimum speed
-    return speed_max;
 }
 
-// calculate the lateral acceleration target to cause the vehicle to drive along the path from origin to destination
+// calculate steering output given a turn rate and speed
-// this function updates the _yaw_error_cd value
+void Mode::calc_steering_from_turn_rate(float turn_rate, float speed, bool reversed)
-void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed)
 {
-    // record system time of call
+    // add obstacle avoidance response to lateral acceleration target
-    last_steer_to_wp_ms = AP_HAL::millis();
+    // ToDo: replace this type of object avoidance with path planning
-
+    if (!reversed) {
-    // Calculate the required turn of the wheels
+        const float lat_accel_adj = (rover.obstacle.turn_angle / 45.0f) * g.turn_max_g;
-    // negative error = left turn
+        turn_rate += attitude_control.get_turn_rate_from_lat_accel(lat_accel_adj, speed);
-    // positive error = right turn
-    rover.nav_controller->set_reverse(reversed);
-    rover.nav_controller->update_waypoint(origin, destination, g.waypoint_radius);
-    float desired_lat_accel = rover.nav_controller->lateral_acceleration();
-    float desired_heading = rover.nav_controller->target_bearing_cd();
-    if (reversed) {
-        desired_heading = wrap_360_cd(desired_heading + 18000);
-        desired_lat_accel *= -1.0f;
     }
-    _yaw_error_cd = wrap_180_cd(desired_heading - ahrs.yaw_sensor);
 
-    if (rover.sailboat_use_indirect_route(desired_heading)) {
+    // calculate and send final steering command to motor library
-        // sailboats use heading controller when tacking upwind
+    const float steering_out = attitude_control.get_steering_out_rate(turn_rate,
-        desired_heading = rover.sailboat_calc_heading(desired_heading);
+                                                                      g2.motors.limit.steer_left,
-        calc_steering_to_heading(desired_heading, g2.pivot_turn_rate);
+                                                                      g2.motors.limit.steer_right,
-    } else if (rover.use_pivot_steering(_yaw_error_cd)) {
+                                                                      rover.G_Dt);
-        // for pivot turns use heading controller
+    g2.motors.set_steering(steering_out * 4500.0f);
-        calc_steering_to_heading(desired_heading, g2.pivot_turn_rate);
-    } else {
-        // call lateral acceleration to steering controller
-        calc_steering_from_lateral_acceleration(desired_lat_accel, reversed);
-    }
 }
 
 /*

APMrover2/mode.h

@@ -105,9 +105,6 @@ public:
     //   next_leg_bearing_cd should be heading to the following waypoint (used to slow the vehicle in order to make the turn)
     virtual void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN);
 
-    // set desired location as offset from the EKF origin, return true on success
-    bool set_desired_location_NED(const Vector3f& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN);
-
     // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
     virtual bool reached_destination() const { return true; }
 
@@ -156,8 +153,11 @@ protected:
     // decode pilot's input and return heading_out (in cd) and speed_out (in m/s)
     void get_pilot_desired_heading_and_speed(float &heading_out, float &speed_out);
 
-    // calculate steering output to drive along line from origin to destination waypoint
+    // high level call to navigate to waypoint
-    void calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed = false);
+    void navigate_to_waypoint();
+
+    // calculate steering output given a turn rate and speed
+    void calc_steering_from_turn_rate(float turn_rate, float speed, bool reversed);
 
     // calculate steering angle given a desired lateral acceleration
     void calc_steering_from_lateral_acceleration(float lat_accel, bool reversed = false);
@@ -183,11 +183,6 @@ protected:
     //  return value is a new speed (in m/s) which up to the projected maximum speed based on the cruise speed and cruise throttle
     float calc_speed_nudge(float target_speed, float cruise_speed, float cruise_throttle);
 
-    // calculated a reduced speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint
-    // should be called after calc_steering_to_waypoint and before calc_throttle
-    // relies on these internal members being updated: lateral_acceleration, _yaw_error_cd, _distance_to_destination
-    float calc_reduced_speed_for_turn_or_distance(float desired_speed);
-
     // calculate vehicle stopping location using current location, velocity and maximum acceleration
     void calc_stopping_location(Location& stopping_loc);
 
@@ -208,19 +203,13 @@ protected:
     class RC_Channel *&channel_lateral;
     class AR_AttitudeControl &attitude_control;
 
-
     // private members for waypoint navigation
-    Location _origin;           // origin Location (vehicle will travel from the origin to the destination)
-    Location _destination;      // destination Location when in Guided_WP
     float _distance_to_destination; // distance from vehicle to final destination in meters
     bool _reached_destination;  // true once the vehicle has reached the destination
     float _desired_yaw_cd;      // desired yaw in centi-degrees
     float _yaw_error_cd;        // error between desired yaw and actual yaw in centi-degrees
     float _desired_speed;       // desired speed in m/s
-    float _desired_speed_final; // desired speed in m/s when we reach the destination
     float _speed_error;         // ground speed error in m/s
-    uint32_t last_steer_to_wp_ms;   // system time of last call to calc_steering_to_waypoint
-    bool _reversed;             // execute the mission by backing up
 };
 
 
@@ -414,6 +403,7 @@ protected:
     bool have_attitude_target;  // true if we have a valid attitude target
     uint32_t _des_att_time_ms;  // system time last call to set_desired_attitude was made (used for timeout)
     float _desired_yaw_rate_cds;// target turn rate centi-degrees per second
+    bool sent_notification;     // used to send one time notification to ground station
 
     // limits
     struct {
@@ -457,8 +447,8 @@ public:
     bool is_autopilot_mode() const override { return true; }
 
     // return desired heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
-    float wp_bearing() const override { return _desired_yaw_cd; }
+    float wp_bearing() const override { return _desired_yaw_cd * 0.01f; }
-    float nav_bearing() const override { return _desired_yaw_cd; }
+    float nav_bearing() const override { return _desired_yaw_cd * 0.01f; }
     float crosstrack_error() const override { return 0.0f; }
 
     // return distance (in meters) to destination
@@ -467,6 +457,8 @@ public:
 protected:
 
     bool _enter() override;
+
+    Location _destination;      // target location to hold position around
 };
 
 class ModeManual : public Mode
@@ -507,11 +499,13 @@ public:
     bool is_autopilot_mode() const override { return true; }
 
     float get_distance_to_destination() const override { return _distance_to_destination; }
-    bool reached_destination() const override { return _reached_destination; }
+    bool reached_destination() const override;
 
 protected:
 
     bool _enter() override;
+
+    bool sent_notification; // used to send one time notification to ground station
 };
 
 class ModeSmartRTL : public Mode

APMrover2/mode_acro.cpp

@@ -28,7 +28,7 @@ void ModeAcro::update()
     if (g2.motors.has_sail() && rover.sailboat_tacking()) {
         // call heading controller during tacking
         steering_out = attitude_control.get_steering_out_heading(rover.sailboat_get_tack_heading_rad(),
-                                                                 g2.pivot_turn_rate,
+                                                                 g2.wp_nav.get_pivot_rate(),
                                                                  g2.motors.limit.steer_left,
                                                                  g2.motors.limit.steer_right,
                                                                  rover.G_Dt);

APMrover2/mode_auto.cpp

@@ -12,10 +12,10 @@ bool ModeAuto::_enter()
     }
 
     // initialise waypoint speed
-    set_desired_speed_to_default();
+    g2.wp_nav.set_desired_speed_to_default();
 
     // init location target
-    set_desired_location(rover.current_loc);
+    g2.wp_nav.set_desired_location(rover.current_loc);
 
     // other initialisation
     auto_triggered = false;
@@ -41,18 +41,9 @@ void ModeAuto::update()
     switch (_submode) {
         case Auto_WP:
         {
-            _distance_to_destination = rover.current_loc.get_distance(_destination);
+            if (!g2.wp_nav.reached_destination()) {
-            const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius;
+                // update navigation controller
-            // check if we've reached the destination
+                navigate_to_waypoint();
-            if (!_reached_destination && (near_wp || rover.current_loc.past_interval_finish_line(_origin, _destination))) {
-                // trigger reached
-                _reached_destination = true;
-            }
-            // determine if we should keep navigating
-            if (!_reached_destination) {
-                // continue driving towards destination
-                calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed);
-                calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
             } else {
                 // we have reached the destination so stay here
                 if (rover.is_boat()) {
@@ -62,6 +53,8 @@ void ModeAuto::update()
                 } else {
                     stop_vehicle();
                 }
+                // update distance to destination
+                _distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
             }
             break;
         }
@@ -118,10 +111,22 @@ void ModeAuto::calc_throttle(float target_speed, bool nudge_allowed, bool avoida
 // return distance (in meters) to destination
 float ModeAuto::get_distance_to_destination() const
 {
-    if (_submode == Auto_RTL) {
+    switch (_submode) {
+    case Auto_WP:
+        return _distance_to_destination;
+    case Auto_HeadingAndSpeed:
+        // no valid distance so return zero
+        return 0.0f;
+    case Auto_RTL:
         return rover.mode_rtl.get_distance_to_destination();
+    case Auto_Loiter:
+        return rover.mode_loiter.get_distance_to_destination();
+    case Auto_Guided:
+        return rover.mode_guided.get_distance_to_destination();
     }
-    return _distance_to_destination;
+
+    // this line should never be reached
+    return 0.0f;
 }
 
 // set desired location to drive to
@@ -138,7 +143,7 @@ bool ModeAuto::reached_destination() const
 {
     switch (_submode) {
     case Auto_WP:
-        return _reached_destination;
+        return g2.wp_nav.reached_destination();
         break;
     case Auto_HeadingAndSpeed:
         // always return true because this is the safer option to allow missions to continue
@@ -523,7 +528,7 @@ void ModeAuto::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd)
     }
 
     // set auto target
-    const float speed_max = get_speed_default();
+    const float speed_max = g2.wp_nav.get_default_speed();
     set_desired_heading_and_speed(desired_heading_cd, constrain_float(cmd.content.set_yaw_speed.speed, -speed_max, speed_max));
 }
 
@@ -595,7 +600,7 @@ bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
 
     // if a location target was set, return true once vehicle is close
     if (guided_target.valid) {
-        if (rover.current_loc.get_distance(guided_target.loc) <= rover.g.waypoint_radius) {
+        if (rover.current_loc.get_distance(guided_target.loc) <= rover.g2.wp_nav.get_radius()) {
             return true;
         }
     }

APMrover2/mode_guided.cpp

@@ -3,12 +3,16 @@
 
 bool ModeGuided::_enter()
 {
+    // set desired location to reasonable stopping point
+    if (!g2.wp_nav.set_desired_location_to_stopping_location()) {
+        return false;
+    }
+    _guided_mode = Guided_WP;
+
     // initialise waypoint speed
-    set_desired_speed_to_default();
+    g2.wp_nav.set_desired_speed_to_default();
 
-    // set desired location to reasonable stopping point
+    sent_notification = false;
-    calc_stopping_location(_destination);
-    set_desired_location(_destination);
 
     return true;
 }
@@ -18,19 +22,17 @@ void ModeGuided::update()
     switch (_guided_mode) {
         case Guided_WP:
         {
-            _distance_to_destination = rover.current_loc.get_distance(_destination);
-            const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius;
             // check if we've reached the destination
-            if (!_reached_destination && (near_wp || rover.current_loc.past_interval_finish_line(_origin, _destination))) {
+            if (!g2.wp_nav.reached_destination()) {
-                _reached_destination = true;
+                // update navigation controller
-                rover.gcs().send_mission_item_reached_message(0);
+                navigate_to_waypoint();
-            }
-            // determine if we should keep navigating
-            if (!_reached_destination) {
-                // drive towards destination
-                calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed);
-                calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
             } else {
+                // send notification
+                if (!sent_notification) {
+                    sent_notification = true;
+                    rover.gcs().send_mission_item_reached_message(0);
+                }
+
                 // we have reached the destination so stay here
                 if (rover.is_boat()) {
                     if (!start_loiter()) {
@@ -39,6 +41,8 @@ void ModeGuided::update()
                 } else {
                     stop_vehicle();
                 }
+                // update distance to destination
+                _distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
             }
             break;
         }
@@ -110,10 +114,19 @@ void ModeGuided::update()
 // return distance (in meters) to destination
 float ModeGuided::get_distance_to_destination() const
 {
-    if (_guided_mode != Guided_WP || _reached_destination) {
+    switch (_guided_mode) {
+    case Guided_WP:
+        return _distance_to_destination;
+    case Guided_HeadingAndSpeed:
+    case Guided_TurnRateAndSpeed:
         return 0.0f;
+    case Guided_Loiter:
+        rover.mode_loiter.get_distance_to_destination();
+        break;
     }
-    return _distance_to_destination;
+
+    // we should never reach here but just in case, return 0
+    return 0.0f;
 }
 
 // return true if vehicle has reached or even passed destination
@@ -122,12 +135,10 @@ bool ModeGuided::reached_destination() const
     switch (_guided_mode) {
     case Guided_WP:
         return _reached_destination;
-        break;
     case Guided_HeadingAndSpeed:
     case Guided_TurnRateAndSpeed:
     case Guided_Loiter:
         return true;
-        break;
     }
 
     // we should never reach here but just in case, return true is the safer option
@@ -138,12 +149,12 @@ bool ModeGuided::reached_destination() const
 void ModeGuided::set_desired_location(const struct Location& destination,
                                       float next_leg_bearing_cd)
 {
-    // call parent
+    if (g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd)) {
-    Mode::set_desired_location(destination, next_leg_bearing_cd);
 
-    // handle guided specific initialisation and logging
+        // handle guided specific initialisation and logging
-    _guided_mode = ModeGuided::Guided_WP;
+        _guided_mode = ModeGuided::Guided_WP;
-    rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_destination.lat, _destination.lng, 0), Vector3f(_desired_speed, 0.0f, 0.0f));
+        rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_desired_speed(), 0.0f, 0.0f));
+    }
 }
 
 // set desired attitude

APMrover2/mode_rtl.cpp

@@ -9,37 +9,47 @@ bool ModeRTL::_enter()
     }
 
     // initialise waypoint speed
-    set_desired_speed_to_default(true);
+    if (is_positive(g2.rtl_speed)) {
+        g2.wp_nav.set_desired_speed(g2.rtl_speed);
+    } else {
+        g2.wp_nav.set_desired_speed_to_default();
+    }
 
     // set target to the closest rally point or home
 #if AP_RALLY == ENABLED
-    set_desired_location(rover.g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt));
+    g2.wp_nav.set_desired_location(g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt));
 #else
     // set destination
-    set_desired_location(rover.home);
+    g2.wp_nav.set_desired_location(rover.home);
 #endif
 
+    sent_notification = false;
+
     return true;
 }
 
 void ModeRTL::update()
 {
-    // calculate distance to home
-    _distance_to_destination = rover.current_loc.get_distance(_destination);
-    const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius;
-    // check if we've reached the destination
-    if (!_reached_destination && (near_wp || rover.current_loc.past_interval_finish_line(_origin, _destination))) {
-        // trigger reached
-        _reached_destination = true;
-        gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
-    }
     // determine if we should keep navigating
-    if (!_reached_destination || (rover.is_boat() && !near_wp)) {
+    if (!g2.wp_nav.reached_destination() || rover.is_boat()) {
-        // continue driving towards destination
+        // update navigation controller
-        calc_steering_to_waypoint(_reached_destination ? rover.current_loc :_origin, _destination, _reversed);
+        navigate_to_waypoint();
-        calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
     } else {
+        // send notification
+        if (!sent_notification) {
+            sent_notification = true;
+            gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
+        }
+
         // we've reached destination so stop
         stop_vehicle();
+
+        // update distance to destination
+        _distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
     }
 }
+
+bool ModeRTL::reached_destination() const
+{
+    return g2.wp_nav.reached_destination();
+}

APMrover2/mode_smart_rtl.cpp

@@ -14,15 +14,20 @@ bool ModeSmartRTL::_enter()
         return false;
     }
 
-    // initialise waypoint speed
+    // set desired location to reasonable stopping point
-    set_desired_speed_to_default(true);
+    if (!g2.wp_nav.set_desired_location_to_stopping_location()) {
+        return false;
+    }
 
-    // init location target
+    // initialise waypoint speed
-    set_desired_location(rover.current_loc);
+    if (is_positive(g2.rtl_speed)) {
+        g2.wp_nav.set_desired_speed(g2.rtl_speed);
+    } else {
+        g2.wp_nav.set_desired_speed_to_default();
+    }
 
     // init state
     smart_rtl_state = SmartRTL_WaitForPathCleanup;
-    _reached_destination = false;
 
     return true;
 }
@@ -52,20 +57,19 @@ void ModeSmartRTL::update()
                 }
                 _load_point = false;
                 // set target destination to new point
-                if (!set_desired_location_NED(next_point)) {
+                if (!g2.wp_nav.set_desired_location_NED(next_point)) {
                     // this failure should never happen but we add it just in case
                     gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
                     smart_rtl_state = SmartRTL_Failure;
                 }
             }
+            // update navigation controller
+            navigate_to_waypoint();
+
             // check if we've reached the next point
-            _distance_to_destination = rover.current_loc.get_distance(_destination);
+            if (g2.wp_nav.reached_destination()) {
-            if (_distance_to_destination <= rover.g.waypoint_radius || rover.current_loc.past_interval_finish_line(_origin, _destination)) {
                 _load_point = true;
             }
-            // continue driving towards destination
-            calc_steering_to_waypoint(_origin, _destination, _reversed);
-            calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
             break;
 
         case SmartRTL_StopAtHome:
@@ -73,8 +77,7 @@ void ModeSmartRTL::update()
             _reached_destination = true;
             if (rover.is_boat()) {
                 // boats attempt to hold position at home
-                calc_steering_to_waypoint(rover.current_loc, _destination, _reversed);
+                navigate_to_waypoint();
-                calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
             } else {
                 // rovers stop
                 stop_vehicle();

APMrover2/sailboat.cpp

@@ -67,7 +67,7 @@ float Rover::sailboat_get_VMG() const
     if (!g2.attitude_control.get_forward_speed(speed)) {
         return 0.0f;
     }
-    return (speed * cosf(wrap_PI(radians(nav_controller->target_bearing_cd()) - ahrs.yaw)));
+    return (speed * cosf(wrap_PI(radians(g2.wp_nav.wp_bearing_cd() * 0.01f) - ahrs.yaw)));
 }
 
 // handle user initiated tack while in acro mode
@@ -151,14 +151,14 @@ float Rover::sailboat_calc_heading(float desired_heading_cd)
 
     // trigger tack if cross track error larger than waypoint_overshoot parameter
     // this effectively defines a 'corridor' of width 2*waypoint_overshoot that the boat will stay within
-    if ((fabsf(rover.nav_controller->crosstrack_error()) >= g.waypoint_overshoot) && !is_zero(g.waypoint_overshoot) && !sailboat_tacking()) {
+    if ((fabsf(g2.wp_nav.crosstrack_error()) >= g2.wp_nav.get_overshoot()) && !is_zero(g2.wp_nav.get_overshoot()) && !sailboat_tacking()) {
         // make sure the new tack will reduce the cross track error
         // if were on starboard tack we are traveling towards the left hand boundary
-        if (is_positive(rover.nav_controller->crosstrack_error()) && (current_tack == TACK_STARBOARD)) {
+        if (is_positive(g2.wp_nav.crosstrack_error()) && (current_tack == TACK_STARBOARD)) {
             should_tack = true;
         }
         // if were on port tack we are traveling towards the right hand boundary
-        if (is_negative(rover.nav_controller->crosstrack_error()) && (current_tack == TACK_PORT)) {
+        if (is_negative(g2.wp_nav.crosstrack_error()) && (current_tack == TACK_PORT)) {
             should_tack = true;
         }
     }