323 lines
12 KiB
Rust
323 lines
12 KiB
Rust
use crate::bail;
|
|
use crate::fat_error::{ContextExt, FatResult};
|
|
use crate::hal::Box;
|
|
use crate::hal::{DetectionResult, Moistures, Sensor};
|
|
use crate::log::{LogMessage, LOG_ACCESS};
|
|
use alloc::format;
|
|
use alloc::string::ToString;
|
|
use async_trait::async_trait;
|
|
use canapi::id::{classify, plant_id, MessageKind, IDENTIFY_CMD_OFFSET};
|
|
use canapi::SensorSlot;
|
|
use embassy_embedded_hal::shared_bus::blocking::i2c::I2cDevice;
|
|
use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
|
|
use embassy_time::{Duration, 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::{Async, 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.into_async().start();
|
|
|
|
loop {
|
|
let rec = twai.receive();
|
|
match rec {
|
|
Ok(_) => {}
|
|
Err(err) => {
|
|
info!("Error receiving CAN message: {err:?}");
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
Timer::after_millis(10).await;
|
|
|
|
let mut moistures = Moistures::default();
|
|
let _ = Self::wait_for_can_measurements(&mut twai, &mut moistures)
|
|
.with_timeout(Duration::from_millis(5000))
|
|
.await;
|
|
|
|
can_power.set_low();
|
|
|
|
let config = twai.stop().into_blocking();
|
|
twai_config.replace(config);
|
|
|
|
Ok(moistures)
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
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 {plant} sensor {sensor:?}");
|
|
}
|
|
Err(err) => {
|
|
info!(
|
|
"Error sending test message to plant {plant} sensor {sensor:?}: {err:?}"
|
|
);
|
|
}
|
|
}
|
|
} else {
|
|
info!("Error building CAN frame");
|
|
}
|
|
}
|
|
}
|
|
|
|
let mut moistures = Moistures::default();
|
|
let _ = Self::wait_for_can_measurements(&mut as_async, &mut moistures)
|
|
.with_timeout(Duration::from_millis(5000))
|
|
.await;
|
|
|
|
let config = as_async.stop().into_blocking();
|
|
can_power.set_low();
|
|
twai_config.replace(config);
|
|
|
|
let result = moistures.into();
|
|
|
|
info!("Autodetection result: {result:?}");
|
|
Ok(result)
|
|
}
|
|
}
|
|
}
|
|
|
|
async fn wait_for_can_measurements(
|
|
as_async: &mut Twai<'_, Async>,
|
|
moistures: &mut Moistures,
|
|
) -> FatResult<()> {
|
|
loop {
|
|
match as_async.receive_async().await {
|
|
Ok(can_frame) => match can_frame.id() {
|
|
Id::Standard(id) => {
|
|
info!("Received CAN message: {id:?}");
|
|
let rawid = id.as_raw();
|
|
match classify(rawid) {
|
|
None => {}
|
|
Some(msg) => {
|
|
info!(
|
|
"received message of kind {:?} (plant: {}, sensor: {:?})",
|
|
msg.0, msg.1, msg.2
|
|
);
|
|
if msg.0 == MessageKind::MoistureData {
|
|
let plant = msg.1 as usize;
|
|
let sensor = msg.2;
|
|
let data = can_frame.data();
|
|
|
|
match sensor {
|
|
SensorSlot::A => {
|
|
moistures.sensor_a_hz[plant] = data[0] as f32;
|
|
}
|
|
SensorSlot::B => {
|
|
moistures.sensor_b_hz[plant] = data[0] as f32;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
Id::Extended(ext) => {
|
|
warn!("Received extended ID: {ext:?}");
|
|
}
|
|
},
|
|
Err(err) => {
|
|
error!("Error receiving CAN message: {err:?}");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
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 sample in results.iter_mut() {
|
|
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;
|
|
*sample = 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)
|
|
}
|
|
}
|
|
|
|
impl From<Moistures> for DetectionResult {
|
|
fn from(value: Moistures) -> Self {
|
|
let mut result = DetectionResult::default();
|
|
for (plant, sensor) in value.sensor_a_hz.iter().enumerate() {
|
|
result.plant[plant].sensor_a = *sensor > 1.0_f32;
|
|
}
|
|
for (plant, sensor) in value.sensor_b_hz.iter().enumerate() {
|
|
result.plant[plant].sensor_b = *sensor > 1.0_f32;
|
|
}
|
|
result
|
|
}
|
|
}
|