Reduce calculation cost in PWM interrupt handler

tfrog-motordriver/controlPWM.c

@@ -111,12 +111,13 @@ void controlPWM_config(int i)
 
   // initialize encoder zero position buffer
   motor[i].enc0_buf_len =
-      motor_param[i].enc_denominator < ENC0_BUF_MAX_DENOMINATOR ?
+      motor_param[i].enc_denominator < ENC0_DENOMINATOR_MAX ?
           motor_param[i].enc_denominator * 6 :
           ENC0_BUF_MAX;
   int j;
   for (j = 0; j < motor[i].enc0_buf_len; ++j)
     motor[i].enc0_buf[j] = ENC0_BUF_UNKNOWN;
+  motor[i].enc0_buf_updated = 0;
 
   motor_param[i].enc_drev[0] = motor_param[i].enc_rev * 1 / 6;
   motor_param[i].enc_drev[1] = motor_param[i].enc_rev * 2 / 6;
@@ -629,26 +630,9 @@ void FIQ_PWMPeriod()
       enc0 -= motor[i].vel1 * motor_param[i].hall_delay_factor / 32768;
 
       // Fill enc0_buf and calculate average
-      if (hall_pos >= ENC0_BUF_MAX)
-        hall_pos = hall_pos % ENC0_BUF_MAX;
-
+      hall_pos = hall_pos % ENC0_BUF_MAX;
       motor[i].enc0_buf[hall_pos] = enc0;
-      int j;
-      int sum_enc0_err = 0, num_enc0 = 0;
-      for (j = 0; j < motor[i].enc0_buf_len; ++j)
-      {
-        if (motor[i].enc0_buf[j] != ENC0_BUF_UNKNOWN)
-        {
-          int err = motor[i].enc0_buf[j] - enc0;
-          normalize(&err, -motor_param[i].enc_rev_h, motor_param[i].enc_rev);
-
-          sum_enc0_err += err;
-          num_enc0++;
-        }
-      }
-      enc0 += (sum_enc0_err / num_enc0);
-      normalize(&enc0, 0, motor_param[i].enc_rev);
-      motor_param[i].enc0 = enc0;
+      motor[i].enc0_buf_updated = 1;
     }
   }
 

tfrog-motordriver/controlVelocity.c

@@ -363,6 +363,30 @@ void timer0_vel_calc()
         FilterExp_FilterAngle(
             &motor[i].enc0_lpf, motor_param[i].enc0,
             motor_param[i].enc_rev, motor_param[i].enc_rev_raw);
+
+    // Calculate 1 rev average of encoder origin
+    if (!motor[i].enc0_buf_updated)
+      continue;
+
+    motor[i].enc0_buf_updated = 0;
+
+    int enc0 = motor_param[i].enc0;
+    int sum_enc0_err = 0, num_enc0 = 0;
+    for (int j = 0; j < motor[i].enc0_buf_len; ++j)
+    {
+      const int e0 = motor[i].enc0_buf[j];
+      if (e0 != ENC0_BUF_UNKNOWN)
+      {
+        int err = e0 - enc0;
+        normalize(&err, -motor_param[i].enc_rev_h, motor_param[i].enc_rev);
+
+        sum_enc0_err += err;
+        num_enc0++;
+      }
+    }
+    enc0 += sum_enc0_err / num_enc0;
+    normalize(&enc0, 0, motor_param[i].enc_rev_raw);
+    motor_param[i].enc0 = enc0;
   }
 
   ISR_VelocityControl();

tfrog-motordriver/controlVelocity.h

@@ -41,10 +41,15 @@ typedef enum _ErrorID
 } ErrorID;
 #define ERROR_NUM 6
 
-#define ENC0_BUF_MAX_DENOMINATOR 16  // up to 16 electric revolution
-#define ENC0_BUF_MAX (ENC0_BUF_MAX_DENOMINATOR * 6)
+// ENC0_BUF_MAX must be 2^n to reduce computation cost.
+#define ENC0_BUF_MAX 64
+#define ENC0_DENOMINATOR_MAX ((int)(ENC0_BUF_MAX / 6))
 #define ENC0_BUF_UNKNOWN 0x7FFFFFF
 
+static_assert(
+    ENC0_BUF_MAX && ((ENC0_BUF_MAX & (ENC0_BUF_MAX - 1)) == 0),
+    "ENC0_BUF_MAX must be 2^n");
+
 #define ACCEL_FILTER_TIME 15  // Timeconstant in velocity control steps
 #define ENC0_FILTER_TIME 32   // Timeconstant in velocity control steps
 
@@ -60,8 +65,6 @@ typedef struct _MotorState
   unsigned int spd_cnt;
   unsigned short enc;
   short dir;
-  int enc0_buf[ENC0_BUF_MAX];
-  unsigned short enc0_buf_len;
 
   struct
   {
@@ -80,6 +83,11 @@ typedef struct _MotorState
   int error;
   int64_t error_integ;
   char control_init;
+
+  char enc0_buf_updated;
+  int enc0_buf[ENC0_BUF_MAX];
+  unsigned short enc0_buf_len;
+
   YPSpur_servo_level servo_level;
   ErrorID error_state;
   FilterExp enc0_lpf;