From 365e1030dbfbb46b668e55f1d74f88495d547d82 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 21 Apr 2018 15:39:49 +0900 Subject: [PATCH] AR_AttitudeControl: separate speed limiting to new method This allows the desired speed limiting (by acceleration) to be done before the speed-to-throttle PID controller is run. This is required so the avoidance calls (which work on the desired speed) can be run after limiting but before the PID controllers --- libraries/APM_Control/AR_AttitudeControl.cpp | 27 ++++++++++++-------- libraries/APM_Control/AR_AttitudeControl.h | 4 +++ 2 files changed, 21 insertions(+), 10 deletions(-) diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index 5127f69554..b81f487457 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -362,16 +362,6 @@ float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor } _speed_last_ms = now; - // acceleration limit desired speed - if (is_positive(_throttle_accel_max)) { - // reset desired speed to current speed on first iteration - if (!is_positive(dt)) { - desired_speed = speed; - } else { - const float speed_change_max = _throttle_accel_max * dt; - desired_speed = constrain_float(desired_speed, _desired_speed - speed_change_max, _desired_speed + speed_change_max); - } - } // record desired speed for next iteration _desired_speed = desired_speed; @@ -496,6 +486,23 @@ float AR_AttitudeControl::get_desired_speed() const return _desired_speed; } +// get acceleration limited desired speed +float AR_AttitudeControl::get_desired_speed_accel_limited(float desired_speed) const +{ + // return zero if no recent calls to speed controller + const uint32_t now = AP_HAL::millis(); + if ((_speed_last_ms == 0) || ((now - _speed_last_ms) > AR_ATTCONTROL_TIMEOUT_MS) || !is_positive(_throttle_accel_max)) { + return desired_speed; + } + + // calculate dt + float dt = (now - _speed_last_ms) / 1000.0f; + + // acceleration limit desired speed + const float speed_change_max = _throttle_accel_max * dt; + return constrain_float(desired_speed, _desired_speed - speed_change_max, _desired_speed + speed_change_max); +} + // get minimum stopping distance (in meters) given a speed (in m/s) float AR_AttitudeControl::get_stopping_distance(float speed) { diff --git a/libraries/APM_Control/AR_AttitudeControl.h b/libraries/APM_Control/AR_AttitudeControl.h index 57e78fdd72..82b37fd94e 100644 --- a/libraries/APM_Control/AR_AttitudeControl.h +++ b/libraries/APM_Control/AR_AttitudeControl.h @@ -73,6 +73,7 @@ public: void set_throttle_limits(float throttle_accel_max, float throttle_decel_max); // return a throttle output from -1 to +1 given a desired speed in m/s (use negative speeds to travel backwards) + // desired_speed argument should already have been passed through get_desired_speed_accel_limited function // motor_limit should be true if motors have hit their upper or lower limits // cruise speed should be in m/s, cruise throttle should be a number from -1 to +1 float get_throttle_out_speed(float desired_speed, bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle); @@ -94,6 +95,9 @@ public: // get latest desired speed recorded during call to get_throttle_out_speed. For reporting purposes only float get_desired_speed() const; + // get acceleration limited desired speed + float get_desired_speed_accel_limited(float desired_speed) const; + // get minimum stopping distance (in meters) given a speed (in m/s) float get_stopping_distance(float speed);