diff --git a/fw/src/main.cpp b/fw/src/main.cpp
index 43d7feb..11323cf 100644
--- a/fw/src/main.cpp
+++ b/fw/src/main.cpp
@@ -129,7 +129,7 @@ void write_dual_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val1, uint8_t val2) {
 SemaphoreHandle_t mutex;
 struct UdralServoState servo_state = {0};
 
-void mutexed_state_sync(UdralServoState *const state) {
+void mutexed_state_sync(UdralServoState *const state, const float hz) {
   xSemaphoreTake(mutex, portMAX_DELAY);
   servo_state.target_force = state->target_force;
   servo_state.target_acceleration = state->target_acceleration;
@@ -137,6 +137,19 @@ void mutexed_state_sync(UdralServoState *const state) {
   servo_state.target_position = state->target_position;
   servo_state.arming = state->arming;
   servo_state.pid = state->pid;
+
+  float curr_angle = servo_state.curr_position;
+  static float curr_angle_last = curr_angle;
+  float curr_vel = (curr_angle - curr_angle_last) * hz;
+  curr_angle_last = curr_angle;
+
+  static float curr_vel_last = curr_vel;
+  float curr_accel =(curr_vel - curr_vel_last) * hz;
+  curr_vel_last = curr_vel;
+
+  servo_state.curr_acceleration = curr_accel;
+  servo_state.curr_velocity = curr_vel;
+
   *state = servo_state;
   xSemaphoreGive(mutex);
 }
@@ -273,34 +286,21 @@ void loop() {
 #ifdef MOTOR
   bool armed = false;
 
-  float curr_pcb_temp_kelvin = temp_pcb + 273.15f;
+  /* adc reading */
+  drv8323s.focdriver->voltage_power_supply = vdrive_read;
+  const float curr_pcb_temp_kelvin = temp_pcb + 273.15f;
+
   xSemaphoreTake(mutex, portMAX_DELAY);
   armed = servo_state.arming.armed;
   float target = servo_state.target_force;
 
-  servo_state.controller_temperature = curr_pcb_temp_kelvin; 
+  servo_state.controller_temperature = curr_pcb_temp_kelvin;
   servo_state.motor_temperature = NAN;
 
   float curr_angle = encoder_calibrated.getAngle();
-  float curr_vel = encoder_calibrated.getVelocity();
-
-  unsigned long now = millis();
-  static unsigned long last_update = now;
-  unsigned long tdelta = now - last_update;
-  last_update = now;
-  float curr_accel =
-      tdelta ? (curr_vel - servo_state.curr_velocity) / tdelta : 0;
-
-  servo_state.curr_acceleration = curr_accel;
-  servo_state.curr_velocity = curr_vel;
   servo_state.curr_position = curr_angle;
   servo_state.curr_force = servo_state.target_force;
 
-
-  servo_state.curr_velocity = encoder.getAngle();
-  servo_state.curr_acceleration = encoder_absolute.getAngle();
-  servo_state.curr_force = abs_inc_sensor.getAngle();
-
   servo_state.vcc_volts = drv8323s.focdriver->voltage_power_supply;
   servo_state.current = NAN;
   xSemaphoreGive(mutex);
@@ -320,7 +320,6 @@ void loop() {
   }
 
   digitalWrite(LED_PIN, armed ? LOW : HIGH);
-  drv8323s.focdriver->voltage_power_supply = vdrive_read;
 
   motor.loopFOC();
 #else
diff --git a/fw/src/udral_servo.hpp b/fw/src/udral_servo.hpp
index c782aa6..6b5e8ff 100644
--- a/fw/src/udral_servo.hpp
+++ b/fw/src/udral_servo.hpp
@@ -100,7 +100,7 @@ struct UdralServoState {
   };
 };
 
-typedef void (*state_sync_f)(UdralServoState *const);
+typedef void (*state_sync_f)(UdralServoState *const, const float hz);
 
 struct UdralServoInternalState {
   CanardMicrosecond started_at;
@@ -1067,17 +1067,19 @@ int udral_loop(state_sync_f servo_state_sync_f)
 
     // Now the node is initialized and we're ready to roll.
     state.started_at = getMonotonicMicroseconds();
-    const CanardMicrosecond fast_loop_period = MEGA / 50;
+    const unsigned int hz = 50;
+    const CanardMicrosecond fast_loop_period = MEGA / hz;
     CanardMicrosecond next_fast_iter_at = state.started_at + fast_loop_period;
     CanardMicrosecond next_1_hz_iter_at = state.started_at + MEGA;
     CanardMicrosecond next_01_hz_iter_at = state.started_at + MEGA * 10;
     do
     {
-        state.user_state_sync_f(&state.servo);
+        vTaskDelay(1);
         // Run a trivial scheduler polling the loops that run the business logic.
         CanardMicrosecond now_usec = getMonotonicMicroseconds();
         if (now_usec >= next_fast_iter_at)
         {
+            state.user_state_sync_f(&state.servo, hz);
             next_fast_iter_at += fast_loop_period;
             handleFastLoop(&state, now_usec);
         }