From 971c57fa13c8122e107291202b97bd608e895ae9 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Wed, 5 Mar 2025 21:10:17 -0500
Subject: [PATCH] graceful decay on gyro failure

also switched gyro iic pins because the other ones were
shorted under the pico :|
---
 controller/src/main.rs           |  4 ++--
 controller/src/sensor_manager.rs | 39 ++++++++++++++++++++------------
 2 files changed, 26 insertions(+), 17 deletions(-)

diff --git a/controller/src/main.rs b/controller/src/main.rs
index 07d3502..539c4e9 100644
--- a/controller/src/main.rs
+++ b/controller/src/main.rs
@@ -104,8 +104,8 @@ async fn main(spawner: Spawner) {
     f.compare_b = 0;
     let flip_pwm = Pwm::new_output_b(p.PWM_SLICE0, p.PIN_17, f.clone());
 
-    let sda = p.PIN_20;
-    let scl = p.PIN_21;
+    let sda = p.PIN_8;
+    let scl = p.PIN_9;
     let config = embassy_rp::i2c::Config::default();
     let bus = embassy_rp::i2c::I2c::new_async(p.I2C0, scl, sda, Irqs, config);
 
diff --git a/controller/src/sensor_manager.rs b/controller/src/sensor_manager.rs
index 7622946..b5001d4 100644
--- a/controller/src/sensor_manager.rs
+++ b/controller/src/sensor_manager.rs
@@ -9,7 +9,7 @@ use embassy_time::{Delay, Duration, Instant, Timer};
 use embassy_rp::gpio::Output;
 use embassy_rp::i2c::{Async, I2c};
 use embedded_hal_bus::i2c::RefCellDevice;
-use log::info;
+use log::{error, info};
 use mpu6050::Mpu6050;
 use nalgebra::Vector3;
 
@@ -62,10 +62,19 @@ async fn init_sensors(mut hardware: SensorHardware) {
         tof_r.set_address(0x34).unwrap();
     }
 
-    //let mut gyro = Mpu6050::new_with_addr(hardware.bus_gyro,0x68);
-    //gyro.init(&mut Delay).unwrap();
-    //gyro.set_gyro_range(mpu6050::device::GyroRange::D2000).unwrap();
-    //gyro.set_accel_range(mpu6050::device::AccelRange::G16).unwrap();
+    info!("initting gyro");
+    let mut gyro = Mpu6050::new_with_addr(hardware.bus_gyro,0x68);
+    let mut gyro_alive = true;
+    match gyro.init(&mut Delay) {
+        Ok(()) => {
+            gyro.set_gyro_range(mpu6050::device::GyroRange::D2000).unwrap_or_else(|_| gyro_alive = false);
+            gyro.set_accel_range(mpu6050::device::AccelRange::G16).unwrap_or_else(|_| gyro_alive = false);
+            info!("init gyro");
+        }
+        e => {
+            error!("gyro init err: {e:?}");
+        }
+    }
 
 
     let mut data = SensorData {
@@ -90,16 +99,16 @@ async fn init_sensors(mut hardware: SensorHardware) {
             }
         }
 
-        //POLL_STATE.store(PollState::Gyro as u8, Ordering::SeqCst);
-        //if let Ok(gyro) = gyro.get_gyro() {
-        //    info!("rotation: {gyro}");
-        //    data.gyro = Some(gyro);
-        //    updated = true;
-        //}
-        //if let Ok(accel) = gyro.get_acc() {
-        //    data.accel = Some(accel);
-        //    updated = true;
-        //}
+        if gyro_alive {
+            POLL_STATE.store(PollState::Gyro as u8, Ordering::SeqCst);
+            if let Ok(gyro) = gyro.get_gyro() {
+                info!("rotation: {gyro}");
+                data.gyro = Some(gyro);
+            }
+            if let Ok(accel) = gyro.get_acc() {
+                data.accel = Some(accel);
+            }
+        }
         POLL_STATE.store(PollState::None as u8, Ordering::SeqCst);
 
         info!("sensors: {data:?}");