Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Randy Mackay 2018-04-21 15:39:49 +09:00
parent da5df5ca20
commit 365e1030db
2 changed files with 21 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -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)
{

4
libraries/APM_Control/AR_AttitudeControl.h
View File

@ -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);