diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 0020fd529f..88bcbd2b2b 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -62,14 +62,11 @@
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
-#include "flight/alt_hold.h"
-#include "flight/autopilot.h"
 #include "flight/failsafe.h"
 #include "flight/gps_rescue.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
 #include "flight/position.h"
-#include "flight/pos_hold.h"
 #include "flight/rpm_filter.h"
 #include "flight/servos.h"
 #include "flight/imu.h"
@@ -80,8 +77,12 @@
 
 #include "pg/pg.h"
 #include "pg/pg_ids.h"
+
+#include "pg/alt_hold.h"
+#include "pg/autopilot.h"
 #include "pg/motor.h"
 #include "pg/rx.h"
+#include "pg/pos_hold.h"
 
 #include "rx/rx.h"
 
diff --git a/src/main/cms/cms_menu_gps_rescue.c b/src/main/cms/cms_menu_gps_rescue.c
index c09e2e10c3..b9981a47c3 100644
--- a/src/main/cms/cms_menu_gps_rescue.c
+++ b/src/main/cms/cms_menu_gps_rescue.c
@@ -37,10 +37,11 @@
 
 #include "config/config.h"
 
-#include "flight/autopilot.h"
-#include "flight/gps_rescue.h"
 #include "flight/position.h"
 
+#include "pg/autopilot.h"
+#include "pg/gps_rescue.h"
+
 static uint16_t gpsRescueConfig_minStartDistM; //meters
 static uint8_t gpsRescueConfig_altitudeMode;
 static uint16_t gpsRescueConfig_initialClimbM; // meters
diff --git a/src/main/fc/init.c b/src/main/fc/init.c
index 9e6eacca56..7b9f7a2673 100644
--- a/src/main/fc/init.c
+++ b/src/main/fc/init.c
@@ -832,7 +832,7 @@ void init(void)
 #endif
 
     positionInit();
-    autopilotInit(autopilotConfig());
+    autopilotInit();
 
 #if defined(USE_VTX_COMMON) || defined(USE_VTX_CONTROL)
     vtxTableInit();
diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c
index 3fa890addd..e2485640a5 100644
--- a/src/main/fc/rc.c
+++ b/src/main/fc/rc.c
@@ -47,9 +47,6 @@
 #include "flight/pid_init.h"
 
 #include "pg/rx.h"
-#include "pg/pos_hold.h"
-#include "pg/autopilot.h"
-
 #include "rx/rx.h"
 
 #include "sensors/battery.h"
diff --git a/src/main/flight/alt_hold.c b/src/main/flight/alt_hold.c
index a36611d981..700c2d6674 100644
--- a/src/main/flight/alt_hold.c
+++ b/src/main/flight/alt_hold.c
@@ -32,6 +32,7 @@
 #include "flight/position.h"
 
 #include "rx/rx.h"
+#include "pg/autopilot.h"
 
 #include "alt_hold.h"
 
diff --git a/src/main/flight/autopilot.c b/src/main/flight/autopilot.c
index 9e239909ec..a95ba42f3b 100644
--- a/src/main/flight/autopilot.c
+++ b/src/main/flight/autopilot.c
@@ -34,6 +34,7 @@
 #include "rx/rx.h"
 #include "sensors/gyro.h"
 
+#include "pg/autopilot.h"
 #include "autopilot.h"
 
 #define ALTITUDE_P_SCALE  0.01f
@@ -52,7 +53,7 @@ static pidCoefficient_t positionPidCoeffs;
 static float altitudeI = 0.0f;
 static float throttleOut = 0.0f;
 
