From c8f42273136af1224d5a9353faa90a3717690d80 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 5 Jun 2018 12:33:13 +0900 Subject: [PATCH] Rover: remove SPEED_TURN_GAIN Loiter was the only mode using this, replaced with hardcoded gain. The desired speed in Loiter also uses a hardcoded gain so this should work --- APMrover2/Parameters.cpp | 9 --------- APMrover2/Parameters.h | 3 +-- APMrover2/mode_loiter.cpp | 4 ++-- 3 files changed, 3 insertions(+), 13 deletions(-) diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 2552d62ca9..8739f97520 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -100,15 +100,6 @@ const AP_Param::Info Rover::var_info[] = { // @User: Standard GSCALAR(speed_cruise, "CRUISE_SPEED", CRUISE_SPEED), - // @Param: SPEED_TURN_GAIN - // @DisplayName: Target speed reduction while turning - // @Description: The percentage to reduce the throttle to while turning. If this is 100% then the target speed is not reduced while turning. If this is 50% then the target speed is reduced in proportion to the turn rate, with a reduction of 50% when the steering is maximally deflected. - // @Units: % - // @Range: 0 100 - // @Increment: 1 - // @User: Standard - GSCALAR(speed_turn_gain, "SPEED_TURN_GAIN", 50), - // @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. diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index c194476082..8bdce61800 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -102,7 +102,7 @@ public: k_param_crosstrack_gain = 150, // unused k_param_crosstrack_entry_angle, // unused k_param_speed_cruise, - k_param_speed_turn_gain, + k_param_speed_turn_gain, // unused k_param_speed_turn_dist, // unused k_param_ch7_option, k_param_auto_trigger_pin, @@ -225,7 +225,6 @@ public: // navigation parameters // AP_Float speed_cruise; - AP_Int8 speed_turn_gain; AP_Int8 ch7_option; AP_Int8 auto_trigger_pin; AP_Float auto_kickstart; diff --git a/APMrover2/mode_loiter.cpp b/APMrover2/mode_loiter.cpp index cc55bbe0bf..b885781524 100644 --- a/APMrover2/mode_loiter.cpp +++ b/APMrover2/mode_loiter.cpp @@ -43,8 +43,8 @@ void ModeLoiter::update() } // reduce desired speed if yaw_error is large - // note: copied from calc_reduced_speed_for_turn_or_distance - float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * ((100.0f - g.speed_turn_gain) * 0.01f); + // 45deg of error reduces speed to 75%, 90deg of error reduces speed to 50% + float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * 0.5f; _desired_speed *= yaw_error_ratio; }