cleanups
This commit is contained in:
314
Software/MainBoard/rust/src/hal/v4_sensor.rs
Normal file
314
Software/MainBoard/rust/src/hal/v4_sensor.rs
Normal file
@@ -0,0 +1,314 @@
|
||||
use canapi::id::{classify, plant_id, MessageKind, IDENTIFY_CMD_OFFSET};
|
||||
use crate::bail;
|
||||
use crate::fat_error::{ContextExt, FatError, FatResult};
|
||||
use canapi::{SensorSlot};
|
||||
use crate::hal::{DetectionResult, Moistures, Sensor};
|
||||
use crate::hal::Box;
|
||||
use crate::log::{LogMessage, LOG_ACCESS};
|
||||
use alloc::format;
|
||||
use alloc::string::ToString;
|
||||
use async_trait::async_trait;
|
||||
use bincode::config;
|
||||
use embassy_embedded_hal::shared_bus::blocking::i2c::I2cDevice;
|
||||
use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
|
||||
use embassy_time::{Instant, Timer, WithTimeout};
|
||||
use embedded_can::{Frame, Id};
|
||||
use esp_hal::gpio::Output;
|
||||
use esp_hal::i2c::master::I2c;
|
||||
use esp_hal::pcnt::unit::Unit;
|
||||
use esp_hal::twai::{EspTwaiFrame, StandardId, Twai, TwaiConfiguration};
|
||||
use esp_hal::{Blocking};
|
||||
use log::{error, info, warn};
|
||||
use pca9535::{GPIOBank, Pca9535Immediate, StandardExpanderInterface};
|
||||
|
||||
const REPEAT_MOIST_MEASURE: usize = 10;
|
||||
|
||||
|
||||
|
||||
#[async_trait(?Send)]
|
||||
pub trait SensorInteraction {
|
||||
async fn measure_moisture_hz(&mut self) -> FatResult<Moistures>;
|
||||
}
|
||||
|
||||
const MS0: u8 = 1_u8;
|
||||
const MS1: u8 = 0_u8;
|
||||
const MS2: u8 = 3_u8;
|
||||
const MS3: u8 = 4_u8;
|
||||
const MS4: u8 = 2_u8;
|
||||
const SENSOR_ON: u8 = 5_u8;
|
||||
|
||||
pub enum SensorImpl {
|
||||
PulseCounter {
|
||||
signal_counter: Unit<'static, 0>,
|
||||
sensor_expander:
|
||||
Pca9535Immediate<I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Blocking>>>,
|
||||
},
|
||||
CanBus {
|
||||
twai_config: Option<TwaiConfiguration<'static, Blocking>>,
|
||||
can_power: Output<'static>,
|
||||
},
|
||||
}
|
||||
|
||||
#[async_trait(?Send)]
|
||||
impl SensorInteraction for SensorImpl {
|
||||
async fn measure_moisture_hz(&mut self) -> FatResult<Moistures> {
|
||||
match self {
|
||||
SensorImpl::PulseCounter {
|
||||
signal_counter,
|
||||
sensor_expander,
|
||||
..
|
||||
} => {
|
||||
let mut result = Moistures::default();
|
||||
for plant in 0..crate::hal::PLANT_COUNT{
|
||||
result.sensor_a_hz[plant] = Self::inner_pulse(plant, Sensor::A, signal_counter, sensor_expander).await?;
|
||||
info!("Sensor {} {:?}: {}", plant, Sensor::A, result.sensor_a_hz[plant]);
|
||||
result.sensor_b_hz[plant] = Self::inner_pulse(plant, Sensor::B, signal_counter, sensor_expander).await?;
|
||||
info!("Sensor {} {:?}: {}", plant, Sensor::B, result.sensor_b_hz[plant]);
|
||||
}
|
||||
Ok(result)
|
||||
}
|
||||
|
||||
SensorImpl::CanBus {
|
||||
twai_config,
|
||||
can_power,
|
||||
} => {
|
||||
can_power.set_high();
|
||||
let config = twai_config.take().expect("twai config not set");
|
||||
let mut twai = config.start();
|
||||
|
||||
loop {
|
||||
let rec = twai.receive();
|
||||
match rec {
|
||||
Ok(_) => {}
|
||||
Err(err) => {
|
||||
info!("Error receiving CAN message: {:?}", err);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Timer::after_millis(10).await;
|
||||
let can = Self::inner_can(&mut twai).await;
|
||||
|
||||
can_power.set_low();
|
||||
|
||||
let config = twai.stop();
|
||||
twai_config.replace(config);
|
||||
|
||||
let value = can?;
|
||||
Ok(value)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
impl SensorImpl {
|
||||
pub async fn autodetect(&mut self) -> FatResult<DetectionResult> {
|
||||
match self {
|
||||
SensorImpl::PulseCounter { .. } => {
|
||||
bail!("Only CAN bus implementation supports autodetection")
|
||||
}
|
||||
SensorImpl::CanBus {
|
||||
twai_config,
|
||||
can_power,
|
||||
} => {
|
||||
// Power on CAN transceiver and start controller
|
||||
can_power.set_high();
|
||||
let config = twai_config.take().expect("twai config not set");
|
||||
let mut as_async = config.into_async().start();
|
||||
// Give CAN some time to stabilize
|
||||
Timer::after_millis(10).await;
|
||||
|
||||
// Send a few test messages per potential sensor node
|
||||
for plant in 0..crate::hal::PLANT_COUNT {
|
||||
for sensor in [Sensor::A, Sensor::B] {
|
||||
let target = StandardId::new(plant_id(IDENTIFY_CMD_OFFSET, sensor.into(), plant as u16)).context(">> Could not create address for sensor! (plant: {}) <<")?;
|
||||
let can_buffer = [0_u8; 0];
|
||||
if let Some(frame) = EspTwaiFrame::new(target, &can_buffer) {
|
||||
// Try a few times; we intentionally ignore rx here and rely on stub logic
|
||||
let resu = as_async.transmit_async(&frame).await;
|
||||
match resu {
|
||||
Ok(_) => {
|
||||
info!(
|
||||
"Sent test message to plant {} sensor {:?}",
|
||||
plant, sensor
|
||||
);
|
||||
}
|
||||
Err(err) => {
|
||||
info!("Error sending test message to plant {} sensor {:?}: {:?}", plant, sensor, err);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
info!("Error building CAN frame");
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
let mut result = DetectionResult::default();
|
||||
loop {
|
||||
match as_async.receive_async().with_deadline(Instant::from_millis(100)).await {
|
||||
Ok(or) => {
|
||||
match or {
|
||||
Ok(can_frame) => {
|
||||
match can_frame.id() {
|
||||
Id::Standard(id) => {
|
||||
let rawid = id.as_raw();
|
||||
match classify(rawid) {
|
||||
None => {}
|
||||
Some(msg) => {
|
||||
if msg.0 == MessageKind::MoistureData {
|
||||
let plant = msg.1 as usize;
|
||||
let sensor = msg.2;
|
||||
match sensor {
|
||||
SensorSlot::A => {
|
||||
result.plant[plant].sensor_a = true;
|
||||
}
|
||||
SensorSlot::B => {
|
||||
result.plant[plant].sensor_b = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Id::Extended(ext) => {
|
||||
warn!("Received extended ID: {:?}", ext);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
Err(err ) => {
|
||||
error!("Error receiving CAN message: {:?}", err);
|
||||
break;
|
||||
}
|
||||
}
|
||||
info!("Received CAN message: {:?}", or);
|
||||
|
||||
}
|
||||
Err(err) => {
|
||||
error!("Timeout receiving CAN message: {:?}", err);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
let config = as_async.stop().into_blocking();
|
||||
can_power.set_low();
|
||||
twai_config.replace(config);
|
||||
|
||||
info!("Autodetection result: {:?}", result);
|
||||
Ok(result)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub async fn inner_pulse(plant: usize, sensor: Sensor, signal_counter: &mut Unit<'_, 0>, sensor_expander: &mut Pca9535Immediate<I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Blocking>>>) -> FatResult<f32> {
|
||||
|
||||
let mut results = [0_f32; REPEAT_MOIST_MEASURE];
|
||||
for repeat in 0..REPEAT_MOIST_MEASURE {
|
||||
signal_counter.pause();
|
||||
signal_counter.clear();
|
||||
|
||||
//Disable all
|
||||
sensor_expander.pin_set_high(GPIOBank::Bank0, MS4)?;
|
||||
|
||||
let sensor_channel = match sensor {
|
||||
Sensor::A => plant as u32,
|
||||
Sensor::B => (15 - plant) as u32,
|
||||
};
|
||||
|
||||
let is_bit_set = |b: u8| -> bool { sensor_channel & (1 << b) != 0 };
|
||||
if is_bit_set(0) {
|
||||
sensor_expander.pin_set_high(GPIOBank::Bank0, MS0)?;
|
||||
} else {
|
||||
sensor_expander.pin_set_low(GPIOBank::Bank0, MS0)?;
|
||||
}
|
||||
if is_bit_set(1) {
|
||||
sensor_expander.pin_set_high(GPIOBank::Bank0, MS1)?;
|
||||
} else {
|
||||
sensor_expander.pin_set_low(GPIOBank::Bank0, MS1)?;
|
||||
}
|
||||
if is_bit_set(2) {
|
||||
sensor_expander.pin_set_high(GPIOBank::Bank0, MS2)?;
|
||||
} else {
|
||||
sensor_expander.pin_set_low(GPIOBank::Bank0, MS2)?;
|
||||
}
|
||||
if is_bit_set(3) {
|
||||
sensor_expander.pin_set_high(GPIOBank::Bank0, MS3)?;
|
||||
} else {
|
||||
sensor_expander.pin_set_low(GPIOBank::Bank0, MS3)?;
|
||||
}
|
||||
|
||||
sensor_expander.pin_set_low(GPIOBank::Bank0, MS4)?;
|
||||
sensor_expander.pin_set_high(GPIOBank::Bank0, SENSOR_ON)?;
|
||||
|
||||
let measurement = 100; // TODO what is this scaling factor? what is its purpose?
|
||||
let factor = 1000f32 / measurement as f32;
|
||||
|
||||
//give some time to stabilize
|
||||
Timer::after_millis(10).await;
|
||||
signal_counter.resume();
|
||||
Timer::after_millis(measurement).await;
|
||||
signal_counter.pause();
|
||||
sensor_expander.pin_set_high(GPIOBank::Bank0, MS4)?;
|
||||
sensor_expander.pin_set_low(GPIOBank::Bank0, SENSOR_ON)?;
|
||||
sensor_expander.pin_set_low(GPIOBank::Bank0, MS0)?;
|
||||
sensor_expander.pin_set_low(GPIOBank::Bank0, MS1)?;
|
||||
sensor_expander.pin_set_low(GPIOBank::Bank0, MS2)?;
|
||||
sensor_expander.pin_set_low(GPIOBank::Bank0, MS3)?;
|
||||
Timer::after_millis(10).await;
|
||||
let unscaled = 1337; //signal_counter.get_counter_value()? as i32;
|
||||
let hz = unscaled as f32 * factor;
|
||||
LOG_ACCESS
|
||||
.lock()
|
||||
.await
|
||||
.log(
|
||||
LogMessage::RawMeasure,
|
||||
unscaled as u32,
|
||||
hz as u32,
|
||||
&plant.to_string(),
|
||||
&format!("{sensor:?}"),
|
||||
)
|
||||
.await;
|
||||
results[repeat] = hz;
|
||||
}
|
||||
results.sort_by(|a, b| a.partial_cmp(b).unwrap()); // floats don't seem to implement total_ord
|
||||
|
||||
let mid = results.len() / 2;
|
||||
let median = results[mid];
|
||||
Ok(median)
|
||||
}
|
||||
|
||||
async fn inner_can(
|
||||
twai: &mut Twai<'static, Blocking>,
|
||||
) -> FatResult<Moistures> {
|
||||
[0_u8; 8];
|
||||
config::standard();
|
||||
|
||||
let timeout = Instant::now()
|
||||
.checked_add(embassy_time::Duration::from_millis(100))
|
||||
.context("Timeout")?;
|
||||
loop {
|
||||
let answer = twai.receive();
|
||||
match answer {
|
||||
Ok(answer) => {
|
||||
info!("Received CAN message: {:?}", answer);
|
||||
}
|
||||
Err(error) => match error {
|
||||
nb::Error::Other(error) => {
|
||||
return Err(FatError::CanBusError { error });
|
||||
}
|
||||
nb::Error::WouldBlock => {
|
||||
if Instant::now() > timeout {
|
||||
bail!("Timeout waiting for CAN answer");
|
||||
}
|
||||
Timer::after_millis(10).await;
|
||||
}
|
||||
},
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user