compile maybe?

Author: techcatgato

bricks/_common/mpconfigport.h

@@ -155,13 +155,19 @@ typedef long mp_off_t;
 #define PYBRICKS_VM_HOOK_LOOP_EXTRA
 #endif
 
+void pb_background_odometry_update(void);
+void pb_background_pursuit_update(void);
+
 #define MICROPY_VM_HOOK_LOOP \
     do { \
         PYBRICKS_VM_HOOK_LOOP_EXTRA \
         extern bool pbio_os_run_processes_once(void); \
         pbio_os_run_processes_once(); \
+        /* Call our experimental hooks every VM cycle */ \
+        pb_background_odometry_update(); \
+        pb_background_pursuit_update(); \
     } while (0);
-
+    
 #define MICROPY_GC_HOOK_LOOP(i) do { \
         if (((i) & 0xf) == 0) { \
             MICROPY_VM_HOOK_LOOP \

pybricks/experimental/odometry.c

@@ -40,17 +40,17 @@ volatile float sp_a = 0.0f, sp_b = 0.0f, sp_c = 0.0f, sp_d = 0.0f, sp_x_end = 0.
 void pb_background_odometry_update(void) {
     if (!odom_running || !left_servo_ptr || !right_servo_ptr) return;
 
-    uint32_t now = mp_hal_ticks_ms();
-    if (now - last_odom_time_ms < 5) return;
-    last_odom_time_ms = now;
-
     int32_t cur_l, cur_r, unused_rate;
     pbio_servo_get_state_user(left_servo_ptr, &cur_l, &unused_rate);
     pbio_servo_get_state_user(right_servo_ptr, &cur_r, &unused_rate);
 
     int32_t delta_l = cur_l - last_left_angle;
     int32_t delta_r = cur_r - last_right_angle;
 
+    // Update state immediately to prevent dropped ticks
+    last_left_angle = cur_l;
+    last_right_angle = cur_r;
+
     if (delta_l != 0 || delta_r != 0) {
         float dL = (float)delta_l * odom_deg_to_mm;
         float dR = (float)delta_r * odom_deg_to_mm;
@@ -62,11 +62,9 @@ void pb_background_odometry_update(void) {
         global_y += dD * pb_fast_sin(avg_h);
         global_h += dH;
 
+        // Keep heading within [-PI, PI]
         while (global_h > 3.14159f) global_h -= 6.28318f;
         while (global_h < -3.14159f) global_h += 6.28318f;
-
-        last_left_angle = cur_l;
-        last_right_angle = cur_r;
     }
 }
 
@@ -108,15 +106,19 @@ void pb_background_pursuit_update(void) {
 mp_obj_t experimental_start_odometry(size_t n_args, const mp_obj_t *args) {
     left_servo_ptr = ((pb_type_pupdevices_Motor_obj_t *)MP_OBJ_TO_PTR(args[0]))->srv;
     right_servo_ptr = ((pb_type_pupdevices_Motor_obj_t *)MP_OBJ_TO_PTR(args[1]))->srv;
-    odom_deg_to_mm = mp_obj_get_float(args[2]) / 360.0f;
+    
+    // Using mm_per_deg directly as passed from Python
+    odom_deg_to_mm = mp_obj_get_float(args[2]); 
     odom_inv_track = 1.0f / mp_obj_get_float(args[3]);
+    
     global_x = mp_obj_get_float(args[4]);
     global_y = mp_obj_get_float(args[5]);
     global_h = mp_obj_get_float(args[6]);
 
-    int32_t unused_rate;
-    pbio_servo_get_state_user(left_servo_ptr, (int32_t*)&last_left_angle, &unused_rate);
-    pbio_servo_get_state_user(right_servo_ptr, (int32_t*)&last_right_angle, &unused_rate);
+    int32_t unused;
+    pbio_servo_get_state_user(left_servo_ptr, (int32_t*)&last_left_angle, &unused);
+    pbio_servo_get_state_user(right_servo_ptr, (int32_t*)&last_right_angle, &unused);
+    
     odom_running = true;
     return mp_const_none;
 }