Rover: mode refactoring

add ahrs reference
add set-desired-location method
move _reached_destination member in from child
calc_lateral_acceleration args renamed and added comemnts
calc_lateral_acceleration updates _yaw_error_cd
remove calc_lateral_acceleration method with no arguments
calc_throttle updates _speed_error and becomes protected
remove unused variables from calc_throttle
calc_reduced_speed_for_turn_or_distance reworked
do not use rover throttle or rtl_complete
calc_nav_steer comment updates
remove unused update_navigation

APMrover2/mode.cpp

@@ -2,6 +2,7 @@
 #include "Rover.h"
 
 Mode::Mode() :
+    ahrs(rover.ahrs),
     g(rover.g),
     g2(rover.g2),
     channel_steer(rover.channel_steer),
@@ -15,12 +16,10 @@ void Mode::exit()
     _exit();
 
     lateral_acceleration = 0.0f;
-    rover.throttle = 500;
     rover.g.pidSpeedThrottle.reset_I();
     if (!rover.in_auto_reverse) {
         rover.set_reverse(false);
     }
-    rover.rtl_complete = false;
 }
 
 bool Mode::enter()
@@ -29,51 +28,42 @@ bool Mode::enter()
     return _enter();
 }
 
+// set desired location
+void Mode::set_desired_location(const struct Location& destination)
+{
+    // record targets
+    _origin = rover.current_loc;
+    _destination = destination;
+    _desired_speed = g.speed_cruise;
+
+    // initialise distance
+    _distance_to_destination = get_distance(rover.current_loc, _destination);
+    _reached_destination = false;
+}
+
+// set desired heading and speed
+void Mode::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
+{
+    // handle initialisation
+    _reached_destination = false;
+
+    // record targets
+    _desired_yaw_cd = yaw_angle_cd;
+    _desired_speed = target_speed;
+}
+
 void Mode::calc_throttle(float target_speed)
 {
-    int16_t &throttle = rover.throttle;
+    // get ground speed from vehicle
-    const int32_t next_navigation_leg_cd = rover.next_navigation_leg_cd;
+    const float &groundspeed = rover.ground_speed;
-    const AP_AHRS &ahrs = rover.ahrs;
+
-    const float wp_distance = rover.wp_distance;
+    // calculate ground speed and ground speed error
-    float &groundspeed_error = rover.groundspeed_error;
+    _speed_error = fabsf(target_speed) - groundspeed;
-    const float ground_speed = rover.ground_speed;
 
     const float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise;
-    const int throttle_target = throttle_base + calc_throttle_nudge();
+    const float throttle_target = throttle_base + calc_throttle_nudge();
 
-    /*
+    float throttle = throttle_target + (g.pidSpeedThrottle.get_pid(_speed_error * 100.0f) / 100.0f);
-        reduce target speed in proportion to turning rate, up to the
-        SPEED_TURN_GAIN percentage.
-    */
-    float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g * GRAVITY_MSS));
-    steer_rate = constrain_float(steer_rate, 0.0f, 1.0f);
-
-    // use g.speed_turn_gain for a 90 degree turn, and in proportion
-    // for other turn angles
-    const int32_t turn_angle = wrap_180_cd(next_navigation_leg_cd - ahrs.yaw_sensor);
-    const float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0.0f, 1.0f);
-    const float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f;
-
-    float reduction = 1.0f - steer_rate * speed_turn_reduction;
-
-    if (is_autopilot_mode() && rover.mode_guided.guided_mode != ModeGuided::Guided_Velocity && wp_distance <= g.speed_turn_dist) {
-        // in auto-modes we reduce speed when approaching waypoints
-        const float reduction2 = 1.0f - speed_turn_reduction;
-        if (reduction2 < reduction) {
-            reduction = reduction2;
-        }
-    }
-
-    // reduce the target speed by the reduction factor
-    target_speed *= reduction;
-
-    groundspeed_error = fabsf(target_speed) - ground_speed;
-
-    throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100.0f) / 100.0f);
-
-    // also reduce the throttle by the reduction factor. This gives a
-    // much faster response in turns
-    throttle *= reduction;
 
     if (rover.in_reverse) {
         g2.motors.set_throttle(constrain_int16(-throttle, -g.throttle_max, -g.throttle_min));
@@ -81,7 +71,7 @@ void Mode::calc_throttle(float target_speed)
         g2.motors.set_throttle(constrain_int16(throttle, g.throttle_min, g.throttle_max));
     }
 
