ank/cruisecontrol
1
Fork 0
Code Issues Activity

graceful decay on gyro failure

Browse source 
also switched gyro iic pins because the other ones were
shorted under the pico :|
This commit is contained in:
Andy Killorin 2025-03-05 21:10:17 -05:00
parent b063a41783
commit 971c57fa13
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
2 changed files with 26 additions and 17 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
controller/src/main.rs
View file

@ -104,8 +104,8 @@ async fn main(spawner: Spawner) {
f.compare_b = 0; f.compare_b = 0;
let flip_pwm = Pwm::new_output_b(p.PWM_SLICE0, p.PIN_17, f.clone()); let flip_pwm = Pwm::new_output_b(p.PWM_SLICE0, p.PIN_17, f.clone());
let sda = p.PIN_20; let sda = p.PIN_8;
let scl = p.PIN_21; let scl = p.PIN_9;
let config = embassy_rp::i2c::Config::default(); let config = embassy_rp::i2c::Config::default();
let bus = embassy_rp::i2c::I2c::new_async(p.I2C0, scl, sda, Irqs, config); let bus = embassy_rp::i2c::I2c::new_async(p.I2C0, scl, sda, Irqs, config);

39
controller/src/sensor_manager.rs
View file

@ -9,7 +9,7 @@ use embassy_time::{Delay, Duration, Instant, Timer};
use embassy_rp::gpio::Output; use embassy_rp::gpio::Output;
use embassy_rp::i2c::{Async, I2c}; use embassy_rp::i2c::{Async, I2c};
use embedded_hal_bus::i2c::RefCellDevice; use embedded_hal_bus::i2c::RefCellDevice;
use log::info; use log::{error, info};
use mpu6050::Mpu6050; use mpu6050::Mpu6050;
use nalgebra::Vector3; use nalgebra::Vector3;
@ -62,10 +62,19 @@ async fn init_sensors(mut hardware: SensorHardware) {
tof_r.set_address(0x34).unwrap(); tof_r.set_address(0x34).unwrap();
} }
//let mut gyro = Mpu6050::new_with_addr(hardware.bus_gyro,0x68); info!("initting gyro");
//gyro.init(&mut Delay).unwrap(); let mut gyro = Mpu6050::new_with_addr(hardware.bus_gyro,0x68);
//gyro.set_gyro_range(mpu6050::device::GyroRange::D2000).unwrap(); let mut gyro_alive = true;
//gyro.set_accel_range(mpu6050::device::AccelRange::G16).unwrap(); 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 { 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 gyro_alive {
//if let Ok(gyro) = gyro.get_gyro() { POLL_STATE.store(PollState::Gyro as u8, Ordering::SeqCst);
// info!("rotation: {gyro}"); if let Ok(gyro) = gyro.get_gyro() {
// data.gyro = Some(gyro); info!("rotation: {gyro}");
// updated = true; data.gyro = Some(gyro);
//} }
//if let Ok(accel) = gyro.get_acc() { if let Ok(accel) = gyro.get_acc() {
// data.accel = Some(accel); data.accel = Some(accel);
// updated = true; }
//} }
POLL_STATE.store(PollState::None as u8, Ordering::SeqCst); POLL_STATE.store(PollState::None as u8, Ordering::SeqCst);
info!("sensors: {data:?}"); info!("sensors: {data:?}");