vehicles - update autothrottle to use future CBA pid component

addons/main/script_mod.hpp

@@ -11,7 +11,7 @@
 
 // MINIMAL required version for the Mod. Components can specify others..
 #define REQUIRED_VERSION 2.20
-#define REQUIRED_CBA_VERSION {3,18,4}
+#define REQUIRED_CBA_VERSION {3,18,5}
 
 #ifndef COMPONENT_BEAUTIFIED
     #define COMPONENT_BEAUTIFIED COMPONENT

addons/vehicles/functions/fnc_autoThrottle.sqf

@@ -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]];
@@ -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 {
@@ -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;