make navigation a modifyable system

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);
+    };
+};

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

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

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

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 <ARRAY>
+ *
+ * Return Value:
+ * Commanded acceleration normal to LOS in world space <ARRAY>
+ *
+ * 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

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;

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