From 51542f90535b16d41bd972087d1263c29d8dd22a Mon Sep 17 00:00:00 2001 From: Brandon Danyluk Date: Thu, 18 Mar 2021 23:28:55 -0600 Subject: [PATCH] Basic proportional navigation and PID controller --- .../functions/fnc_changeMissileDirection.sqf | 11 +- .../functions/fnc_guidancePFH.sqf | 100 ++++++++++++------ .../missileguidance/functions/fnc_onFired.sqf | 5 +- 3 files changed, 75 insertions(+), 41 deletions(-) diff --git a/addons/missileguidance/functions/fnc_changeMissileDirection.sqf b/addons/missileguidance/functions/fnc_changeMissileDirection.sqf index d560f05e2a..5d9c8f0e38 100644 --- a/addons/missileguidance/functions/fnc_changeMissileDirection.sqf +++ b/addons/missileguidance/functions/fnc_changeMissileDirection.sqf @@ -16,10 +16,11 @@ * Public: No */ -params ["_projectile", "_v"]; +params ["_projectile", "_pitch", "_yaw", "_roll"]; + +private _dir = [sin _yaw * cos _pitch, cos _yaw * cos _pitch, sin _pitch]; +private _up = [[sin _roll, -sin _pitch, cos _roll * cos _pitch], -_yaw] call BIS_fnc_rotateVector2D; + +_projectile setVectorDirAndUp [_dir, _up]; -private _l = sqrt ((_v select 0) ^ 2 + (_v select 1) ^ 2); -private _r = -(_v select 2) / _l; -_projectile setVectorDirAndUp [ _v, [(_v select 0) * _r,(_v select 1) * _r, _l] ]; -_projectile setVelocity (_v vectorMultiply (vectorMagnitude (velocity _projectile))); diff --git a/addons/missileguidance/functions/fnc_guidancePFH.sqf b/addons/missileguidance/functions/fnc_guidancePFH.sqf index 8050dfcff5..c83bbc6756 100644 --- a/addons/missileguidance/functions/fnc_guidancePFH.sqf +++ b/addons/missileguidance/functions/fnc_guidancePFH.sqf @@ -24,7 +24,7 @@ params ["_args", "_pfID"]; _args params ["_firedEH", "_launchParams", "_flightParams", "_seekerParams", "_stateParams"]; _firedEH params ["_shooter","","","","_ammo","","_projectile"]; _launchParams params ["","_targetLaunchParams"]; -_stateParams params ["_lastRunTime", "_seekerStateParams", "_attackProfileStateParams", "_lastKnownPosState"]; +_stateParams params ["_lastRunTime", "_seekerStateParams", "_attackProfileStateParams", "_lastKnownPosState", "_pidData"]; if (!alive _projectile || isNull _projectile || isNull _shooter) exitWith { [_pfID] call CBA_fnc_removePerFrameHandler; @@ -57,46 +57,76 @@ private _profileAdjustedTargetPos = [_seekerTargetPos, _args, _attackProfileStat // If we have no seeker target, then do not change anything // If there is no deflection on the missile, this cannot change and therefore is redundant. Avoid calculations for missiles without any deflection if ((_minDeflection != 0 || {_maxDeflection != 0}) && {_profileAdjustedTargetPos isNotEqualTo [0,0,0]}) then { + // Get a commanded acceleration via proportional navigation (https://youtu.be/Osb7anMm1AY) + // Use a simple PID controller to get the desired pitch, yaw, and roll + // Simulate moving servos by moving in each DOF by a fixed amount per frame + // Then setVectorDirAndUp to allow ARMA to translate the velocity to whatever PhysX says - private _targetVector = _projectilePos vectorFromTo _profileAdjustedTargetPos; - private _adjustVector = _targetVector vectorDiff (vectorDir _projectile); - _adjustVector params ["_adjustVectorX", "_adjustVectorY", "_adjustVectorZ"]; + private _rollDegreesPerSecond = 15; + private _yawDegreesPerSecond = 15; + private _pitchDegreesPerSecond = 15; - private _yaw = 0; - private _pitch = 0; - private _roll = 0; + private _proportionalGain = 1.6; + private _integralGain = 0; + private _derivativeGain = 0; - if (_adjustVectorX < 0) then { - _yaw = - ( (_minDeflection max ((abs _adjustVectorX) min _maxDeflection) ) ); - } else { - if (_adjustVectorX > 0) then { - _yaw = ( (_minDeflection max (_adjustVectorX min _maxDeflection) ) ); - }; - }; - if (_adjustVectorY < 0) then { - _roll = - ( (_minDeflection max ((abs _adjustVectorY) min _maxDeflection) ) ); - } else { - if (_adjustVectorY > 0) then { - _roll = ( (_minDeflection max (_adjustVectorY min _maxDeflection) ) ); - }; - }; - if (_adjustVectorZ < 0) then { - _pitch = - ( (_minDeflection max ((abs _adjustVectorZ) min _maxDeflection) ) ); - } else { - if (_adjustVectorZ > 0) then { - _pitch = ( (_minDeflection max (_adjustVectorZ min _maxDeflection) ) ); - }; - }; - private _finalAdjustVector = [_yaw, _roll, _pitch]; + _pidData params ["_pid", "_lastTargetPosition", "_lastLineOfSight", "_currentPitchYawRoll"]; + _currentPitchYawRoll params ["_pitch", "_yaw", "_roll"]; - TRACE_3("", _pitch, _yaw, _roll); - TRACE_3("", _targetVector, _adjustVector, _finalAdjustVector); + private _navigationGain = 3; + + private _lineOfSight = (_projectile vectorWorldToModelVisual (_profileAdjustedTargetPos vectorDiff _projectilePos)); + + private _losDelta = _lineOfSight vectorDiff _lastLineOfSight; + private _losRate = (vectorMagnitude _losDelta) / _runtimeDelta; + private _closingVelocity = -_losRate; + + private _commandedLateralAcceleration = _navigationGain * _losRate * _closingVelocity; + + private _commandedAcceleration = [_lineOfSight#2, -(_lineOfSight#0), 0] vectorMultiply _commandedLateralAcceleration; + + private _acceleration = [0, 0]; + { + (_pid select _forEachIndex) params ["", "_lastDerivative", "_integral"]; + // think about this in xz plane where x = yaw, z = pitch + + private _commandedAccelerationAxis = _commandedAcceleration select _forEachIndex; + + private _proportional = _commandedAccelerationAxis * _proportionalGain; + + private _d0 = _commandedAccelerationAxis * _derivativeGain; + private _derivative = (_d0 - _lastDerivative) / _runtimeDelta; + + _integral = _integral + (_d0 * _runtimeDelta * _integralGain); + + private _pidSum = _proportional + _integral + _derivative; + + (_pid select _forEachIndex) set [1, _d0]; + (_pid select _forEachIndex) set [2, _integral]; + + _acceleration set [_forEachIndex, _pidSum]; + } forEach _acceleration; if (accTime > 0) then { - private _changeVector = (vectorDir _projectile) vectorAdd _finalAdjustVector; - TRACE_2("",_projectile,_changeVector); - [_projectile, _changeVector] call FUNC(changeMissileDirection); + _acceleration params ["_pitchChange", "_yawChange"]; + + private _clampedPitch = (-_pitchChange min _pitchDegreesPerSecond) max -_pitchDegreesPerSecond; + private _clampedYaw = (_yawChange min _yawDegreesPerSecond) max -_yawDegreesPerSecond; + + _pitch = _pitch + _clampedPitch * _runtimeDelta; + _yaw = _yaw + _clampedYaw * _runtimeDelta; + + [_projectile, _pitch, _yaw, 0] call FUNC(changeMissileDirection); + + _currentPitchYawRoll set [0, _pitch]; + _currentPitchYawRoll set [1, _yaw]; }; + + _pidData set [0, _pid]; + _pidData set [1, _profileAdjustedTargetPos]; + _pidData set [2, _lineOfSight]; + _pidData set [3, _currentPitchYawRoll]; + _stateParams set [4, _pidData]; }; #ifdef DRAW_GUIDANCE_INFO @@ -105,7 +135,7 @@ drawIcon3D ["\a3\ui_f\data\IGUI\Cfg\Cursors\selectover_ca.paa", [1,0,0,1], ASLto private _ps = "#particlesource" createVehicleLocal (ASLtoAGL _projectilePos); _PS setParticleParams [["\A3\Data_f\cl_basic", 8, 3, 1], "", "Billboard", 1, 3.0141, [0, 0, 2], [0, 0, 0], 1, 1.275, 1, 0, [1, 1], [[1, 0, 0, 1], [1, 0, 0, 1], [1, 0, 0, 1]], [1], 1, 0, "", "", nil]; -_PS setDropInterval 3.0; +_PS setDropInterval 1.0; #endif _stateParams set [0, diag_tickTime]; diff --git a/addons/missileguidance/functions/fnc_onFired.sqf b/addons/missileguidance/functions/fnc_onFired.sqf index 422639c452..de5531c5e0 100644 --- a/addons/missileguidance/functions/fnc_onFired.sqf +++ b/addons/missileguidance/functions/fnc_onFired.sqf @@ -95,6 +95,9 @@ if (_seekLastTargetPos && {!isNil "_target"}) then { _lastKnownPosState set [1, [0,0,0]]; }; +private _pitchYaw = (vectorDir _projectile) call CBA_fnc_vect2Polar; +private _pidData = [[[0, 0, 0], [0, 0, 0], [0, 0, 0]], [0, 0, 0], [0, 1, 0], [_pitchYaw select 0, _pitchYaw select 1, 0]]; + TRACE_4("Beginning ACE guidance system",_target,_ammo,_seekerType,_attackProfile); private _args = [_this, [ _shooter, @@ -115,7 +118,7 @@ private _args = [_this, getNumber ( _config >> "seekerMaxRange" ), getNumber ( _config >> "seekerMinRange" ) ], - [ diag_tickTime, [], [], _lastKnownPosState] + [ diag_tickTime, [], [], _lastKnownPosState, _pidData] ]; private _onFiredFunc = getText (configFile >> QGVAR(SeekerTypes) >> _seekerType >> "onFired");