ank/cruisecontrol
1
Fork 0
Code Issues Activity

gracefully handle senosr death

Browse source
This commit is contained in:
Andy Killorin 2025-03-05 12:55:57 -05:00
parent 6192627e25
commit 31c847f39a
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
1 changed files with 25 additions and 34 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

47
controller/src/sensor_manager.rs
View file

@ -34,9 +34,7 @@ static POLL_STATE: AtomicU8 = AtomicU8::new(PollState::None as u8);
#[repr(u8)]
enum PollState {
None,
TofL,
TofR,
TofS,
Tof,
Gyro,
}
@ -45,18 +43,24 @@ async fn init_sensors(mut hardware: SensorHardware) {
let bus = RefCell::new(hardware.bus_tof);
Timer::after_millis(1).await;
let mut tof_s = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).unwrap();
let mut tof_s = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).ok();
if let Some(ref mut tof_s) = tof_s {
tof_s.set_address(0x32).unwrap();
}
hardware.tof_l_enable.set_high();
Timer::after_micros(1200).await; // DS11555 3.2
let mut tof_l = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).unwrap();
let mut tof_l = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).ok();
if let Some(ref mut tof_l) = tof_l {
tof_l.set_address(0x33).unwrap();
}
hardware.tof_r_enable.set_high();
Timer::after_micros(1200).await; // DS11555 3.2
let mut tof_r = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).unwrap();
let mut tof_r = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).ok();
if let Some(ref mut tof_r) = tof_r {
tof_r.set_address(0x34).unwrap();
}
//let mut gyro = Mpu6050::new_with_addr(hardware.bus_gyro,0x68);
//gyro.init(&mut Delay).unwrap();
@ -71,30 +75,20 @@ async fn init_sensors(mut hardware: SensorHardware) {
gyro: None,
accel: None,
};
let mut updated_ts = Instant::now();
loop {
let mut updated = false;
POLL_STATE.store(PollState::TofS as u8, Ordering::SeqCst);
if let Ok(dist) = tof_s.read_range_single_millimeters_blocking() {
data.tof_s = Some(dist);
updated = true;
POLL_STATE.store(PollState::Tof as u8, Ordering::SeqCst);
for (tof, store) in
[(&mut tof_s, &mut data.tof_s), (&mut tof_l, &mut data.tof_l), (&mut tof_r, &mut data.tof_r)] {
if let Some(ref mut tof) = tof {
if let Ok(dist) = tof.read_range_single_millimeters_blocking() {
*store = Some(dist);
} else {
*store = None;
}
Timer::after_millis(3).await;
POLL_STATE.store(PollState::TofL as u8, Ordering::SeqCst);
if let Ok(dist) = tof_l.read_range_single_millimeters_blocking() {
data.tof_l = Some(dist);
updated = true;
}
Timer::after_millis(3).await;
POLL_STATE.store(PollState::TofR as u8, Ordering::SeqCst);
if let Ok(dist) = tof_r.read_range_single_millimeters_blocking() {
data.tof_r = Some(dist);
updated = true;
}
Timer::after_millis(3).await;
//POLL_STATE.store(PollState::Gyro as u8, Ordering::SeqCst);
//if let Ok(gyro) = gyro.get_gyro() {
@ -106,12 +100,9 @@ async fn init_sensors(mut hardware: SensorHardware) {
// data.accel = Some(accel);
// updated = true;
//}
if updated {
updated_ts = Instant::now();
}
POLL_STATE.store(PollState::None as u8, Ordering::SeqCst);
info!("sensors: {data:?}");
//CHANNEL.send((data.clone(), updated_ts.clone())).await;
CHANNEL.send((data.clone(), Instant::now())).await;
}
}