From fd8a55addf9c315f73a1a4f43b1073cd84937f56 Mon Sep 17 00:00:00 2001 From: Brandon Danyluk Date: Sat, 10 Apr 2021 13:11:20 -0600 Subject: [PATCH] make navigation a modifyable system --- addons/missileguidance/ACE_GuidanceConfig.hpp | 9 ++ addons/missileguidance/CfgAmmo.hpp | 10 ++- addons/missileguidance/XEH_PREP.hpp | 6 ++ .../functions/fnc_guidancePFH.sqf | 88 +++++-------------- .../functions/fnc_navigationType_proNav.sqf | 51 +++++++++++ .../missileguidance/functions/fnc_onFired.sqf | 35 +++++--- .../functions/fnc_proNav_onFired.sqf | 38 ++++++++ 7 files changed, 157 insertions(+), 80 deletions(-) create mode 100644 addons/missileguidance/functions/fnc_navigationType_proNav.sqf create mode 100644 addons/missileguidance/functions/fnc_proNav_onFired.sqf diff --git a/addons/missileguidance/ACE_GuidanceConfig.hpp b/addons/missileguidance/ACE_GuidanceConfig.hpp index 8e3324b09d..4b22e639fb 100644 --- a/addons/missileguidance/ACE_GuidanceConfig.hpp +++ b/addons/missileguidance/ACE_GuidanceConfig.hpp @@ -91,3 +91,12 @@ class GVAR(SeekerTypes) { onFired = QFUNC(ahr_onFired); }; }; + +class GVAR(NavigationTypes) { + class ProportionalNavigation { + name = "Proportional Navigation"; + + functionName = QFUNC(navigationType_proNav); + onFired = QFUNC(proNav_onFired); + }; +}; diff --git a/addons/missileguidance/CfgAmmo.hpp b/addons/missileguidance/CfgAmmo.hpp index a2b07b7fff..5fa6d4b61f 100644 --- a/addons/missileguidance/CfgAmmo.hpp +++ b/addons/missileguidance/CfgAmmo.hpp @@ -69,9 +69,8 @@ class CfgAmmo { class ADDON { enabled = 1; - minDeflection = 0.00005; // Minium flap deflection for guidance - maxDeflection = 0.025; // Maximum flap deflection for guidance - incDeflection = 0.00005; // The incrmeent in which deflection adjusts. + pitchRate = 40; // degrees per second + yawRate = 40; canVanillaLock = 0; @@ -81,6 +80,11 @@ class CfgAmmo { defaultSeekerLockMode = "LOBL"; seekerLockModes[] = { "LOBL" }; + + defaultNavigationType = "ProportionalNavigation"; + navigationTypes[] = { "ProportionalNavigation" }; + + navigationGain = 3; seekerAngle = 180; // Angle in front of the missile which can be searched seekerAccuracy = 1; // seeker accuracy multiplier diff --git a/addons/missileguidance/XEH_PREP.hpp b/addons/missileguidance/XEH_PREP.hpp index 075f2f03c5..484929e541 100644 --- a/addons/missileguidance/XEH_PREP.hpp +++ b/addons/missileguidance/XEH_PREP.hpp @@ -29,6 +29,9 @@ PREP(attackProfile_BEAM); PREP(attackProfile_JAV_DIR); PREP(attackProfile_JAV_TOP); +// Navigation Profiles +PREP(navigationType_proNav); + // Seeker search functions PREP(seekerType_SALH); PREP(seekerType_Optic); @@ -41,3 +44,6 @@ PREP(wire_onFired); // Seeker OnFired PREP(SACLOS_onFired); PREP(ahr_onFired); + +// Navigation OnFired +PREP(proNav_onFired); diff --git a/addons/missileguidance/functions/fnc_guidancePFH.sqf b/addons/missileguidance/functions/fnc_guidancePFH.sqf index 27c116b852..c93362ed85 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", "_pidData"]; +_stateParams params ["_lastRunTime", "_seekerStateParams", "_attackProfileStateParams", "_lastKnownPosState", "_navigationParameters"]; if (!alive _projectile || isNull _projectile || isNull _shooter) exitWith { [_pfID] call CBA_fnc_removePerFrameHandler; @@ -42,9 +42,8 @@ if (accTime > 0) then { _adjustTime = 0; }; -private _minDeflection = ((_flightParams select 0) - ((_flightParams select 0) * _adjustTime)) max 0; -private _maxDeflection = (_flightParams select 1) * _adjustTime; -// private _incDeflection = _flightParams select 2; // todo +private _pitchRate = _flightParams select 0; +private _yawRate = _flightParams select 1; // Run seeker function: private _seekerTargetPos = [[0,0,0], _args, _seekerStateParams, _lastKnownPosState] call FUNC(doSeekerSearch); @@ -56,84 +55,43 @@ private _projectilePos = getPosASLVisual _projectile; // 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 _rollDegreesPerSecond = 60; - private _yawDegreesPerSecond = 60; - private _pitchDegreesPerSecond = 60; - - private _proportionalGain = 1; - private _integralGain = 0; - private _derivativeGain = 0; - - private _projectileVelocity = velocity _projectile; - private _projectileSpeed = vectorMagnitude _projectileVelocity; - - _pidData params ["_pid", "_lastMissileFrame", "_currentPitchYawRoll"]; - _currentPitchYawRoll params ["_pitch", "_yaw", "_roll"]; - _lastMissileFrame params ["_lastTargetPosition", "_lastTargetVelocity", "_lastLineOfSight"]; - - // Proportional navigation implemented via "Fundamentals of proportional navigation" by Stephen Murtaugh and Harry Criel - private _navigationGain = 3; - // integrate target velocity for realistic inference of velocity - private _targetVelocity = (_seekerTargetPos vectorDiff _lastTargetPosition) vectorMultiply (1 / diag_deltaTime); - private _targetAcceleration = (_targetVelocity vectorDiff _lastTargetVelocity) vectorMultiply (1 / diag_deltaTime); - - private _closingVelocity = _targetVelocity vectorDiff _projectileVelocity; - - private _lineOfSight = vectorNormalized (_profileAdjustedTargetPos vectorDiff _projectilePos); - - // the los rate is tiny, so we multiply by a constant of a power of ten to get more aggressive acceleration - // this is just due to how we measure our LOS delta, the vectors involved are _tiny_ - private _losDelta = _lineOfSight vectorDiff _lastLineOfSight; - private _losRate = 1000 * (vectorMagnitude _losDelta) / TIMESTEP_FACTOR; - - private _lateralAcceleration = (_navigationGain * _losRate); - private _commandedAcceleration = _closingVelocity vectorMultiply _lateralAcceleration; - - // we need acceleration normal to our LOS - private _commandedAccelerationProjected = _lineOfSight vectorMultiply (_commandedAcceleration vectorDotProduct _lineOfSight); - _commandedAcceleration = _commandedAcceleration vectorDiff _commandedAccelerationProjected; +if ((_pitchRate != 0 || {_yawRate != 0}) && {_profileAdjustedTargetPos isNotEqualTo [0,0,0]}) then { + private _commandedAcceleration = [_args, TIMESTEP_FACTOR, _seekerTargetPos, _profileAdjustedTargetPos] call FUNC(navigationType_proNav); #ifdef DRAW_GUIDANCE_INFO private _projectilePosAGL = ASLToAGL _projectilePos; drawIcon3D ["\a3\ui_f\data\IGUI\Cfg\Cursors\selectover_ca.paa", [1,0,0,1], _projectilePosAGL vectorAdd [0, 0, 1], 0.75, 0.75, 0, str _commandedAcceleration, 1, 0.025, "TahomaB"]; - drawIcon3D ["\a3\ui_f\data\IGUI\Cfg\Cursors\selectover_ca.paa", [1,0,0,1], ASLToAGL (_seekerTargetPos vectorAdd _targetVelocity), 0.75, 0.75, 0, "Predicted Position", 1, 0.025, "TahomaB"]; - drawLine3D [_projectilePosAGL, _projectilePosAGL vectorAdd _commandedAcceleration, [1, 0, 1, 1]]; - - private _seekerPosAGL = ASLToAGL _seekerTargetPos; - drawLine3D [_seekerPosAGL, _seekerPosAGL vectorAdd _targetVelocity, [0, 1, 1, 1]]; - drawLine3D [_seekerPosAGL, _seekerPosAGL vectorAdd (_projectilePos vectorDiff _seekerTargetPos), [0, 1, 1, 1]]; #endif + // activate missile servos and change direction if (!isGamePaused && accTime > 0) then { + _navigationParameters params ["", "_currentPitchYawRoll"]; + _currentPitchYawRoll params ["_yaw", "_roll", "_pitch"]; + _commandedAcceleration = _projectile vectorWorldToModelVisual _commandedAcceleration; _commandedAcceleration params ["_yawChange", "", "_pitchChange"]; - private _clampedPitch = (_pitchChange min _pitchDegreesPerSecond) max -_pitchDegreesPerSecond; - private _clampedYaw = (_yawChange min _yawDegreesPerSecond) max -_yawDegreesPerSecond; + private _clampedPitch = (_pitchChange min _pitchRate) max -_pitchRate; + private _clampedYaw = (_yawChange min _yawRate) max -_yawRate; - _pitch = _pitch + _clampedPitch * diag_deltaTime; - _yaw = _yaw + _clampedYaw * diag_deltaTime; + TRACE_9("pitch/yaw/roll",_pitch,_yaw,_roll,_yawChange,_pitchChange,_pitchRate,_yawRate,_clampedPitch,_clampedYaw); + + _pitch = _pitch + _clampedPitch * TIMESTEP_FACTOR; + _yaw = _yaw + _clampedYaw * TIMESTEP_FACTOR; + + TRACE_3("new pitch/yaw/roll",_pitch,_yaw,_roll); [_projectile, _pitch, _yaw, 0] call FUNC(changeMissileDirection); - _projectile setVelocityModelSpace [0, _projectileSpeed, 0]; + _projectile setVelocityModelSpace [0, vectorMagnitude velocity _projectile, 0]; - _currentPitchYawRoll set [0, _pitch]; - _currentPitchYawRoll set [1, _yaw]; + _currentPitchYawRoll set [0, _yaw]; + _currentPitchYawRoll set [2, _pitch]; + + _navigationParameters set [1, _currentPitchYawRoll]; }; - _pidData set [0, _pid]; - if (accTime > 0) then { - _pidData set [1, [_seekerTargetPos, _targetVelocity, _lineOfSight]]; - }; - _pidData set [2, _currentPitchYawRoll]; - _stateParams set [4, _pidData]; + _stateParams set [4, _navigationParameters]; _args set [4, _stateParams]; }; diff --git a/addons/missileguidance/functions/fnc_navigationType_proNav.sqf b/addons/missileguidance/functions/fnc_navigationType_proNav.sqf new file mode 100644 index 0000000000..9d4272ff1f --- /dev/null +++ b/addons/missileguidance/functions/fnc_navigationType_proNav.sqf @@ -0,0 +1,51 @@ +#include "script_component.hpp" +/* + * Author: Brandon (TCVM) + * Determine path for projectile to take in accordance to proportional navigation + * + * Arguments: + * Guidance Arg Array + * + * Return Value: + * Commanded acceleration normal to LOS in world space + * + * Example: + * [] call ace_missileguidance_fnc_navigationType_proNav + * + * Public: No + */ +params ["_args", "_timestep", "_seekerTargetPos", "_profileAdjustedTargetPos"]; +_args params ["_firedEH", "", "", "", "_stateParams"]; +_firedEH params ["","","","","","","_projectile"]; +_stateParams params ["", "", "", "","_navigationParams"]; +_navigationParams params ["_lastMissileFrame", "", "_navigationGain"]; +_lastMissileFrame params ["_lastTargetPosition", "_lastTargetVelocity", "_lastLineOfSight"]; + +// Proportional navigation implemented via "Fundamentals of proportional navigation" by Stephen Murtaugh and Harry Criel +private _projectileVelocity = velocity _projectile; + +// integrate target velocity for realistic inference of velocity +private _targetVelocity = (_seekerTargetPos vectorDiff _lastTargetPosition) vectorMultiply (1 / _timestep); +private _targetAcceleration = (_targetVelocity vectorDiff _lastTargetVelocity) vectorMultiply (1 / _timestep); + +private _closingVelocity = _targetVelocity vectorDiff _projectileVelocity; + +private _lineOfSight = vectorNormalized (_profileAdjustedTargetPos vectorDiff getPosASLVisual _projectile); + +// the los rate is tiny, so we multiply by a constant of a power of ten to get more aggressive acceleration +// this is just due to how we measure our LOS delta, the vectors involved are _tiny_ +private _losDelta = _lineOfSight vectorDiff _lastLineOfSight; +private _losRate = 1000 * (vectorMagnitude _losDelta) / _timestep; + +private _lateralAcceleration = (_navigationGain * _losRate); +private _commandedAcceleration = _closingVelocity vectorMultiply _lateralAcceleration; + +// we need acceleration normal to our LOS +private _commandedAccelerationProjected = _lineOfSight vectorMultiply (_commandedAcceleration vectorDotProduct _lineOfSight); +_commandedAcceleration = _commandedAcceleration vectorDiff _commandedAccelerationProjected; + +if (accTime > 0) then { + _navigationParams set [0, [_seekerTargetPos, _targetVelocity, _lineOfSight]]; +}; + +_commandedAcceleration diff --git a/addons/missileguidance/functions/fnc_onFired.sqf b/addons/missileguidance/functions/fnc_onFired.sqf index 149975208c..2d9e00ff08 100644 --- a/addons/missileguidance/functions/fnc_onFired.sqf +++ b/addons/missileguidance/functions/fnc_onFired.sqf @@ -44,6 +44,7 @@ private _target = _shooter getVariable [QGVAR(target), nil]; private _targetPos = _shooter getVariable [QGVAR(targetPosition), nil]; private _seekerType = _shooter getVariable [QGVAR(seekerType), nil]; private _attackProfile = _shooter getVariable [QGVAR(attackProfile), nil]; +private _navigationType = _shooter getVariable [QGVAR(navigationType), nil]; if ((getNumber (configFile >> "CfgAmmo" >> _ammo >> QUOTE(ADDON) >> "useModeForAttackProfile")) == 1) then { _attackProfile = getText (configFile >> "CfgWeapons" >> _weapon >> _mode >> QGVAR(attackProfile)) }; @@ -52,7 +53,7 @@ private _lockMode = _shooter getVariable [QGVAR(lockMode), nil]; private _laserCode = _shooter getVariable [QEGVAR(laser,code), ACE_DEFAULT_LASER_CODE]; private _laserInfo = [_laserCode, ACE_DEFAULT_LASER_WAVELENGTH, ACE_DEFAULT_LASER_WAVELENGTH]; -TRACE_6("getVars",_target,_targetPos,_seekerType,_attackProfile,_lockMode,_laserCode); +TRACE_7("getVars",_target,_targetPos,_seekerType,_attackProfile,_lockMode,_laserCode,_navigationType); private _launchPos = getPosASL (vehicle _shooter); @@ -65,6 +66,9 @@ if (isNil "_attackProfile" || {!(_attackProfile in (getArray (_config >> "attack if (isNil "_lockMode" || {!(_lockMode in (getArray (_config >> "seekerLockModes")))}) then { _lockMode = getText (_config >> "defaultSeekerLockMode"); }; +if (isNil "_navigationType" || {!(_navigationType in (getArray (_config >> "navigationTypes")))}) then { + _navigationType = getText (_config >> "defaultNavigationType"); +}; // If we didn't get a target, try to fall back on tab locking if (isNil "_target") then { @@ -76,7 +80,7 @@ if (isNil "_target") then { private _canUseLock = getNumber (_config >> "canVanillaLock"); // @TODO: Get vanilla target if (_canUseLock > 0 || difficulty < 1) then { - private _vanillaTarget = cursorTarget; + private _vanillaTarget = missileTarget _projectile; TRACE_1("Using Vanilla Locking", _vanillaTarget); if (!isNil "_vanillaTarget") then { @@ -95,22 +99,23 @@ 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, 0, 0], [0, 0, 0]], [_pitchYaw select 0, _pitchYaw select 1, 0]]; +private _navigationParameters = [ + // set up in navigation type onFired function +]; -TRACE_4("Beginning ACE guidance system",_target,_ammo,_seekerType,_attackProfile); +TRACE_5("Beginning ACE guidance system",_target,_ammo,_seekerType,_attackProfile,_navigationType); private _args = [_this, [ _shooter, [_target, _targetPos, _launchPos], _seekerType, _attackProfile, _lockMode, - _laserInfo + _laserInfo, + _navigationType ], [ - getNumber ( _config >> "minDeflection" ), - getNumber ( _config >> "maxDeflection" ), - getNumber ( _config >> "incDeflection" ) + getNumber ( _config >> "pitchRate" ), + getNumber ( _config >> "yawRate" ) ], [ getNumber ( _config >> "seekerAngle" ), @@ -118,7 +123,7 @@ private _args = [_this, getNumber ( _config >> "seekerMaxRange" ), getNumber ( _config >> "seekerMinRange" ) ], - [ diag_tickTime, [], [], _lastKnownPosState, _pidData] + [ diag_tickTime, [], [], _lastKnownPosState, _navigationParameters] ]; private _onFiredFunc = getText (configFile >> QGVAR(SeekerTypes) >> _seekerType >> "onFired"); @@ -133,6 +138,12 @@ if (_onFiredFunc != "") then { _args call (missionNamespace getVariable _onFiredFunc); }; +_onFiredFunc = getText (configFile >> QGVAR(NavigationTypes) >> _navigationType >> "onFired"); +TRACE_1("",_onFiredFunc); +if (_onFiredFunc != "") then { + _args call (missionNamespace getVariable _onFiredFunc); +}; + // Run the "onFired" function passing the full guidance args array _onFiredFunc = getText (_config >> "onFired"); TRACE_1("",_onFiredFunc); @@ -143,9 +154,9 @@ if (_onFiredFunc != "") then { // Reverse: // _args params ["_firedEH", "_launchParams", "_flightParams", "_seekerParams", "_stateParams"]; // _firedEH params ["_shooter","","","","_ammo","","_projectile"]; -// _launchParams params ["_shooter","_targetLaunchParams","_seekerType","_attackProfile","_lockMode","_laserInfo"]; +// _launchParams params ["_shooter","_targetLaunchParams","_seekerType","_attackProfile","_lockMode","_laserInfo","_navigationType"]; // _targetLaunchParams params ["_target", "_targetPos", "_launchPos"]; -// _stateParams params ["_lastRunTime", "_seekerStateParams", "_attackProfileStateParams", "_lastKnownPosState"]; +// _stateParams params ["_lastRunTime", "_seekerStateParams", "_attackProfileStateParams", "_lastKnownPosState","_navigationParams"]; // _seekerParams params ["_seekerAngle", "_seekerAccuracy", "_seekerMaxRange", "_seekerMinRange"]; [LINKFUNC(guidancePFH),0, _args ] call CBA_fnc_addPerFrameHandler; diff --git a/addons/missileguidance/functions/fnc_proNav_onFired.sqf b/addons/missileguidance/functions/fnc_proNav_onFired.sqf new file mode 100644 index 0000000000..1f4e72ec1d --- /dev/null +++ b/addons/missileguidance/functions/fnc_proNav_onFired.sqf @@ -0,0 +1,38 @@ +#include "script_component.hpp" +/* + * Author: Brandon (TCVM) + * Sets up proportional navigation state arrays (called from missileGuidance's onFired). + * + * Arguments: + * Guidance Arg Array + * + * Return Value: + * None + * + * Example: + * [] call ace_missileguidance_fnc_proNav_onFired + * + * Public: No + */ +params ["_firedEH", "", "", "", "_stateParams"]; +_firedEH params ["_shooter","","","","_ammo","","_projectile"]; +_launchParams params ["_shooter","_targetLaunchParams","_seekerType","_attackProfile","_lockMode","_laserInfo","_navigationType"]; +_targetLaunchParams params ["_target", "_targetPos", "_launchPos"]; +_stateParams params ["_lastRunTime", "_seekerStateParams", "_attackProfileStateParams", "_lastKnownPosState","_navigationParams"]; +_seekerParams params ["_seekerAngle", "_seekerAccuracy", "_seekerMaxRange", "_seekerMinRange"]; + +private _ammoConfig = configOf _projectile; +private _navigationGain = getNumber (_ammoConfig >> QUOTE(ADDON) >> "navigationGain"); + +private _pitchYaw = (vectorDir _projectile) call CBA_fnc_vect2Polar; + +_navigationParams = [ + [ // Last Missile Frame + [0, 0, 0], // Last target position + [0, 0, 0], // Last target velocity + [0, 0, 0] // Last line of sight + ], + [_pitchYaw select 1, 0, _pitchYaw select 2], // current yaw/roll/pitch + _navigationGain // navigation gain of missile. Set in the navigation onFired function +]; +_stateParams set [4, _navigationParams];