use std::time::{SystemTime, UNIX_EPOCH};
use prost::Message;
use crate::types::{BatteryReading, CalibrationReading, DiagnosticsReading, FrameReading, SensorReading};
use crate::wire;
fn now_ms() -> f64 {
SystemTime::now()
.duration_since(UNIX_EPOCH)
.expect("system clock before epoch")
.as_secs_f64()
* 1000.0
}
pub fn is_valid_frame(data: &[u8]) -> bool {
if data.is_empty() {
return false;
}
wire::Frame::decode(data).is_ok()
}
pub fn parse_frame(data: &[u8]) -> Option<FrameReading> {
if data.is_empty() {
return None;
}
let frame = wire::Frame::decode(data).ok()?;
Some(FrameReading {
timestamp: now_ms(),
acc_x: frame.acc_x,
acc_y: frame.acc_y,
acc_z: frame.acc_z,
ang_x: frame.ang_x,
ang_y: frame.ang_y,
ang_z: frame.ang_z,
temperature: frame.temp,
ir_left: frame.ir_l,
red_left: frame.red_l,
amb_left: frame.amb_l,
ir_right: frame.ir_r,
red_right: frame.red_r,
amb_right: frame.amb_r,
ir_pulse: frame.ir_p,
red_pulse: frame.red_p,
amb_pulse: frame.amb_p,
})
}
pub fn parse_adc(data: &[u8]) -> Option<BatteryReading> {
let adc = wire::Adc::decode(data).ok()?;
Some(BatteryReading {
timestamp: now_ms(),
voltage_mv: adc.voltage,
charging: adc.charging,
usb_connected: adc.usb,
})
}
pub fn parse_calibration(data: &[u8]) -> Option<CalibrationReading> {
let cal = wire::Calibration::decode(data).ok()?;
Some(CalibrationReading {
timestamp: now_ms(),
offset_left: cal.offset_l,
offset_right: cal.offset_r,
offset_pulse: cal.offset_p,
auto_calibration: cal.enable,
low_power_mode: cal.low_power_mode,
})
}
pub fn parse_diagnostics(data: &[u8]) -> Option<DiagnosticsReading> {
let diag = wire::Diagnostics::decode(data).ok()?;
let adc = diag.adc.map(|a| BatteryReading {
timestamp: now_ms(),
voltage_mv: a.voltage,
charging: a.charging,
usb_connected: a.usb,
});
Some(DiagnosticsReading {
timestamp: now_ms(),
adc,
imu_ok: diag.imu_ok,
sensor_ok: diag.sensor_ok,
})
}
pub fn parse_sensor(data: &[u8]) -> Option<SensorReading> {
let sensor = wire::Sensor::decode(data).ok()?;
Some(SensorReading {
timestamp: now_ms(),
address: sensor.address,
data: sensor.data,
})
}