clamp radians, gyrodometry

common/mb_odometry.c

@@ -54,7 +54,7 @@ void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
 
     // Gyro: we only need to use yaw
     // Since each time step is small, no need to integrate
-    float del_theta_gyro = mb_state->gyro[2] * (PI / 180.0) * DT;
+    float del_theta_gyro = mb_clamp_radians(mb_state->gyro[2] * (PI / 180.0) * DT);
     float del_go = del_theta_gyro - del_theta;
     if (Gyro == T){
         if (abs(del_go) > del_thesh){mb_odometry->theta += del_theta_gyro * DT;}