From 2accb5831d4f9967b10a326e0133a120ea24f3d6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 3 Aug 2017 17:08:09 +0900 Subject: [PATCH] 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 | 145 +++++++++++++++++++++++++-------------------- APMrover2/mode.h | 48 +++++++++++---- 2 files changed, 118 insertions(+), 75 deletions(-) diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 20ef435981..4981318684 100644 --- a/APMrover2/mode.cpp +++ b/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(); } -void Mode::calc_throttle(float target_speed) +// set desired location +void Mode::set_desired_location(const struct Location& destination) { - int16_t &throttle = rover.throttle; - const int32_t next_navigation_leg_cd = rover.next_navigation_leg_cd; - const AP_AHRS &ahrs = rover.ahrs; - const float wp_distance = rover.wp_distance; - float &groundspeed_error = rover.groundspeed_error; - const float ground_speed = rover.ground_speed; + // 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; +} - const float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise; - const int throttle_target = throttle_base + calc_throttle_nudge(); - - /* - 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; - } - } +// set desired heading and speed +void Mode::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) +{ + // handle initialisation + _reached_destination = false; - // reduce the target speed by the reduction factor - target_speed *= reduction; + // record targets + _desired_yaw_cd = yaw_angle_cd; + _desired_speed = target_speed; +} - groundspeed_error = fabsf(target_speed) - ground_speed; +void Mode::calc_throttle(float target_speed) +{ + // get ground speed from vehicle + const float &groundspeed = rover.ground_speed; + + // calculate ground speed and ground speed error + _speed_error = fabsf(target_speed) - groundspeed; - throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100.0f) / 100.0f); + const float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise; + const float throttle_target = throttle_base + calc_throttle_nudge(); - // also reduce the throttle by the reduction factor. This gives a - // much faster response in turns - throttle *= reduction; + float throttle = throttle_target + (g.pidSpeedThrottle.get_pid(_speed_error * 100.0f) / 100.0f); 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; } -/* - * Calculate desired turn angles (in medium freq loop) - */ -void Mode::calc_lateral_acceleration(const struct Location &last_WP, const struct Location &next_WP) +// 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 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()) { - const int16_t bearing_error = wrap_180_cd(rover.nav_controller->target_bearing_cd() - rover.ahrs.yaw_sensor) / 100; - if (bearing_error > 0) { + _yaw_error_cd = wrap_180_cd(rover.nav_controller->target_bearing_cd() - ahrs.yaw_sensor); + if (rover.use_pivot_steering(_yaw_error_cd)) { + 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; } diff --git a/APMrover2/mode.h b/APMrover2/mode.h index b704edf4b2..578c0b824a 100644 --- a/APMrover2/mode.h +++ b/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 };