-    if (!rover.in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) {
+    if (!rover.in_reverse && g.braking_percent != 0 && _speed_error < -g.braking_speederr) {
         // the user has asked to use reverse throttle to brake. Apply
         // it in proportion to the ground speed error, but only when
         // our ground speed error is more than BRAKING_SPEEDERR.
@@ -89,7 +79,7 @@ void Mode::calc_throttle(float target_speed)
         // We use a linear gain, with 0 gain at a ground speed error
         // of braking_speederr, and 100% gain when groundspeed_error
         // is 2*braking_speederr
-        const float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0.0f, 1.0f);
+        const float brake_gain = constrain_float(((-_speed_error)-g.braking_speederr)/g.braking_speederr, 0.0f, 1.0f);
         const int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
         g2.motors.set_throttle(constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min));
 
@@ -97,19 +87,8 @@ void Mode::calc_throttle(float target_speed)
         // go negative
         rover.set_reverse(true);
     }
-
-    if (rover.mode_guided.guided_mode != ModeGuided::Guided_Velocity) {
-        if (rover.use_pivot_steering()) {
-            // In Guided Velocity, only the steering input is used to calculate the pivot turn.
-            g2.motors.set_throttle(0.0f);
-        }
-    }
 }
 
-void Mode::calc_lateral_acceleration()
-{
-    calc_lateral_acceleration(rover.current_loc, rover.next_WP);
-}
 
 // calculate pilot input to nudge throttle up or down
 int16_t Mode::calc_throttle_nudge()
@@ -129,21 +108,59 @@ int16_t Mode::calc_throttle_nudge()
     return throttle_nudge;
 }
 
