Move PID control to the IMU callback

Author: lukash

src/main.c

@@ -160,17 +160,13 @@ static void main_freq_update_reconfigure(float frequency) {
     Data *d = (Data *) ARG;
 
     motor_data_configure(&d->motor, d->float_conf.atr_filter, frequency);
-    pid_configure(&d->pid, frequency);
     motor_control_configure(&d->motor_control, &d->float_conf, frequency);
 
     torque_tilt_configure(&d->torque_tilt, &d->float_conf, frequency);
     atr_configure(&d->atr, &d->float_conf, frequency);
     brake_tilt_configure(&d->brake_tilt, &d->float_conf, frequency);
     turn_tilt_configure(&d->turn_tilt, &d->float_conf, frequency);
     remote_configure(&d->remote, &d->float_conf, frequency);
-    booster_configure(&d->booster, frequency);
-
-    ema_configure(&d->balance_current, 25.0f, frequency);
 
     log_msg(
         "Main freq reconfgure old: %dHz new: %dHz",
@@ -183,6 +179,10 @@ static void main_freq_update_reconfigure(float frequency) {
 static void imu_freq_update_reconfigure(float frequency) {
     Data *d = (Data *) ARG;
 
+    pid_configure(&d->pid, frequency);
+    booster_configure(&d->booster, frequency);
+    ema_configure(&d->balance_current, 25.0f, frequency);
+
     log_msg(
         "IMU freq reconfgure old: %dHz new: %dHz",
         (int32_t) d->imu_t.filter_current_frequency,
@@ -822,11 +822,22 @@ static void imu_ref_callback(float *acc, float *gyro, float *mag, float dt) {
     Data *d = (Data *) ARG;
 
     time_t time = VESC_IF->system_time_ticks();
-    unused(time);
 
     balance_filter_update(&d->balance_filter, gyro, acc, dt);
 
     frequency_tracker_update(&d->imu_t, dt);
+
+    imu_update(&d->imu, &d->balance_filter, &d->state);
+
+    if (d->state.state == STATE_RUNNING) {
+        pid_control(d, dt);
+    }
+
+    motor_control_apply(
+        &d->motor_control, d->motor.abs_erpm_smooth.value, d->state.state, &d->time
+    );
+
+    data_recorder_sample(&d->data_record, d, time);
 }
 
 static void refloat_thd(void *arg) {
@@ -850,8 +861,6 @@ static void refloat_thd(void *arg) {
 
         time_update(&d->time, d->state.state);
 
-        imu_update(&d->imu, &d->balance_filter, &d->state);
-
         beeper_update(d);
 
         charging_timeout(&d->charging, &d->state);
@@ -992,8 +1001,6 @@ static void refloat_thd(void *arg) {
                 }
             }
 
-            pid_control(d, dt);
-
             break;
         case (STATE_READY):
             if (d->state.mode == MODE_FLYWHEEL) {
@@ -1113,12 +1120,6 @@ static void refloat_thd(void *arg) {
             break;
         }
 
-        motor_control_apply(
-            &d->motor_control, d->motor.abs_erpm_smooth.value, d->state.state, &d->time
-        );
-
-        data_recorder_sample(&d->data_record, d, d->time.now);
-
         systime_t remaining = start_time - VESC_IF->system_time_ticks() + d->main_loop_ticks;
         if (remaining > 0 && remaining <= d->main_loop_ticks) {
             sleep_ticks = remaining;