-typedef struct {
+typedef struct earthFrame_s {
     bool isStopping;
     float distance;
     float previousDistance;
@@ -68,7 +69,7 @@ typedef enum {
     NS
 } axisEF_t;
 
-typedef struct {
+typedef struct autopilotState_s {
     float gpsDataIntervalS;
     float gpsDataFreqHz;
     float sanityCheckDistance;
@@ -80,9 +81,9 @@ typedef struct {
     float pidSumCraft[EF_AXIS_COUNT];
     pt3Filter_t upsample[EF_AXIS_COUNT];
     earthFrame_t efAxis[EF_AXIS_COUNT];
-} posHoldState;
+} autopilotState_t;
 
-static posHoldState posHold = {
+static autopilotState_t ap = {
     .gpsDataIntervalS = 0.1f,
     .gpsDataFreqHz = 10.0f,
     .sanityCheckDistance = 1000.0f,
@@ -100,27 +101,27 @@ float autopilotAngle[RP_AXIS_COUNT];
 void resetPositionControlEFParams(earthFrame_t *efAxis)
 {
     // at start only
-    pt1FilterInit(&efAxis->velocityLpf, posHold.pt1Gain); // Clear and initialise the filters
-    pt1FilterInit(&efAxis->accelerationLpf, posHold.pt1Gain);
+    pt1FilterInit(&efAxis->velocityLpf, ap.pt1Gain); // Clear and initialise the filters
+    pt1FilterInit(&efAxis->accelerationLpf, ap.pt1Gain);
     efAxis->isStopping = true; // Enter starting 'phase'
     efAxis->integral = 0.0f;
 }
 
 void resetPt3UpsampleFilters(void)
 {
-    pt3FilterInit(&posHold.upsample[AI_ROLL], posHold.upsampleCutoff);
-    pt3FilterInit(&posHold.upsample[AI_PITCH], posHold.upsampleCutoff);
+    pt3FilterInit(&ap.upsample[AI_ROLL], ap.upsampleCutoff);
+    pt3FilterInit(&ap.upsample[AI_PITCH], ap.upsampleCutoff);
 }
 
-void resetPositionControl(gpsLocation_t initialTargetLocation)
+void resetPositionControl(gpsLocation_t *initialTargetLocation)
 {
     // from pos_hold.c when initiating position hold at target location
-    currentTargetLocation = initialTargetLocation;
-    posHold.sticksActive = false;
+    currentTargetLocation = *initialTargetLocation;
+    ap.sticksActive = false;
     // set sanity check distance according to groundspeed at start
-    posHold.sanityCheckDistance = gpsSol.groundSpeed > 500 ? gpsSol.groundSpeed * 2.0f : 1000.0f;
-    resetPositionControlEFParams(&posHold.efAxis[EW]);
-    resetPositionControlEFParams(&posHold.efAxis[NS]);
+    ap.sanityCheckDistance = gpsSol.groundSpeed > 500 ? gpsSol.groundSpeed * 2.0f : 1000.0f;
+    resetPositionControlEFParams(&ap.efAxis[EW]);
+    resetPositionControlEFParams(&ap.efAxis[NS]);
     resetPt3UpsampleFilters(); // clear anything from previous iteration
 }
 
@@ -129,26 +130,26 @@ void initializeEfAxisFilters(earthFrame_t *efAxis, float filterGain) {
     pt1FilterInit(&efAxis->accelerationLpf, filterGain);
 }
 
-void autopilotInit(const autopilotConfig_t *config)
+void autopilotInit(void)
 {
-    posHold.sticksActive = false;
-    posHold.maxAngle = autopilotConfig()->max_angle;
-    altitudePidCoeffs.Kp = config->altitude_P * ALTITUDE_P_SCALE;
-    altitudePidCoeffs.Ki = config->altitude_I * ALTITUDE_I_SCALE;
-    altitudePidCoeffs.Kd = config->altitude_D * ALTITUDE_D_SCALE;
-    altitudePidCoeffs.Kf = config->altitude_F * ALTITUDE_F_SCALE;
-    positionPidCoeffs.Kp = config->position_P * POSITION_P_SCALE;
-    positionPidCoeffs.Ki = config->position_I * POSITION_I_SCALE;
-    positionPidCoeffs.Kd = config->position_D * POSITION_D_SCALE;
-    positionPidCoeffs.Kf = config->position_A * POSITION_A_SCALE; // Kf used for acceleration
+    ap.sticksActive = false;
+    ap.maxAngle = autopilotConfig()->max_angle;
+    altitudePidCoeffs.Kp = autopilotConfig()->altitude_P * ALTITUDE_P_SCALE;
+    altitudePidCoeffs.Ki = autopilotConfig()->altitude_I * ALTITUDE_I_SCALE;
+    altitudePidCoeffs.Kd = autopilotConfig()->altitude_D * ALTITUDE_D_SCALE;
+    altitudePidCoeffs.Kf = autopilotConfig()->altitude_F * ALTITUDE_F_SCALE;
+    positionPidCoeffs.Kp = autopilotConfig()->position_P * POSITION_P_SCALE;
+    positionPidCoeffs.Ki = autopilotConfig()->position_I * POSITION_I_SCALE;
+    positionPidCoeffs.Kd = autopilotConfig()->position_D * POSITION_D_SCALE;
+    positionPidCoeffs.Kf = autopilotConfig()->position_A * POSITION_A_SCALE; // Kf used for acceleration
     // initialise filters with approximate filter gain
-    posHold.upsampleCutoff = pt3FilterGain(UPSAMPLING_CUTOFF_HZ, 0.01f); // 5Hz, assuming 100Hz task rate
+    ap.upsampleCutoff = pt3FilterGain(UPSAMPLING_CUTOFF_HZ, 0.01f); // 5Hz, assuming 100Hz task rate
     resetPt3UpsampleFilters();
     // Initialise PT1 filters for earth frame axes NS and EW
-    posHold.pt1Cutoff = config->position_cutoff * 0.01f;
-    posHold.pt1Gain = pt1FilterGain(posHold.pt1Cutoff, 0.1f); // assume 10Hz GPS connection at start
-    initializeEfAxisFilters(&posHold.efAxis[EW], posHold.pt1Gain);
-    initializeEfAxisFilters(&posHold.efAxis[NS], posHold.pt1Gain);
+    ap.pt1Cutoff = autopilotConfig()->position_cutoff * 0.01f;
+    ap.pt1Gain = pt1FilterGain(ap.pt1Cutoff, 0.1f); // assume 10Hz GPS connection at start
+    initializeEfAxisFilters(&ap.efAxis[EW], ap.pt1Gain);
+    initializeEfAxisFilters(&ap.efAxis[NS], ap.pt1Gain);
 }
 
 void resetAltitudeControl (void) {
@@ -207,20 +208,20 @@ void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAl
 
 void setSticksActiveStatus(bool areSticksActive)
 {
-    posHold.sticksActive = areSticksActive;
+    ap.sticksActive = areSticksActive;
 }
 
 void setTargetLocation(gpsLocation_t newTargetLocation)
 {
     currentTargetLocation = newTargetLocation;
-    posHold.efAxis[EW].previousDistance = 0.0f; // reset to avoid D and A spikess
-    posHold.efAxis[NS].previousDistance = 0.0f;
+    ap.efAxis[EW].previousDistance = 0.0f; // reset to avoid D and A spikess
+    ap.efAxis[NS].previousDistance = 0.0f;
     // function is intended for only small changes in position
     // for example, where the step distance change reflects an intended velocity, determined by a client function
     // if we had a 'target_ground_speed' value, like in gps_rescue, we can make a function that starts and stops smoothly and targets that velocity
 }
 
-void resetLocation(earthFrame_t *efAxis, axisEF_t loopAxis)
+void updateLocation(earthFrame_t *efAxis, axisEF_t loopAxis)
 {
     if (loopAxis == EW) {
         currentTargetLocation.lon = gpsSol.llh.lon; // update East-West / / longitude position
@@ -234,62 +235,63 @@ bool positionControl(void)
 {
     static uint16_t gpsStamp = 0;
     if (gpsHasNewData(&gpsStamp)) {
-        posHold.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.01s to 1.0s
-        posHold.gpsDataFreqHz = 1.0f / posHold.gpsDataIntervalS;
+        ap.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.05 - 2.5s
+        ap.gpsDataFreqHz = getGpsDataFrequencyHz();
 
         // first get NS and EW distances from current location (gpsSol.llh) to target location
         vector2_t gpsDistance;
         GPS_latLongVectors(&gpsSol.llh, &currentTargetLocation, &gpsDistance.x, &gpsDistance.y); // X is EW, Y is NS
-        posHold.efAxis[EW].distance = gpsDistance.x;
-        posHold.efAxis[NS].distance = gpsDistance.y;
+        ap.efAxis[EW].distance = gpsDistance.x;
+        ap.efAxis[NS].distance = gpsDistance.y;
 
         const float distanceCm = vector2Norm(&gpsDistance);
 
-        const float leak = 1.0f - 0.4f * posHold.gpsDataIntervalS;
+        const float leak = 1.0f - 0.4f * ap.gpsDataIntervalS;
         // leak iTerm while sticks are centered, 2s time constant approximately
-        const float lpfGain = pt1FilterGain(posHold.pt1Cutoff, posHold.gpsDataIntervalS);
+        const float lpfGain = pt1FilterGain(ap.pt1Cutoff, ap.gpsDataIntervalS);
 
         // ** Sanity check **
         // primarily to detect flyaway from no Mag or badly oriented Mag
         // must accept some overshoot at the start, especially if entering at high speed
-        if (distanceCm > posHold.sanityCheckDistance) {
+        if (distanceCm > ap.sanityCheckDistance) {
             return false;
         }
 
         static float prevPidDASquared = 0.0f; // if we limit DA on true vector length
         const float maxDAAngle = 35.0f; // limit in degrees; arbitrary angle
+        const float sqMaxDAAngle = sq(maxDAAngle);
 
         for (axisEF_t loopAxis = EW; loopAxis <= NS; loopAxis++) {
-            earthFrame_t *efAxis = &posHold.efAxis[loopAxis];
+            earthFrame_t *efAxis = &ap.efAxis[loopAxis];
             // separate PID controllers for latitude (NorthSouth or NS) and longitude (EastWest or EW)
 
             // ** P **
             const float pidP = efAxis->distance * positionPidCoeffs.Kp;
 
             // ** I **
-            efAxis->integral += efAxis->isStopping ? 0.0f : efAxis->distance * posHold.gpsDataIntervalS;
+            efAxis->integral += efAxis->isStopping ? 0.0f : efAxis->distance * ap.gpsDataIntervalS;
             // only add to iTerm while in hold phase
             const float pidI = efAxis->integral * positionPidCoeffs.Ki;
 
             // ** D ** //
             // Velocity derived from GPS position works better than module supplied GPS Speed and Heading information
 
-            float velocity = (efAxis->distance - efAxis->previousDistance) * posHold.gpsDataFreqHz; // cm/s, minimum step 11.1 cm/s
+            float velocity = (efAxis->distance - efAxis->previousDistance) * ap.gpsDataFreqHz; // cm/s, minimum step 11.1 cm/s
             efAxis->previousDistance = efAxis->distance;
             pt1FilterUpdateCutoff(&efAxis->velocityLpf, lpfGain);
             const float velocityFiltered = pt1FilterApply(&efAxis->velocityLpf, velocity);
             float pidD = velocityFiltered * positionPidCoeffs.Kd;
 
-            float acceleration = (velocity - efAxis->previousVelocity) * posHold.gpsDataFreqHz;
+            float acceleration = (velocity - efAxis->previousVelocity) * ap.gpsDataFreqHz;
             efAxis->previousVelocity = velocity;
             pt1FilterUpdateCutoff(&efAxis->accelerationLpf, lpfGain);
             const float accelerationFiltered = pt1FilterApply(&efAxis->accelerationLpf, acceleration);
             const float pidA = accelerationFiltered * positionPidCoeffs.Kf;
 
-            if (posHold.sticksActive) {
+            if (ap.sticksActive) {
                 // sticks active 'phase'
                 efAxis->isStopping = true;
-                resetLocation(efAxis, loopAxis);
+                updateLocation(efAxis, loopAxis);
                 // while sticks are moving, reset the location on each axis, to maintain a usable D value
                 // slowly leak iTerm away, approx 2s time constant
                 efAxis->integral *= leak;
@@ -299,20 +301,20 @@ bool positionControl(void)
                 pidD *= 1.6f; // aribitrary D boost to stop more quickly when sticks are centered
                 if (velocity * velocityFiltered < 0.0f) {
                     // when an axis has nearly stopped moving, reset it and end it's start phase
-                    resetLocation(efAxis, loopAxis);
+                    updateLocation(efAxis, loopAxis);
                     efAxis->isStopping = false;
                 }
             }
 
+            float pidDA = pidD + pidA;
+
             // limit sum of D and A per axis based on total DA vector length, otherwise can be too aggressive when starting at speed
             // limit is 35 degrees from D and A alone, arbitrary value.  20 is a bit too low, allows a lot of overshoot
             // note: an angle of more than 35 degrees can still be achieved as P and I grow
-
-            float pidDA = pidD + pidA;
-            const float pidDASquared = pidDA * pidDA;
-            float mag = sqrtf(pidDASquared + prevPidDASquared);
-            if (mag > maxDAAngle) {
-                pidDA *= (maxDAAngle / mag);
+            const float pidDASquared = sq(pidD + pidA);
+            float magSq = pidDASquared + prevPidDASquared;
+            if (magSq > sqMaxDAAngle && magSq > 0.0f) {
+                pidDA *= maxDAAngle / sqrtf(magSq);
             }
             prevPidDASquared = pidDASquared;
 
@@ -328,42 +330,42 @@ bool positionControl(void)
             }
         } // end for loop
 
-        if (posHold.sticksActive) {
+        if (ap.sticksActive) {
             // keep update sanity check distance while sticks are out
-            posHold.sanityCheckDistance = gpsSol.groundSpeed > 500 ? gpsSol.groundSpeed * 2.0f : 1000.0f;
+            ap.sanityCheckDistance = gpsSol.groundSpeed > 500 ? gpsSol.groundSpeed * 2.0f : 1000.0f;
             // if a Position Hold deadband is set, and sticks are outside deadband, allow pilot control in angle mode
-            posHold.pidSumCraft[AI_ROLL] = 0.0f;
-            posHold.pidSumCraft[AI_PITCH] = 0.0f;
+            ap.pidSumCraft[AI_ROLL] = 0.0f;
+            ap.pidSumCraft[AI_PITCH] = 0.0f;
         } else {
             // ** Rotate pid Sum to quad frame of reference, into pitch and roll **
             const float headingRads = DECIDEGREES_TO_RADIANS(attitude.values.yaw);
             const float sinHeading = sin_approx(headingRads);
             const float cosHeading = cos_approx(headingRads);
-            posHold.pidSumCraft[AI_ROLL] = -sinHeading * posHold.efAxis[NS].pidSum + cosHeading * posHold.efAxis[EW].pidSum;
-            posHold.pidSumCraft[AI_PITCH] = cosHeading * posHold.efAxis[NS].pidSum + sinHeading * posHold.efAxis[EW].pidSum;
+            ap.pidSumCraft[AI_ROLL] = -sinHeading * ap.efAxis[NS].pidSum + cosHeading * ap.efAxis[EW].pidSum;
+            ap.pidSumCraft[AI_PITCH] = cosHeading * ap.efAxis[NS].pidSum + sinHeading * ap.efAxis[EW].pidSum;
 
             // limit angle vector to maxAngle
-            const float angleMagnitude = sqrtf(posHold.pidSumCraft[AI_ROLL] * posHold.pidSumCraft[AI_ROLL] + posHold.pidSumCraft[AI_PITCH] * posHold.pidSumCraft[AI_PITCH]);
-            if (angleMagnitude > posHold.maxAngle && angleMagnitude > 0.0f) {
-                const float limiter = posHold.maxAngle / angleMagnitude;
-                posHold.pidSumCraft[AI_ROLL] *= limiter;  // Scale the roll value
-                posHold.pidSumCraft[AI_PITCH] *= limiter; // Scale the pitch value
+            const float angleMagSq = sq(ap.pidSumCraft[AI_ROLL]) + sq(ap.pidSumCraft[AI_PITCH]);
+            if (angleMagSq > sq(ap.maxAngle) && angleMagSq > 0.0f) {
+                const float limiter = ap.maxAngle / sqrtf(angleMagSq);
+                ap.pidSumCraft[AI_ROLL] *= limiter;  // Scale the roll value
+                ap.pidSumCraft[AI_PITCH] *= limiter; // Scale the pitch value
             }
         }
     }
 
     // ** Final output to pid.c Angle Mode at 100Hz with primitive upsampling**
-    autopilotAngle[AI_ROLL] = pt3FilterApply(&posHold.upsample[AI_ROLL], posHold.pidSumCraft[AI_ROLL]);
-    autopilotAngle[AI_PITCH] = pt3FilterApply(&posHold.upsample[AI_PITCH], posHold.pidSumCraft[AI_PITCH]);
+    autopilotAngle[AI_ROLL] = pt3FilterApply(&ap.upsample[AI_ROLL], ap.pidSumCraft[AI_ROLL]);
+    autopilotAngle[AI_PITCH] = pt3FilterApply(&ap.upsample[AI_PITCH], ap.pidSumCraft[AI_PITCH]);
     // note: upsampling should really be done in earth frame, to avoid 10Hz wobbles if pilot yaws and the controller is applying significant pitch or roll
 
     if (gyroConfig()->gyro_filter_debug_axis == FD_ROLL) {
-        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(posHold.efAxis[EW].distance));    // cm
-        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(posHold.efAxis[EW].pidSum * 10)); // deg * 10
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(ap.efAxis[EW].distance));    // cm
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(ap.efAxis[EW].pidSum * 10)); // deg * 10
         DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(autopilotAngle[AI_ROLL] * 10));   // deg * 10
     } else {
-        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(posHold.efAxis[NS].distance));
-        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(posHold.efAxis[NS].pidSum * 10));
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(ap.efAxis[NS].distance));
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(ap.efAxis[NS].pidSum * 10));
         DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(autopilotAngle[AI_PITCH] * 10));
     }
     return true;
@@ -381,5 +383,5 @@ float getAutopilotThrottle(void)
 
 bool isAutopilotActive(void)
 {
-    return !posHold.sticksActive;
+    return !ap.sticksActive;
 }
diff --git a/src/main/flight/autopilot.h b/src/main/flight/autopilot.h
index ff04abf917..c8afadd4c7 100644
--- a/src/main/flight/autopilot.h
+++ b/src/main/flight/autopilot.h
@@ -17,19 +17,17 @@
 
 #pragma once
 
-#include "pg/autopilot.h"
-#include "flight/pid.h"
 #include "io/gps.h"
 
 extern float autopilotAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
 
-void autopilotInit(const autopilotConfig_t *config);
+void autopilotInit(void);
 void resetAltitudeControl(void);
 void setSticksActiveStatus(bool areSticksActive);
-void resetPositionControl(gpsLocation_t initialTargetLocation);
+void resetPositionControl(gpsLocation_t *initialTargetLocation);
 void moveTargetLocation(int32_t latStep, int32_t lonStep);
-void (posControlOnNewGpsData) (void);
-void (posControlOutput) (void);
+void posControlOnNewGpsData(void);
+void posControlOutput(void);
 bool positionControl(void);
 void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAltitudeStep);
 
diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c
index 48984dd756..aa61788e8c 100644
--- a/src/main/flight/gps_rescue.c
+++ b/src/main/flight/gps_rescue.c
@@ -46,6 +46,7 @@
 
 #include "io/gps.h"
 #include "rx/rx.h"
+#include "pg/autopilot.h"
 #include "sensors/acceleration.h"
 
 #include "gps_rescue.h"
@@ -512,9 +513,9 @@ static void sensorUpdate(void)
     rescueState.sensor.groundSpeedCmS = gpsSol.groundSpeed; // cm/s
 
     rescueState.sensor.gpsDataIntervalSeconds = getGpsDataIntervalSeconds();
-    // Range from 10ms (100hz) to 1000ms (1Hz). Intended to cover common GPS data rates and exclude unusual values.
+    // Range from 50ms (20hz) to 2500ms (0.4Hz). Intended to cover common GPS data rates and exclude unusual values.
 
-    rescueState.sensor.velocityToHomeCmS = ((prevDistanceToHomeCm - rescueState.sensor.distanceToHomeCm) / rescueState.sensor.gpsDataIntervalSeconds);
+    rescueState.sensor.velocityToHomeCmS = ((prevDistanceToHomeCm - rescueState.sensor.distanceToHomeCm) * getGpsDataFrequencyHz());
     // positive = towards home.  First value is useless since prevDistanceToHomeCm was zero.
     prevDistanceToHomeCm = rescueState.sensor.distanceToHomeCm;
 
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 4108bf8b84..b00bcc5e16 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -57,6 +57,8 @@
 #include "pg/pg.h"
 #include "pg/pg_ids.h"
 
+#include "pg/autopilot.h"
+
 #include "sensors/acceleration.h"
 #include "sensors/battery.h"
 #include "sensors/gyro.h"
@@ -577,7 +579,7 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_
             angleLimit = 85.0f; // allow autopilot to use whatever angle it needs to stop
         }
         // limit pilot requested angle to half the autopilot angle to avoid excess speed and chaotic stops
-        angleLimit = fminf (0.5f * autopilotConfig()->max_angle, angleLimit);
+        angleLimit = fminf(0.5f * autopilotConfig()->max_angle, angleLimit);
     }
 #endif
 
