From eb519bfb79773b8be42fb1c90bb24b75772b0998 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 8 Jun 2018 11:03:44 +0900 Subject: [PATCH] AP_MotorsUGV: MOT_SPD_SCA_BASE param for min speed scaling This replaces the hardcoded 1m/s start point for speed scaling with a configurable value --- APMrover2/AP_MotorsUGV.cpp | 14 +++++++++++--- APMrover2/AP_MotorsUGV.h | 1 + 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/APMrover2/AP_MotorsUGV.cpp b/APMrover2/AP_MotorsUGV.cpp index 405d4d4ad6..93bb9c0fbd 100644 --- a/APMrover2/AP_MotorsUGV.cpp +++ b/APMrover2/AP_MotorsUGV.cpp @@ -88,6 +88,14 @@ const AP_Param::GroupInfo AP_MotorsUGV::var_info[] = { // @User: Advanced AP_GROUPINFO("VEC_THR_BASE", 10, AP_MotorsUGV, _vector_throttle_base, 0.0f), + // @Param: SPD_SCA_BASE + // @DisplayName: Motor speed scaling base speed + // @Description: Speed above which steering is scaled down when using regular steering/throttle vehicles. zero to disable speed scaling + // @Units: m/s + // @Range: 0 10 + // @User: Advanced + AP_GROUPINFO("SPD_SCA_BASE", 11, AP_MotorsUGV, _speed_scale_base, 1.0f), + AP_GROUPEND }; @@ -431,9 +439,9 @@ void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering steering *= constrain_float(_vector_throttle_base / fabsf(throttle), 0.0f, 1.0f); } } else { - // scale steering down as speed increase above 1m/s - if (fabsf(ground_speed) > 1.0f) { - steering *= (1.0f / fabsf(ground_speed)); + // scale steering down as speed increase above MOT_SPD_SCA_BASE (1 m/s default) + if (is_positive(_speed_scale_base) && (fabsf(ground_speed) > _speed_scale_base)) { + steering *= (_speed_scale_base / fabsf(ground_speed)); } else { // regular steering rover at low speed so set limits to stop I-term build-up in controllers if (!have_skid_steering()) { diff --git a/APMrover2/AP_MotorsUGV.h b/APMrover2/AP_MotorsUGV.h index bb7474d80d..a0832ea265 100644 --- a/APMrover2/AP_MotorsUGV.h +++ b/APMrover2/AP_MotorsUGV.h @@ -128,6 +128,7 @@ protected: AP_Int8 _throttle_max; // throttle maximum percentage AP_Float _thrust_curve_expo; // thrust curve exponent from -1 to +1 with 0 being linear AP_Float _vector_throttle_base; // throttle level above which steering is scaled down when using vector thrust. zero to disable vectored thrust + AP_Float _speed_scale_base; // speed above which steering is scaled down when using regular steering/throttle vehicles. zero to disable speed scaling // internal variables float _steering; // requested steering as a value from -4500 to +4500