Forget using a PID loop, just lock to the throttle function and simplify it.

lib/throttle.ks

@@ -1,29 +1,30 @@
 // Functions for calculating thrust values.
 @lazyglobal off.
 
+local G is 9.81.
+lock G to GetGravAcc().
+
+function GetGravAcc {
+  if HAS_GRAV_SENSOR = true {
+    return SHIP:SENSORS:GRAV:MAG.
+  }
+  return SHIP:BODY:MU / ((SHIP:BODY:RADIUS+SHIP:ALTITUDE)^2).
+}
+
 // Returns the throttle value you should use to achieve the
 // target TWR. If TWR can't be achieved, returns 1.0. (full throttle)
 function ThrottleToTWR {
   parameter targetTWR is 1.5.
 
   local m is SHIP:MASS.
-  local g is 9.81.
-  if HAS_GRAV_SENSOR = true {
-    set g to SHIP:SENSORS:GRAV:MAG.
-  }
-  return min((targetTWR*m*g)/SHIP:AVAILABLETHRUST, 1.0).
+  return min((targetTWR*m*G)/SHIP:AVAILABLETHRUST, 1.0).
 }
 
 // Calculates the ship's current TWR.
 function TWR {
   local m is ship:mass.
-  local g is 9.81. // fallback to this approximation that doesn't deal with
-                   // altitude change.
-  if HAS_GRAV_SENSOR = true {
-    set g to SHIP:SENSORS:GRAV:MAG.
-  }
   local t is THROTTLE * SHIP:AVAILABLETHRUST.
-  return t/(m*g).
+  return t/(m*G).
 }
 
 // Check for various sensors and set appropriate global constants.
@@ -51,7 +52,6 @@ function BurnTime {
   local m is SHIP:MASS * 1000.        // Starting mass (kg)
   local e is CONSTANT():E.            // Base of natural log
   local p is en[0]:ISP.               // Engine ISP (s)
-  local g is 9.80665.                 // Gravitational acceleration constant (m/s²)
 
-  return g * m * p * (1 - e^(-dV/(g*p))) / f.
+  return G * m * p * (1 - e^(-dV/(G*p))) / f.
 }