Commit 9bb053ff authored by jxchenlu's avatar jxchenlu
Browse files

clamp radians, gyrodometry

parent a8304ab2
......@@ -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;}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment