fps cap and counter

Author: techcatgato

pybricks/experimental/odometry.c

@@ -27,6 +27,8 @@ static pbio_servo_t *right_servo_ptr = NULL;
 
 volatile bool odom_running = false;
 volatile uint32_t last_odom_time_ms = 0;
+volatile uint32_t last_pursuit_time_ms = 0;
+
 volatile float global_x = 0.0f, global_y = 0.0f, global_h = 0.0f;
 volatile int32_t last_left_angle = 0, last_right_angle = 0;
 float odom_deg_to_mm = 1.0f;
@@ -37,8 +39,32 @@ volatile float p_target_speed = 0.0f;
 volatile float p_lookahead = 120.0f;
 volatile float sp_a = 0.0f, sp_b = 0.0f, sp_c = 0.0f, sp_d = 0.0f, sp_x_end = 0.0f;
 
+// FPS Counter Variables
+volatile uint32_t vm_loop_counter = 0;
+volatile uint32_t current_fps = 0;
+volatile uint32_t last_fps_time_ms = 0;
+
 void pb_background_odometry_update(void) {
-    if (!odom_running || !left_servo_ptr || !right_servo_ptr) return;
+    if (!odom_running) return;
+
+    // --- FPS COUNTER LOGIC ---
+    vm_loop_counter++;
+    uint32_t now = mp_hal_ticks_ms();
+    
+    // Every 1000ms, save the count and reset
+    if (now - last_fps_time_ms >= 1000) {
+        current_fps = vm_loop_counter;
+        vm_loop_counter = 0;
+        last_fps_time_ms = now;
+    }
+    // -------------------------
+
+    if (!left_servo_ptr || !right_servo_ptr) return;
+
+    // --- RATE CAP: 200 Hz (5ms) ---
+    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);
@@ -71,6 +97,12 @@ void pb_background_odometry_update(void) {
 void pb_background_pursuit_update(void) {
     if (!pursuit_running || !left_servo_ptr || !right_servo_ptr) return;
 
+    // --- RATE CAP: 100 Hz (10ms) ---
+    uint32_t now = mp_hal_ticks_ms();
+    if (now - last_pursuit_time_ms < 10) return;
+    last_pursuit_time_ms = now;
+    // -------------------------------
+
     if (global_x >= sp_x_end) {
         pursuit_running = false;
         pbio_servo_stop(left_servo_ptr, PBIO_CONTROL_ON_COMPLETION_BRAKE);
@@ -158,4 +190,8 @@ mp_obj_t experimental_stop_odometry(void) {
     return mp_const_none;
 }
 
+mp_obj_t experimental_get_fps(void) {
+    return mp_obj_new_int_from_uint(current_fps);
+}
+
 #endif // PYBRICKS_PY_EXPERIMENTAL

pybricks/experimental/pb_module_experimental.c

@@ -1,3 +1,4 @@
+// SPDX-License-Identifier: MIT
 #include "py/mpconfig.h"
 #include "py/obj.h"
 #include "py/runtime.h"
@@ -10,13 +11,18 @@ extern mp_obj_t experimental_get_odometry(void);
 extern mp_obj_t experimental_stop_odometry(void);
 extern mp_obj_t experimental_start_pursuit(size_t n_args, const mp_obj_t *args);
 extern mp_obj_t experimental_stop_pursuit(void);
+extern mp_obj_t experimental_get_fps(void);
 
 // Object definitions
 static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(experimental_start_odometry_obj, 7, 7, experimental_start_odometry);
 static MP_DEFINE_CONST_FUN_OBJ_0(experimental_get_odometry_obj, experimental_get_odometry);
 static MP_DEFINE_CONST_FUN_OBJ_0(experimental_stop_odometry_obj, experimental_stop_odometry);
-static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(experimental_start_pursuit_obj, 4, 4, experimental_start_pursuit);
+
+// FIXED: Pursuit takes 7 arguments (a, b, c, d, x_end, speed, lookahead), not 4.
+static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(experimental_start_pursuit_obj, 7, 7, experimental_start_pursuit); 
+
 static MP_DEFINE_CONST_FUN_OBJ_0(experimental_stop_pursuit_obj, experimental_stop_pursuit);
+static MP_DEFINE_CONST_FUN_OBJ_0(experimental_get_fps_obj, experimental_get_fps);
 
 const mp_rom_map_elem_t pb_module_experimental_globals_table[] = {
     { MP_ROM_QSTR(MP_QSTR___name__),          MP_ROM_QSTR(MP_QSTR_experimental) },
@@ -25,6 +31,7 @@ const mp_rom_map_elem_t pb_module_experimental_globals_table[] = {
     { MP_ROM_QSTR(MP_QSTR_stop_odometry),     MP_ROM_PTR(&experimental_stop_odometry_obj) },
     { MP_ROM_QSTR(MP_QSTR_start_pursuit),     MP_ROM_PTR(&experimental_start_pursuit_obj) },
     { MP_ROM_QSTR(MP_QSTR_stop_pursuit),      MP_ROM_PTR(&experimental_stop_pursuit_obj) },
+    { MP_ROM_QSTR(MP_QSTR_get_fps),           MP_ROM_PTR(&experimental_get_fps_obj) },
 };
 MP_DEFINE_CONST_DICT(pb_module_experimental_globals, pb_module_experimental_globals_table);