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;
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);

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::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:?}");