added means to inspect gyro offsets

michaelkamprath · Nov 29, 2024 · 40a796b · 40a796b
1 parent 3df92ae
commit 40a796b
Show file tree

Hide file tree

Showing 3 changed files with 46 additions and 1 deletion.
diff --git a/src/driver.rs b/src/driver.rs
@@ -218,12 +218,13 @@ impl<
         // - button one will select the current function and execute it
         // - if no button is pressed for the idle wait time, return to idle state
 
-        const FUNCTIONS: [&str; 5] = [
+        const FUNCTIONS: [&str; 6] = [
             "Select Path",
             "Heading Display",
             "Drive Straight",
             "Turn Left",
             "Turn Right",
+            "Gyro Offsets",
         ];
         let mut function_idx: usize = 0;
         let mut last_interaction_millis = millis();
@@ -298,6 +299,11 @@ impl<
                         self.robot.turn(-90);
                         self.robot.start_display_reset_timer();
                     }
+                    5 => {
+                        write!(self.robot.clear_lcd().set_lcd_cursor(0, 0), "Gyro Offsets",).ok();
+                        self.robot.display_gyro_offsets().ok();
+                        self.robot.start_display_reset_timer();
+                    }
                     _ => {
                         warn!("handle_functions_menu: invalid function index");
                     }

diff --git a/src/model/heading_core1.rs b/src/model/heading_core1.rs
@@ -114,6 +114,20 @@ impl<'a> HeadingManager<'a> {
                 }) {
                     defmt::error!("Error calibrating gyro");
                 }
+                let new_offsets: Vector3d<i16> = match gyro.get_gyro_offsets() {
+                    Ok(offsets) => Vector3d::<i16> {
+                        x: offsets.x as i16,
+                        y: offsets.y as i16,
+                        z: offsets.z as i16,
+                    },
+                    Err(_e) => {
+                        defmt::error!("Error getting gyro offsets");
+                        Vector3d::<i16> { x: 0, y: 0, z: 0 }
+                    }
+                };
+                critical_section::with(|cs| {
+                    GYRO_OFFSETS.borrow(cs).set(new_offsets);
+                });
             } else {
                 defmt::debug!(
                     "Setting gyro offsets: {{x: {}, y: {}, z: {} }}",
@@ -207,4 +221,9 @@ impl<'a> HeadingManager<'a> {
             });
         });
     }
+
+    /// get the gyro offsets
+    pub fn get_gyro_offsets(&self) -> Vector3d<i16> {
+        critical_section::with(|cs| GYRO_OFFSETS.borrow(cs).get())
+    }
 }
diff --git a/src/robot.rs b/src/robot.rs
@@ -733,6 +733,26 @@ where
 
         Ok(())
     }
+
+    pub fn display_gyro_offsets(
+        &mut self,
+    ) -> Result<(), i2c_character_display::CharacterDisplayError<CriticalSectionDevice<'a, TWI1>>>
+    {
+        let offsets = self.heading_core1.get_gyro_offsets();
+        if let Err(_e) = write!(self.lcd.clear()?.set_cursor(0, 0)?, "Gyro offsets:") {
+            error!("Error writing to LCD");
+        };
+        if let Err(_e) = write!(
+            self.lcd.set_cursor(0, 1)?,
+            "X:{} Y:{} Z:{}",
+            offsets.x,
+            offsets.y,
+            offsets.z
+        ) {
+            error!("Error writing to LCD");
+        };
+        Ok(())
+    }
     //--------------------------------------------------------------------------
     // LCD functions
     //--------------------------------------------------------------------------