Arma3_Epoch/ACE3
1
0
Fork 0
mirror of https://github.com/acemod/ACE3.git synced 2024-08-30 18:23:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

Basic proportional navigation and PID controller

Browse Source
This commit is contained in:
Brandon Danyluk 2021-03-18 23:28:55 -06:00
parent b167dff81a
commit 51542f9053
3 changed files with 75 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
addons/missileguidance/functions/fnc_changeMissileDirection.sqf
View File

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

100
addons/missileguidance/functions/fnc_guidancePFH.sqf
View File

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

5
addons/missileguidance/functions/fnc_onFired.sqf
View File

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