Increase stability of integration for PID controller

src/core/ext/transport/chttp2/transport/chttp2_transport.c

@@ -252,8 +252,7 @@ static void init_transport(grpc_exec_ctx *exec_ctx, grpc_chttp2_transport *t,
   grpc_bdp_estimator_init(&t->bdp_estimator);
   t->last_bdp_ping_finished = gpr_now(GPR_CLOCK_MONOTONIC);
   t->last_pid_update = t->last_bdp_ping_finished;
-  grpc_pid_controller_init(&t->pid_controller, 4, 4, 0);
-  t->log2_bdp_guess = log2(DEFAULT_WINDOW);
+  grpc_pid_controller_init(&t->pid_controller, log2(DEFAULT_WINDOW), 4, 4, 0);
 
   grpc_chttp2_goaway_parser_init(&t->goaway_parser);
   grpc_chttp2_hpack_parser_init(&t->hpack_parser);
@@ -1903,21 +1902,22 @@ static void read_action_locked(grpc_exec_ctx *exec_ctx, void *tp,
       if (memory_pressure > 0.8) {
         target *= 1 - GPR_MIN(1, (memory_pressure - 0.8) / 0.1);
       }
-      bdp_error = target > 0 ? log2(target) - t->log2_bdp_guess
-                             : GPR_MIN(0, -t->log2_bdp_guess);
+      bdp_error =
+          target > 0
+              ? log2(target) - grpc_pid_controller_last(&t->pid_controller)
+              : GPR_MIN(0, -grpc_pid_controller_last(&t->pid_controller));
       gpr_timespec now = gpr_now(GPR_CLOCK_MONOTONIC);
       gpr_timespec dt_timespec = gpr_time_sub(now, t->last_pid_update);
       double dt = (double)dt_timespec.tv_sec + dt_timespec.tv_nsec * 1e-9;
       if (dt > 3) {
         grpc_pid_controller_reset(&t->pid_controller);
       }
-      t->log2_bdp_guess +=
+      double log2_bdp_guess =
           grpc_pid_controller_update(&t->pid_controller, bdp_error, dt);
-      t->log2_bdp_guess = GPR_CLAMP(t->log2_bdp_guess, -5, 21);
       gpr_log(GPR_DEBUG, "%s: err=%lf cur=%lf pressure=%lf target=%lf",
-              t->peer_string, bdp_error, t->log2_bdp_guess, memory_pressure,
+              t->peer_string, bdp_error, log2_bdp_guess, memory_pressure,
               target);
-      update_bdp(exec_ctx, t, pow(2, t->log2_bdp_guess));
+      update_bdp(exec_ctx, t, pow(2, log2_bdp_guess));
       t->last_pid_update = now;
     }
     GRPC_CHTTP2_UNREF_TRANSPORT(exec_ctx, t, "keep_reading");

src/core/ext/transport/chttp2/transport/internal.h

@@ -330,7 +330,6 @@ struct grpc_chttp2_transport {
   /* bdp estimator */
   grpc_bdp_estimator bdp_estimator;
   grpc_pid_controller pid_controller;
-  double log2_bdp_guess;
   grpc_closure start_bdp_ping_locked;
   grpc_closure finish_bdp_ping_locked;
   gpr_timespec last_bdp_ping_finished;

src/core/lib/transport/pid_controller.c

@@ -34,7 +34,9 @@
 #include "src/core/lib/transport/pid_controller.h"
 
 void grpc_pid_controller_init(grpc_pid_controller *pid_controller,
-                              double gain_p, double gain_i, double gain_d) {
+                              double initial_control_value, double gain_p,
+                              double gain_i, double gain_d) {
+  pid_controller->last_control_value = initial_control_value;
   pid_controller->gain_p = gain_p;
   pid_controller->gain_i = gain_i;
   pid_controller->gain_d = gain_d;
@@ -48,10 +50,22 @@ void grpc_pid_controller_reset(grpc_pid_controller *pid_controller) {
 
 double grpc_pid_controller_update(grpc_pid_controller *pid_controller,
                                   double error, double dt) {
-  pid_controller->error_integral += error * dt;
+  /* integrate error using the trapezoid rule */
+  pid_controller->error_integral +=
+      dt * (pid_controller->last_error + error) * 0.5;
   double diff_error = (error - pid_controller->last_error) / dt;
-  pid_controller->last_error = error;
-  return dt * (pid_controller->gain_p * error +
+  /* calculate derivative of control value vs time */
+  double dc_dt = pid_controller->gain_p * error +
                  pid_controller->gain_i * pid_controller->error_integral +
-               pid_controller->gain_d * diff_error);
+                 pid_controller->gain_d * diff_error;
+  double new_control_value = pid_controller->last_control_value +
+                             dt * (pid_controller->last_dc_dt + dc_dt) * 0.5;
+  pid_controller->last_error = error;
+  pid_controller->last_dc_dt = dc_dt;
+  pid_controller->last_control_value = new_control_value;
+  return new_control_value;
+}
+
+double grpc_pid_controller_last(grpc_pid_controller *pid_controller) {
+  return pid_controller->last_control_value;
 }

src/core/lib/transport/pid_controller.h

@@ -47,18 +47,24 @@ typedef struct {
   double gain_d;
   double last_error;
   double error_integral;
+  double last_control_value;
+  double last_dc_dt;
 } grpc_pid_controller;
 
 /** Initialize the controller */
 void grpc_pid_controller_init(grpc_pid_controller *pid_controller,
-                              double gain_p, double gain_i, double gain_d);
+                              double initial_control_value, double gain_p,
+                              double gain_i, double gain_d);
 
 /** Reset the controller: useful when things have changed significantly */
 void grpc_pid_controller_reset(grpc_pid_controller *pid_controller);
 
 /** Update the controller: given a current error estimate, and the time since
-    the last update, returns a delta to the control value */
+    the last update, returns a new control value */
 double grpc_pid_controller_update(grpc_pid_controller *pid_controller,
                                   double error, double dt);
 
+/** Returns the last control value calculated */
+double grpc_pid_controller_last(grpc_pid_controller *pid_controller);
+
 #endif

test/core/transport/pid_controller_test.c

@@ -45,7 +45,7 @@
 static void test_noop(void) {
   gpr_log(GPR_INFO, "test_noop");
   grpc_pid_controller pid;
-  grpc_pid_controller_init(&pid, 1, 1, 1);
+  grpc_pid_controller_init(&pid, 0, 1, 1, 1);
 }
 
 static void test_simple_convergence(double gain_p, double gain_i, double gain_d,
@@ -55,15 +55,14 @@ static void test_simple_convergence(double gain_p, double gain_i, double gain_d,
           "start=%lf",
           gain_p, gain_i, gain_d, dt, set_point, start);
   grpc_pid_controller pid;
-  grpc_pid_controller_init(&pid, 0.2, 0.1, 0.1);
-
-  double current = start;
+  grpc_pid_controller_init(&pid, start, 0.2, 0.1, 0.1);
 
   for (int i = 0; i < 1000; i++) {
-    current += grpc_pid_controller_update(&pid, set_point - current, 1);
+    grpc_pid_controller_update(&pid, set_point - grpc_pid_controller_last(&pid),
+                               1);
   }
 
-  GPR_ASSERT(fabs(set_point - current) < 0.1);
+  GPR_ASSERT(fabs(set_point - grpc_pid_controller_last(&pid)) < 0.1);
   GPR_ASSERT(fabs(pid.error_integral) < 0.1);
 }