From 69541fc89cf8d0e94ca3c000bda6712f0a202dff Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 7 Aug 2018 16:15:05 +0900 Subject: [PATCH] AP_MotorsUGV: use wheel-rate-control for skid steering vehicles --- APMrover2/AP_MotorsUGV.cpp | 35 ++++++++++++++++++++++++++++++----- APMrover2/AP_MotorsUGV.h | 8 ++++++-- 2 files changed, 36 insertions(+), 7 deletions(-) diff --git a/APMrover2/AP_MotorsUGV.cpp b/APMrover2/AP_MotorsUGV.cpp index e1b8f1f645..a7ae887e93 100644 --- a/APMrover2/AP_MotorsUGV.cpp +++ b/APMrover2/AP_MotorsUGV.cpp @@ -321,7 +321,7 @@ void AP_MotorsUGV::output(bool armed, float ground_speed, float dt) output_regular(armed, ground_speed, _steering, _throttle); // output for skid steering style frames - output_skid_steering(armed, _steering, _throttle); + output_skid_steering(armed, _steering, _throttle, dt); // output for frames with vectored and custom motor configurations output_custom_config(armed, _steering, _throttle, _lateral); @@ -582,7 +582,7 @@ void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering } // output to skid steering channels -void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float throttle) +void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float throttle, float dt) { if (!have_skid_steering()) { return; @@ -626,8 +626,8 @@ void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float thrott const float motor_right = throttle_scaled - steering_scaled; // send pwm value to each motor - output_throttle(SRV_Channel::k_throttleLeft, 100.0f * motor_left); - output_throttle(SRV_Channel::k_throttleRight, 100.0f * motor_right); + output_throttle(SRV_Channel::k_throttleLeft, 100.0f * motor_left, dt); + output_throttle(SRV_Channel::k_throttleRight, 100.0f * motor_right, dt); } // output for custom configurations @@ -680,7 +680,7 @@ void AP_MotorsUGV::output_custom_config(bool armed, float steering, float thrott } // output throttle value to main throttle channel, left throttle or right throttle. throttle should be scaled from -100 to 100 -void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, float throttle) +void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, float throttle, float dt) { // sanity check servo function if (function != SRV_Channel::k_throttle && function != SRV_Channel::k_throttleLeft && function != SRV_Channel::k_throttleRight && function != SRV_Channel::k_motor1 && function != SRV_Channel::k_motor2 && function != SRV_Channel::k_motor3 && function!= SRV_Channel::k_motor4) { @@ -690,6 +690,9 @@ void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, f // constrain and scale output throttle = get_scaled_throttle(throttle); + // apply rate control + throttle = get_rate_controlled_throttle(function, throttle, dt); + // set relay if necessary if (_pwm_type == PWM_TYPE_BRUSHED_WITH_RELAY) { // find the output channel, if not found return @@ -794,6 +797,28 @@ float AP_MotorsUGV::get_scaled_throttle(float throttle) const return 100.0f * sign * ((_thrust_curve_expo - 1.0f) + safe_sqrt((1.0f - _thrust_curve_expo) * (1.0f - _thrust_curve_expo) + 4.0f * _thrust_curve_expo * fabsf(throttle_pct))) / (2.0f * _thrust_curve_expo); } +// use rate controller to achieve desired throttle +float AP_MotorsUGV::get_rate_controlled_throttle(SRV_Channel::Aux_servo_function_t function, float throttle, float dt) +{ + // require non-zero dt + if (!is_positive(dt)) { + return throttle; + } + + // attempt to rate control left throttle + if ((function == SRV_Channel::k_throttleLeft) && rover.get_wheel_rate_control().enabled(0)) { + return rover.get_wheel_rate_control().get_rate_controlled_throttle(0, throttle, dt); + } + + // rate control right throttle + if ((function == SRV_Channel::k_throttleRight) && rover.get_wheel_rate_control().enabled(1)) { + return rover.get_wheel_rate_control().get_rate_controlled_throttle(1, throttle, dt); + } + + // return throttle unchanged + return throttle; +} + // return true if motors are moving bool AP_MotorsUGV::active() const { diff --git a/APMrover2/AP_MotorsUGV.h b/APMrover2/AP_MotorsUGV.h index 91baea86ac..6be529b58f 100644 --- a/APMrover2/AP_MotorsUGV.h +++ b/APMrover2/AP_MotorsUGV.h @@ -119,13 +119,14 @@ protected: void output_regular(bool armed, float ground_speed, float steering, float throttle); // output to skid steering channels - void output_skid_steering(bool armed, float steering, float throttle); + void output_skid_steering(bool armed, float steering, float throttle, float dt); // output for vectored and custom motors configuration void output_custom_config(bool armed, float steering, float throttle, float lateral); // output throttle (-100 ~ +100) to a throttle channel. Sets relays if required - void output_throttle(SRV_Channel::Aux_servo_function_t function, float throttle); + // dt is the main loop time interval and is required when rate control is required + void output_throttle(SRV_Channel::Aux_servo_function_t function, float throttle, float dt = 0.0f); // slew limit throttle for one iteration void slew_limit_throttle(float dt); @@ -136,6 +137,9 @@ protected: // scale a throttle using the _thrust_curve_expo parameter. throttle should be in the range -100 to +100 float get_scaled_throttle(float throttle) const; + // use rate controller to achieve desired throttle + float get_rate_controlled_throttle(SRV_Channel::Aux_servo_function_t function, float throttle, float dt); + // external references AP_ServoRelayEvents &_relayEvents;