-/*
+// calculated a reduced speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint
- * Calculate desired turn angles (in medium freq loop)
+// should be called after calc_lateral_acceleration and before calc_throttle
- */
+// relies on these internal members being updated: lateral_acceleration, _yaw_error_cd, _distance_to_destination
-void Mode::calc_lateral_acceleration(const struct Location &last_WP, const struct Location &next_WP)
+float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed)
+{
+    // this method makes use the following internal variables
+    const float yaw_error_cd = _yaw_error_cd;
+    const float target_lateral_accel_G = lateral_acceleration;
+    const float distance_to_waypoint = _distance_to_destination;
+
+    // calculate the yaw_error_ratio which is the error (capped at 90degrees) expressed as a ratio (from 0 ~ 1)
+    float yaw_error_ratio = constrain_float(fabsf(yaw_error_cd / 9000.0f), 0.0f, 1.0f);
+
+    // apply speed_turn_gain parameter (expressed as a percentage) to yaw_error_ratio
+    yaw_error_ratio *= (100 - g.speed_turn_gain) * 0.01f;
+
+    // calculate absolute lateral acceleration expressed as a ratio (from 0 ~ 1) of the vehicle's maximum lateral acceleration
+    float lateral_accel_ratio = constrain_float(fabsf(target_lateral_accel_G / (g.turn_max_g * GRAVITY_MSS)), 0.0f, 1.0f);
+
+    // calculate a lateral acceleration based speed scaling
+    float lateral_accel_speed_scaling = 1.0f - lateral_accel_ratio * yaw_error_ratio;
+
+    // calculate a pivot steering based speed scaling (default to no reduction)
+    float pivot_speed_scaling = 1.0f;
+    if (rover.use_pivot_steering(yaw_error_cd)) {
+        pivot_speed_scaling = 0.0f;
+    }
+
+    // calculate a waypoint distance based scaling (default to no reduction)
+    float distance_speed_scaling = 1.0f;
+    if (is_positive(distance_to_waypoint)) {
+        distance_speed_scaling = 1.0f - yaw_error_ratio;
+    }
+
+    // return minimum speed
+    return desired_speed * MIN(MIN(lateral_accel_speed_scaling, distance_speed_scaling), pivot_speed_scaling);
+}
+
+// calculate the lateral acceleration target to cause the vehicle to drive along the path from origin to destination
+// this function update lateral_acceleration and _yaw_error_cd members
+void Mode::calc_lateral_acceleration(const struct Location &origin, const struct Location &destination)
 {
     // Calculate the required turn of the wheels
     // negative error = left turn
     // positive error = right turn
-    rover.nav_controller->update_waypoint(last_WP, next_WP);
+    rover.nav_controller->update_waypoint(origin, destination);
     lateral_acceleration = rover.nav_controller->lateral_acceleration();
-    if (rover.use_pivot_steering()) {
+    _yaw_error_cd = wrap_180_cd(rover.nav_controller->target_bearing_cd() - ahrs.yaw_sensor);
-        const int16_t bearing_error = wrap_180_cd(rover.nav_controller->target_bearing_cd() - rover.ahrs.yaw_sensor) / 100;
+    if (rover.use_pivot_steering(_yaw_error_cd)) {
-        if (bearing_error > 0) {
+        if (is_positive(_yaw_error_cd)) {
             lateral_acceleration = g.turn_max_g * GRAVITY_MSS;
-        } else {
+        }
+        if (is_negative(_yaw_error_cd)) {
             lateral_acceleration = -g.turn_max_g * GRAVITY_MSS;
         }
     }
@@ -154,7 +171,7 @@ void Mode::calc_lateral_acceleration(const struct Location &last_WP, const struc
 */
 void Mode::calc_nav_steer()
 {
-    // add in obstacle avoidance
+    // add obstacle avoidance response to lateral acceleration target
     if (!rover.in_reverse) {
         lateral_acceleration += (rover.obstacle.turn_angle / 45.0f) * g.turn_max_g;
     }

APMrover2/mode.h

@@ -27,13 +27,6 @@ public:
     // convert user input to targets, implement high level control for this mode
     virtual void update() = 0;
 
-    // calculates the amount of throttle that should be output based
-    // on things like proximity to corners and current speed
-    virtual void calc_throttle(float target_speed);
-
-    // called to determine where the vehicle should go next, and how it should get there
-    virtual void update_navigation() { }  // most modes don't navigate
-
     //
     // attributes of the mode
     //
@@ -61,6 +54,22 @@ public:
     // true if heading is controlled
     virtual bool attitude_stabilized() const { return true; }
 
+    //
+    // navigation methods
+    //
+
+    // return distance (in meters) to destination
+    virtual float get_distance_to_destination() const { return 0.0f; }
+
+    // set desired location and speed
+    virtual void set_desired_location(const struct Location& destination);
+    // 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() { return true; }
+    virtual void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed);
+
+    // get speed error in m/s, returns zero for modes that do not control speed
+    float speed_error() { return _speed_error; }
+
     // Navigation control variables
     // The instantaneous desired lateral acceleration in m/s/s
     float lateral_acceleration;
@@ -76,21 +85,38 @@ protected:
     // calculate steering angle given a desired lateral acceleration
     virtual void calc_nav_steer();
 
-    // calculate desired lateral acceleration using current location and target held in next_WP
-    virtual void calc_lateral_acceleration();
-
     // calculate desired lateral acceleration
-    void calc_lateral_acceleration(const struct Location &last_wp, const struct Location &next_WP);
+    void calc_lateral_acceleration(const struct Location &origin, const struct Location &destination);
+
+    // calculates the amount of throttle that should be output based
+    // on things like proximity to corners and current speed
+    virtual void calc_throttle(float target_speed);
 
     // calculate pilot input to nudge throttle up or down
     int16_t calc_throttle_nudge();
 
+    // 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_lateral_acceleration 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);
+
     // references to avoid code churn:
+    class AP_AHRS &ahrs;
     class Parameters &g;
     class ParametersG2 &g2;
     class RC_Channel *&channel_steer;  // TODO : Pointer reference ?
     class RC_Channel *&channel_throttle;
     class AP_Mission &mission;
+
+    // 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 _speed_error;         // ground speed error in m/s
 };