From c6689fd2e13bcfea8338d3c81ed4e68778511d2b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 27 Nov 2017 20:57:59 +0900 Subject: [PATCH] Rover: rename calc_nav_steer to calc_steering_from_lateral_acceleration rename calc_lateral_acceleration to calc_steering_to_waypoint calc_steering_to_waypoint internally calls calc_steering_from_lateral_acceleration non-functional change --- APMrover2/mode.cpp | 9 ++++++--- APMrover2/mode.h | 8 ++++---- APMrover2/mode_auto.cpp | 3 +-- APMrover2/mode_guided.cpp | 3 +-- APMrover2/mode_rtl.cpp | 3 +-- APMrover2/mode_steering.cpp | 4 ++-- 6 files changed, 15 insertions(+), 15 deletions(-) diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index c5c8729348..a29c90dc57 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -190,7 +190,7 @@ float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed) // 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, bool reversed) +void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed) { // Calculate the required turn of the wheels // negative error = left turn @@ -210,12 +210,15 @@ void Mode::calc_lateral_acceleration(const struct Location &origin, const struct lateral_acceleration = -g.turn_max_g * GRAVITY_MSS; } } + + // call lateral acceleration to steering controller + calc_steering_from_lateral_acceleration(reversed); } /* - calculate steering angle given lateral_acceleration + calculate steering output given lateral_acceleration */ -void Mode::calc_nav_steer(bool reversed) +void Mode::calc_steering_from_lateral_acceleration(bool reversed) { // add obstacle avoidance response to lateral acceleration target if (!reversed) { diff --git a/APMrover2/mode.h b/APMrover2/mode.h index 76b3914d33..a0efd9d780 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -92,10 +92,10 @@ protected: virtual void _exit() { return; } // calculate steering angle given a desired lateral acceleration - void calc_nav_steer(bool reversed = false); + void calc_steering_from_lateral_acceleration(bool reversed = false); - // calculate desired lateral acceleration - void calc_lateral_acceleration(const struct Location &origin, const struct Location &destination, bool reversed = false); + // calculate steering output to drive along line from origin to destination waypoint + void calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed = false); // calculates the amount of throttle that should be output based // on things like proximity to corners and current speed @@ -114,7 +114,7 @@ protected: 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_lateral_acceleration and before calc_throttle + // 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); diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp index 49b1a2bf70..23a1ccf26f 100644 --- a/APMrover2/mode_auto.cpp +++ b/APMrover2/mode_auto.cpp @@ -54,8 +54,7 @@ void ModeAuto::update() bool active_at_destination = _reached_destination && _stay_active_at_dest && (_distance_to_destination > rover.g.waypoint_radius); if (!_reached_destination || active_at_destination) { // continue driving towards destination - calc_lateral_acceleration(active_at_destination ? rover.current_loc : _origin, _destination, _reversed); - calc_nav_steer(_reversed); + calc_steering_to_waypoint(active_at_destination ? rover.current_loc : _origin, _destination, _reversed); calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true); } else { // we have reached the destination so stop diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp index b7301465f6..5728bc5334 100644 --- a/APMrover2/mode_guided.cpp +++ b/APMrover2/mode_guided.cpp @@ -30,8 +30,7 @@ void ModeGuided::update() rover.gcs().send_mission_item_reached_message(0); } // drive towards destination - calc_lateral_acceleration(_origin, _destination); - calc_nav_steer(); + calc_steering_to_waypoint(_origin, _destination); calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true); } else { stop_vehicle(); diff --git a/APMrover2/mode_rtl.cpp b/APMrover2/mode_rtl.cpp index 58c7e902a6..847dc0aed3 100644 --- a/APMrover2/mode_rtl.cpp +++ b/APMrover2/mode_rtl.cpp @@ -29,8 +29,7 @@ void ModeRTL::update() gcs().send_text(MAV_SEVERITY_INFO, "Reached destination"); } // continue driving towards destination - calc_lateral_acceleration(_origin, _destination); - calc_nav_steer(); + calc_steering_to_waypoint(_origin, _destination); calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true); } else { // we've reached destination so stop diff --git a/APMrover2/mode_steering.cpp b/APMrover2/mode_steering.cpp index 36b6daba4e..e7a847e1e7 100644 --- a/APMrover2/mode_steering.cpp +++ b/APMrover2/mode_steering.cpp @@ -49,8 +49,8 @@ void ModeSteering::update() if (is_zero(target_speed) && !is_pivot_turning) { stop_vehicle(); } else { - // run steering controller - calc_nav_steer(reversed); + // run lateral acceleration to steering controller + calc_steering_from_lateral_acceleration(false); calc_throttle(target_speed, false); } }