diff --git a/src/main/flight/pos_hold.c b/src/main/flight/pos_hold.c
index 63751b4f9f..ce193b1b41 100644
--- a/src/main/flight/pos_hold.c
+++ b/src/main/flight/pos_hold.c
@@ -34,9 +34,10 @@
 #include "rx/rx.h"
 #include "sensors/compass.h"
 
+#include "pg/pos_hold.h"
 #include "pos_hold.h"
 
-typedef struct {
+typedef struct posHoldState_s {
     bool posHoldIsOK;
     float deadband;
     bool useStickAdjustment;
@@ -67,7 +68,7 @@ void posHoldStart(void)
         if (!isInPosHoldMode) {
             // start position hold mode
             posHold.posHoldIsOK = true; // true when started, false when autopilot code reports failure
-            resetPositionControl(gpsSol.llh); // sets target location to current location
+            resetPositionControl(&gpsSol.llh); // sets target location to current location
             isInPosHoldMode = true;
         }
     } else {
diff --git a/src/main/flight/pos_hold.h b/src/main/flight/pos_hold.h
index 60e0722f95..f20d740ab5 100644
--- a/src/main/flight/pos_hold.h
+++ b/src/main/flight/pos_hold.h
@@ -17,7 +17,7 @@
 
 #pragma once
 
-#include "pg/pos_hold.h"
+// #include "pg/pos_hold.h"
 
 #ifdef USE_POS_HOLD_MODE
 #include "common/time.h"
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index bd799b9bd0..27614ef2ce 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -92,7 +92,9 @@ GPS_svinfo_t GPS_svinfo[GPS_SV_MAXSATS_M8N];
 #define GPS_TASK_DECAY_SHIFT 9         // Smoothing factor for GPS task re-scheduler
 
 static serialPort_t *gpsPort;
-static float gpsDataIntervalSeconds;
+static float gpsDataIntervalSeconds = 0.1f;
+static float gpsDataFrequencyHz = 10.0f;
+
 static uint16_t currentGpsStamp = 1; // logical timer for received position update
 
 typedef struct gpsInitData_s {
@@ -1762,7 +1764,7 @@ static bool writeGpsSolutionNmea(gpsSolutionData_t *sol, const gpsDataNmea_t *da
             }
              navDeltaTimeMs = (msInTenSeconds + data->time - gpsData.lastNavSolTs) % msInTenSeconds;
              gpsData.lastNavSolTs = data->time;
-             sol->navIntervalMs = constrain(navDeltaTimeMs, 100, 2500);
+             sol->navIntervalMs = constrain(navDeltaTimeMs, 50, 2500);
             // return only one true statement to trigger one "newGpsDataReady" flag per GPS loop
             return true;
 
@@ -2609,7 +2611,8 @@ void onGpsNewData(void)
 
     currentGpsStamp++; // new GPS data available
 
-    gpsDataIntervalSeconds = gpsSol.navIntervalMs / 1000.0f;
+    gpsDataIntervalSeconds = gpsSol.navIntervalMs * 0.001f; // range for navIntervalMs is constrained to 50 - 2500
+    gpsDataFrequencyHz = 1.0f / gpsDataIntervalSeconds;
 
     GPS_calculateDistanceAndDirectionToHome();
     if (ARMING_FLAG(ARMED)) {
@@ -2648,6 +2651,11 @@ float getGpsDataIntervalSeconds(void)
     return gpsDataIntervalSeconds;
 }
 
+float getGpsDataFrequencyHz(void)
+{
+    return gpsDataFrequencyHz;
+}
+
 baudRate_e getGpsPortActualBaudRateIndex(void)
 {
     return lookupBaudRateIndex(serialGetBaudRate(gpsPort));
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index 28487029cf..9ccf5a22db 100644
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -394,6 +394,7 @@ void GPS_latLongVectors(const gpsLocation_t *from, const gpsLocation_t *to, floa
 void gpsSetFixState(bool state);
 
 bool gpsHasNewData(uint16_t *stamp);
-float getGpsDataIntervalSeconds(void);      // sends GPS Nav Data interval to GPS Rescue
+float getGpsDataIntervalSeconds(void);  // range 0.05 - 2.5s
+float getGpsDataFrequencyHz(void);      // range 20Hz - 0.4Hz
 
 baudRate_e getGpsPortActualBaudRateIndex(void);
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index 2fb34a2cba..c0f4128073 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -83,9 +83,7 @@
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
-#include "flight/autopilot.h"
 #include "flight/failsafe.h"
-#include "flight/gps_rescue.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
@@ -119,9 +117,11 @@
 #include "osd/osd_elements.h"
 #include "osd/osd_warnings.h"
 
+#include "pg/autopilot.h"
 #include "pg/beeper.h"
 #include "pg/board.h"
 #include "pg/dyn_notch.h"
+#include "pg/gps_rescue.h"
 #include "pg/gyrodev.h"
 #include "pg/motor.h"
 #include "pg/pos_hold.h"
diff --git a/src/test/unit/althold_unittest.cc b/src/test/unit/althold_unittest.cc
index 1e2dfc5a68..6a0a3fbb93 100644
--- a/src/test/unit/althold_unittest.cc
+++ b/src/test/unit/althold_unittest.cc
@@ -32,7 +32,6 @@ extern "C" {
     #include "fc/runtime_config.h"
 
     #include "flight/alt_hold.h"
-    #include "flight/autopilot.h"
     #include "flight/failsafe.h"
     #include "flight/imu.h"
     #include "flight/pid.h"
@@ -42,6 +41,9 @@ extern "C" {
 
     #include "rx/rx.h"
 
+    #include "pg/alt_hold.h"
+    #include "pg/autopilot.h"
+    
     #include "sensors/acceleration.h"
     #include "sensors/gyro.h"
 
@@ -110,10 +112,11 @@ extern "C" {
     attitudeEulerAngles_t attitude;
     gpsSolutionData_t gpsSol;
 
-    float getAltitudeCm(void) {return 0.0f;}
-    float getAltitudeDerivative(void) {return 0.0f;}
+    float getAltitudeCm(void) { return 0.0f; }
+    float getAltitudeDerivative(void) { return 0.0f; }
     float getCosTiltAngle(void) { return 0.0f; }
-    float getGpsDataIntervalSeconds(void) { return 0.01f; }//    gpsSolutionData_t gpsSol;
+    float getGpsDataIntervalSeconds(void) { return 0.01f; }
+    float getGpsDataFrequencyHz(void) { return 10.0f; }
     float rcCommand[4];
 
     bool gpsHasNewData(uint16_t* gpsStamp) {
diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc
index 61563af4ae..631dc815b5 100644
--- a/src/test/unit/arming_prevention_unittest.cc
+++ b/src/test/unit/arming_prevention_unittest.cc
@@ -36,7 +36,6 @@ extern "C" {
     #include "fc/rc_modes.h"
     #include "fc/runtime_config.h"
 
-    #include "flight/autopilot.h"
     #include "flight/failsafe.h"
     #include "flight/gps_rescue.h"
     #include "flight/imu.h"
@@ -48,11 +47,13 @@ extern "C" {
     #include "io/beeper.h"
     #include "io/gps.h"
 
+    #include "pg/autopilot.h"
     #include "pg/gps_rescue.h"
     #include "pg/motor.h"
+    #include "pg/rx.h"
+
     #include "pg/pg.h"
     #include "pg/pg_ids.h"
-    #include "pg/rx.h"
 
     #include "rx/rx.h"
 
@@ -100,6 +101,7 @@ extern "C" {
     uint8_t activePidLoopDenom = 1;
 
     float getGpsDataIntervalSeconds(void) { return 0.1f; }
+    float getGpsDataFrequencyHz(void) { return 10.0f; }
     void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k) { filter->k = k; }
     void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k) { filter->k = k; }
     void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k) { filter->k = k; }
diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc
index 6f491b5fa8..c0fab3fc9d 100644
--- a/src/test/unit/flight_imu_unittest.cc
+++ b/src/test/unit/flight_imu_unittest.cc
@@ -42,7 +42,6 @@ extern "C" {
     #include "fc/runtime_config.h"
     #include "fc/rc.h"
 
-    #include "flight/autopilot.h"
     #include "flight/imu.h"
     #include "flight/mixer.h"
     #include "flight/pid.h"
@@ -52,6 +51,8 @@ extern "C" {
 
     #include "rx/rx.h"
 
+    #include "pg/autopilot.h"
+
     #include "sensors/acceleration.h"
     #include "sensors/barometer.h"
     #include "sensors/compass.h"