From 3db2cc700ea994e4edc38302a5645ba172ba48af Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 3 May 2018 17:54:13 +0900 Subject: [PATCH] AP_MotorsUGV: positive steering always rotates vehicle right also scale steering down with increased speed for regular rovers add support for disabling scaling of steering --- APMrover2/AP_MotorsUGV.cpp | 56 +++++++++++++++++++++++++++----------- APMrover2/AP_MotorsUGV.h | 11 ++++++-- 2 files changed, 48 insertions(+), 19 deletions(-) diff --git a/APMrover2/AP_MotorsUGV.cpp b/APMrover2/AP_MotorsUGV.cpp index 95df686f93..3002ead06d 100644 --- a/APMrover2/AP_MotorsUGV.cpp +++ b/APMrover2/AP_MotorsUGV.cpp @@ -12,6 +12,7 @@ */ #include +#include #include "SRV_Channel/SRV_Channel.h" #include "AP_MotorsUGV.h" #include "Rover.h" @@ -156,9 +157,12 @@ void AP_MotorsUGV::setup_servo_output() } // set steering as a value from -4500 to +4500 -void AP_MotorsUGV::set_steering(float steering) +// apply_scaling should be set to false for manual modes where +// no scaling by speed or angle should be performed +void AP_MotorsUGV::set_steering(float steering, bool apply_scaling) { _steering = constrain_float(steering, -4500.0f, 4500.0f); + _scale_steering = apply_scaling; } // set throttle as a value from -100 to 100 @@ -196,7 +200,7 @@ bool AP_MotorsUGV::is_omni_rover() const return false; } -void AP_MotorsUGV::output(bool armed, float dt) +void AP_MotorsUGV::output(bool armed, float ground_speed, float dt) { // soft-armed overrides passed in armed status if (!hal.util->get_soft_armed()) { @@ -214,7 +218,7 @@ void AP_MotorsUGV::output(bool armed, float dt) slew_limit_throttle(dt); // output for regular steering/throttle style frames - output_regular(armed, _steering, _throttle); + output_regular(armed, ground_speed, _steering, _throttle); // output for omni style frames output_omni(armed, _steering, _throttle); @@ -399,15 +403,38 @@ void AP_MotorsUGV::setup_pwm_type() } // output to regular steering and throttle channels -void AP_MotorsUGV::output_regular(bool armed, float steering, float throttle) +void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering, float throttle) { // output to throttle channels if (armed) { - // vectored thrust handling - if (have_vectored_thrust() && (fabsf(throttle) > _vector_throttle_base)) { - // scale steering down linearly as throttle increases above _vector_throttle_base - const float steering_scalar = constrain_float(_vector_throttle_base / fabsf(throttle), 0.0f, 1.0f); - steering *= steering_scalar; + if (_scale_steering) { + // vectored thrust handling + if (have_vectored_thrust()) { + if (fabsf(throttle) > _vector_throttle_base) { + // scale steering down linearly as throttle increases above _vector_throttle_base + 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)); + } else { + // regular steering rover at low speed so set limits to stop I-term build-up in controllers + if (!have_skid_steering()) { + limit.steer_left = true; + limit.steer_right = true; + } + } + // reverse steering output if backing up + if (is_negative(ground_speed)) { + steering *= -1.0f; + } + } + } else { + // reverse steering direction when backing up + if (is_negative(throttle)) { + steering *= -1.0f; + } } output_throttle(SRV_Channel::k_throttle, throttle); } else { @@ -439,12 +466,12 @@ void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle) const float magnitude = safe_sqrt((scaled_throttle*scaled_throttle)+(1500*1500)); const float theta = atan2f(scaled_throttle,1500); - // calculate X and Y vectors using the following the equations: vx = cos(θ) * magnitude and vy = sin(θ) * magnitude + // calculate X and Y vectors using the following the equations: vx = cos(?) * magnitude and vy = sin(?) * magnitude const float Vx = -(cosf(theta)*magnitude); const float Vy = -(sinf(theta)*magnitude); // calculate output throttle for each motor and scale it back to a -100 to 100 range - // calculations are done using the following equations: MOTOR1 = –vx, MOTOR2 = 0.5 * v – √(3/2) * vy, MOTOR 3 = 0.5 * vx + √(3/2) * vy + // calculations are done using the following equations: MOTOR1 = –vx, MOTOR2 = 0.5 * v – v(3/2) * vy, MOTOR 3 = 0.5 * vx + v(3/2) * vy // safe_sqrt((3)/2) used because the motors are 120 degrees apart in the frame, this setup is mandatory const int16_t motor_1 = (((-Vx) + scaled_steering) - (2500)) * (100 - (-100)) / (3500 - (2500)) + (-100); const int16_t motor_2 = ((((0.5*Vx)-((safe_sqrt(3)/2)*Vy)) + scaled_steering) - (1121)) * (100 - (-100)) / (2973 - (1121)) + (-100); @@ -502,12 +529,9 @@ void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float thrott throttle_scaled = throttle_scaled / saturation_value; } - // reverse steering direction if throttle is negative to mimic regular rovers - const float steering_dir = is_negative(throttle_scaled) ? -1.0f : 1.0f; - // add in throttle and steering - const float motor_left = throttle_scaled + (steering_dir * steering_scaled); - const float motor_right = throttle_scaled - (steering_dir * steering_scaled); + const float motor_left = throttle_scaled + steering_scaled; + const float motor_right = throttle_scaled - steering_scaled; // send pwm value to each motor output_throttle(SRV_Channel::k_throttleLeft, 100.0f * motor_left); diff --git a/APMrover2/AP_MotorsUGV.h b/APMrover2/AP_MotorsUGV.h index 87158fa8d7..ea53061bc9 100644 --- a/APMrover2/AP_MotorsUGV.h +++ b/APMrover2/AP_MotorsUGV.h @@ -35,8 +35,10 @@ public: void setup_servo_output(); // get or set steering as a value from -4500 to +4500 + // apply_scaling should be set to false for manual modes where + // no scaling by speed or angle should e performed float get_steering() const { return _steering; } - void set_steering(float steering); + void set_steering(float steering, bool apply_scaling = true); // get or set throttle as a value from -100 to 100 float get_throttle() const { return _throttle; } @@ -52,7 +54,9 @@ public: bool have_vectored_thrust() const { return is_positive(_vector_throttle_base); } // output to motors and steering servos - void output(bool armed, float dt); + // ground_speed should be the vehicle's speed over the surface in m/s + // dt should be expected time between calls to this function + void output(bool armed, float ground_speed, float dt); // test steering or throttle output as a percentage of the total (range -100 to +100) // used in response to DO_MOTOR_TEST mavlink command @@ -84,7 +88,7 @@ protected: void setup_pwm_type(); // output to regular steering and throttle channels - void output_regular(bool armed, float steering, float throttle); + void output_regular(bool armed, float ground_speed, float steering, float throttle); // output for omni style frames void output_omni(bool armed, float steering, float throttle); @@ -121,4 +125,5 @@ protected: float _steering; // requested steering as a value from -4500 to +4500 float _throttle; // requested throttle as a value from -100 to 100 float _throttle_prev; // throttle input from previous iteration + bool _scale_steering = true; // true if we should scale steering by speed or angle };