diff --git a/src/driver.rs b/src/driver.rs index ac32f5b..5a2669a 100644 --- 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 index 7245913..7287ea6 100644 --- 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 = match gyro.get_gyro_offsets() { + Ok(offsets) => Vector3d:: { + x: offsets.x as i16, + y: offsets.y as i16, + z: offsets.z as i16, + }, + Err(_e) => { + defmt::error!("Error getting gyro offsets"); + Vector3d:: { 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 { + critical_section::with(|cs| GYRO_OFFSETS.borrow(cs).get()) + } } diff --git a/src/robot.rs b/src/robot.rs index fffa622..596fd99 100644 --- 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>> + { + 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 //--------------------------------------------------------------------------