Skip to content
Merged
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
86 changes: 50 additions & 36 deletions addons/vehicles/functions/fnc_autoThrottle.sqf
Original file line number Diff line number Diff line change
Expand Up @@ -16,9 +16,6 @@
*
* Public: No
*/
#define PID_P 1
#define PID_I 0.3
#define PID_D 0
#define EPSILON 0.001

params ["_driver", "_vehicle", ["_preserveSpeedLimit", false]];
Expand All @@ -34,18 +31,37 @@ playSound "ACE_Sound_Click";
GVAR(isSpeedLimiter) = true;
GVAR(isCruiseControl) = true; // enables SET/RESUME

private _speedLimitMS = ((velocityModelSpace _vehicle) select 1) max (5 / 3.6);
if (!_preserveSpeedLimit) then {
// Convert forward speed to KM/H. `speed _vehicle` isnt accurate enough for this controller to work well, so its easier to use M/S. The system assumes it is KM/H so we need the conversion
GVAR(speedLimit) = (((velocityModelSpace _vehicle) select 1) * 3.6) max 5;
GVAR(speedLimit) = _speedLimitMS * 3.6;
};

private _pid = [4.00, 0.60, 2.50, _speedLimitMS, -1e30, 1e30, 60 * 3] call CBA_pid_fnc_create;

private _config = configOf _vehicle;
private _thrustLogFactor = getNumber (_config >> "throttleToThrustLogFactor");
if (_thrustLogFactor == 0) then {
_thrustLogFactor = 1;
};
private _maxSpeed = getNumber (_config >> "maxSpeed");
private _thrustCoefs = getArray (_config >> "thrustCoef");
private _acceleration = getNumber (_config >> "acceleration");
if (_acceleration == 0) then {
_acceleration = 100;
};

if (_thrustCoefs isEqualTo []) then {
_thrustCoefs = [1];
};
private _thrustCoefsAdjustment = 150 / count _thrustCoefs;

private _airData = [_thrustLogFactor, _maxSpeed, _acceleration, _thrustCoefs, _thrustCoefsAdjustment];

[{
params ["_args", "_idPFH"];
_args params ["_driver", "_vehicle", "_autothrottleParameters"];
_autothrottleParameters params ["_lastVelocity", "_integralValue", "_lastTime", "_lastThrottleValue", "_throttleLogValue"];

// this will take into account game being pausesd
private _deltaTime = CBA_missionTime - _lastTime;
_args params ["_driver", "_vehicle", "_pid", "_airData", "_lastThrottleValue"];
_airData params ["_throttleLogValue", "_maxSpeed", "_acceleration", "_thrustCoefs", "_thrustCoefsAdjustment"];

private _role = _driver call EFUNC(common,getUavControlPosition);
if (GVAR(isUAV)) then {
Expand Down Expand Up @@ -79,35 +95,33 @@ if (!_preserveSpeedLimit) then {
[_idPFH] call CBA_fnc_removePerFrameHandler;
};

if (_deltaTime == 0) exitWith {};

private _forwardVelocity = (velocityModelSpace _vehicle) select 1;
// convert from KM/H to M/S
private _velocityError = (GVAR(speedLimit) / 3.6) - _forwardVelocity;

// strictly speaking this would work better if this error was time to zero acceleration. I can't find the acceleration values in config, however, so this works instead
private _errorDiff = _velocityError - _lastVelocity;

private _p = PID_P * _velocityError;
private _i = _integralValue + (PID_I * _errorDiff * _deltaTime);
private _d = PID_D * _errorDiff / _deltaTime;

private _outputBeforeSaturation = _p + _i + _d;
private _throttle = 0 max (_outputBeforeSaturation min 1);

// if we are saturated, we clamp the integral value to avoid integral windup
if (_outputBeforeSaturation != _throttle) then {
// saturated
_i = _integralValue;
_throttle = 0 max ((_p + _d) min 1);
private _setpoint = _pid getVariable "CBA_pid_setpoint";
if (abs ((GVAR(speedLimit) / 3.6) - _setpoint) > EPSILON) then {
[_pid, GVAR(speedLimit) / 3.6] call CBA_pid_fnc_setpoint;
};
private _velocity = (velocityModelSpace _vehicle) select 1;
private _percentMaxSpeed = _velocity / _maxSpeed;
private _coefIdxBase = floor (100 * _percentMaxSpeed / _thrustCoefsAdjustment);
private _coefIdxNext = _coefIdxBase + 1;
_percentMaxSpeed = _percentMaxSpeed - floor _percentMaxSpeed;

private _thrustCoefBase = 1;
private _thrustCoefNext = 1;
if (_coefIdxBase < count _thrustCoefs) then {
_coefIdxNext = _coefIdxNext min (-1 + count _thrustCoefs);
_thrustCoefBase = _thrustCoefs select _thrustCoefBase;
_thrustCoefNext = _thrustCoefs select _thrustCoefNext;
};

_vehicle setAirplaneThrottle _throttle;
private _thrustCoef = linearConversion [0, 1, _percentMaxSpeed, _thrustCoefBase, _thrustCoefNext];
private _availableAcceleration = _thrustCoef * _acceleration;
private _currentAcceleration = _currentThrottle * _availableAcceleration;

_autothrottleParameters set [0, _d];
_autothrottleParameters set [1, _i];
_autothrottleParameters set [2, CBA_missionTime];
_autothrottleParameters set [3, _throttle];
private _velocityCommand = [_pid, _velocity] call CBA_pid_fnc_update;
private _throttle = 1 min (0 max (_currentThrottle + _velocityCommand / _availableAcceleration));

_vehicle setAirplaneThrottle _throttle;
_args set [4, _throttle];

}, 0, [_driver, _vehicle, [0, 0, CBA_missionTime, -1, getNumber (configOf _vehicle >> "throttleToThrustLogFactor")]]] call CBA_fnc_addPerFrameHandler;
}, 0, [_driver, _vehicle, _pid, _airData, -1]] call CBA_fnc_addPerFrameHandler;