Fix cant and add gravity angle text

vision-module-gui/src/bin/gui.rs

@@ -471,7 +471,7 @@ pub fn main() -> Result<(), Box<dyn std::error::Error>> {
         move |_| {
             let view = view.c();
             let madgwick = &mut view.lock().state.madgwick;
-            *madgwick = ahrs::Madgwick::new_with_quat(1. / accel_config_signal.get().accel_odr as f32, 0.1, madgwick.quat);
+            *madgwick = ahrs::Madgwick::new_with_quat(1. / accel_config_signal.get().accel_odr as f32, 0.04, madgwick.quat);
         }
     });
 
@@ -542,7 +542,6 @@ pub fn main() -> Result<(), Box<dyn std::error::Error>> {
                 frame.fv_aimpoint_y = Some(fv_aimpoint.y);
 
                 let gravity_vec = runner.state.orientation.inverse_transform_vector(&nalgebra::Vector3::z_axis());
-                let gravity_vec = nalgebra::UnitVector3::new_unchecked(gravity_vec.xzy());
                 let gravity_angle = (f64::atan2(-gravity_vec.z as f64, -gravity_vec.x as f64) + std::f64::consts::PI/2.).to_degrees();
                 frame.opposite_cant = Some(gravity_angle);
             }

vision-module-gui/src/lib.rs

@@ -93,7 +93,7 @@ impl Default for MotState {
             wf_data: None,
             screen_id: 0,
             orientation: Rotation3::identity(),
-            madgwick: ahrs::Madgwick::new(1./100., 0.1),
+            madgwick: ahrs::Madgwick::new(1./100., 0.04),
             rotation_mat: Default::default(),
             translation_mat: Default::default(),
             nf_points: Default::default(),

vision-module-gui/src/tracking_canvas_helpers.rs

@@ -83,6 +83,14 @@ pub fn draw(ctx: UI, runner: Arc<Mutex<MotRunner>>, _area: &Area, draw_params: &
     );
 
     let gravity_rot = Rotation2::new(-gravity_angle);
+
+    draw_text(
+        &ctx,
+        20.0,
+        40.0,
+        &format!("gravity_angle = {:.3}", gravity_angle.to_degrees()),
+    );
+
     if raw {
         draw_raw(ctx, state, draw_tf, gravity_rot, &nf_path, &wf_path, &nf_grid_path, &ch_path);
     } else {