diff --git a/firmware/rust1/src/bin/heizung.rs b/firmware/rust1/src/bin/heizung.rs
index 91797c81159b8edb56771cb3afef99d11a3709fd..49a4b333d1f9604aed15584b0786a7ad8065dad1 100644
--- a/firmware/rust1/src/bin/heizung.rs
+++ b/firmware/rust1/src/bin/heizung.rs
@@ -2,7 +2,7 @@
#![no_main]
#![feature(type_alias_impl_trait)]
-use core::sync::atomic::{AtomicU16, AtomicU32, Ordering};
+use core::sync::atomic::*;
use defmt::*;
use embassy_executor::Spawner;
@@ -10,7 +10,9 @@ use embassy_executor::Spawner;
use embassy_futures::select::*;
use embassy_rp::adc::{self, Adc};
use embassy_rp::peripherals::*;
-use embassy_time::{Duration, Timer};
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+use embassy_sync::mutex::Mutex;
+use embassy_time::{Duration, Timer, Instant};
use embassy_rp::gpio::{Input, Level, Output, Pull, Flex};
use embassy_rp::i2c;
use embassy_rp::interrupt;
@@ -79,46 +81,230 @@ async fn uart_task(this: RS485<ModbusServer<ModBusRegs<'static>>>) {
this.run_task().await;
}
+// Zero offset for current measurement will be exponential smoothing with alpha=1/ZERO_OFFSET_FACTOR.
+// The value in zero_offset_raw will be the smoothed offset times ZERO_OFFSET_FACTOR. This factor
+// should be a power of two because we will divide by it.
+const ZERO_OFFSET_FACTOR : u32 = 32;
+
+struct AdcCurrentData {
+ // Value measured while both MOSFETs are off
+ // (multiplied by ZERO_OFFSET_FACTOR)
+ zero_offset_raw: AtomicU32,
+ // Current measured with 2k resistor (raw value and 1/10 mA)
+ current_2k_raw: AtomicU16,
+ current_2k_dezi_milliamps: AtomicU16,
+ // Current measured while main MOSFET is/was on (raw value and 1/10 mA and when this was measured)
+ current_on_raw: AtomicU16,
+ current_on_dezi_milliamps: AtomicU16,
+ current_on_time: Mutex<CriticalSectionRawMutex, Instant>,
+}
+
+impl AdcCurrentData {
+ const fn const_default() -> AdcCurrentData {
+ AdcCurrentData {
+ zero_offset_raw: AtomicU32::new(0),
+ current_2k_raw: AtomicU16::new(0),
+ current_2k_dezi_milliamps: AtomicU16::new(0),
+ current_on_raw: AtomicU16::new(0),
+ current_on_dezi_milliamps: AtomicU16::new(0),
+ current_on_time: Mutex::new(Instant::from_ticks(0)),
+ }
+ }
+}
+
struct AdcData {
- current1: AtomicU16,
- current2: AtomicU16,
analog_in1_raw: AtomicU16,
analog_in1_millivolt: AtomicU16,
+ vcc_raw: AtomicU16,
vcc: AtomicU16,
+ temp_raw: AtomicU16,
+ temp_centicelsius: AtomicU16,
+
+ currents: [AdcCurrentData; 2],
}
-fn convert_to_celsius(raw_temp: u16) -> f32 {
- // According to chapter 4.9.5. Temperature Sensor in RP2040 datasheet
- 27.0 - (raw_temp as f32 * 3.0522 / 4096.0 - 0.706) / 0.001721 as f32
+impl AdcData {
+ const fn const_default() -> AdcData {
+ AdcData {
+ analog_in1_raw: AtomicU16::new(0),
+ analog_in1_millivolt: AtomicU16::new(0),
+ vcc_raw: AtomicU16::new(0),
+ vcc: AtomicU16::new(0),
+ temp_raw: AtomicU16::new(0),
+ temp_centicelsius: AtomicU16::new(0),
+ currents: [AdcCurrentData::const_default(), AdcCurrentData::const_default()],
+ }
+ }
}
#[embassy_executor::task]
-async fn adc_task(shared_data: &'static AdcData, adc: ADC, en_measure_current: Output<'static, PIN_3>,
- mut analog_in1: PIN_26, mut current2: PIN_27, mut current1: PIN_28, mut measure_vcc: PIN_29) {
+async fn adc_task(shared_data: &'static AdcData, adc: ADC, mut en_measure_current: Output<'static, PIN_3>,
+ mut analog_in1: PIN_26, mut current2: PIN_27, mut current1: PIN_28, mut measure_vcc: PIN_29, outputs_active: &'static AtomicU8) {
let irq = interrupt::take!(ADC_IRQ_FIFO);
let mut adc = Adc::new(adc, irq, adc::Config::default());
+ let debug = true;
+ let mut en_measure_current_toggle = 0;
+
loop {
- let level = adc.read(&mut analog_in1).await;
- shared_data.analog_in1_raw.store(level, Ordering::Release);
- let value = 3.05*level as f32/4096. * (10.+2.*5.1)/5.1;
- shared_data.analog_in1_millivolt.store((value * 1000.) as u16, Ordering::Release);
- info!("ADC analog_in1: {} -> {}", level, value);
-
- let level = adc.read(&mut current2).await;
- info!("ADC current2: {}", level);
- let level = adc.read(&mut current1).await;
- info!("ADC current1: {}", level);
-
- let level = adc.read(&mut measure_vcc).await;
- let vcc = 3.05 * level as f32 / 4096. * (100. + 10.)/10.;
- info!("ADC measure_vcc: {} -> {}", level, vcc);
-
- let temp = adc.read_temperature().await;
- info!("Temp: {} -> {} degrees", temp, convert_to_celsius(temp));
- Timer::after(Duration::from_secs(1)).await;
+ if true {
+ let level = adc.read(&mut analog_in1).await;
+ // 3.05 V for 4096 counts, voltage divider with (10k + 5.1k) over 5.1k, result in mV:
+ // level * 3.05/4096. * (10+5.1+5.1)/5.1 * 1000
+ // -> level * 305 * (100+51+51) * 1000 / 100 / 4096 / 51
+ // -> level * 305 * ((100+51+51)/2) * (1000/100) / 2048 / 51 --> fits in 32 bits for level=4096 (but barely)
+ let value = level as u32 * 305 * ((100+51+51)/2) * (1000/100);
+ let value = (value + 2048*51/2) / 2048 / 51;
+ let value = if value > u16::MAX as u32 { u16::MAX } else { value as u16 };
+ shared_data.analog_in1_raw.store(level, Ordering::Relaxed);
+ shared_data.analog_in1_millivolt.store(value, Ordering::Relaxed);
+ if debug {
+ let value2 = 3.05*level as f32/4096. * (10.+2.*5.1)/5.1;
+ info!("ADC analog_in1: {} cnt -> {} V ({} mV)", level, value2, value);
+ }
+ }
+
+ if true {
+ let level = adc.read(&mut measure_vcc).await;
+ // 3.05V for 4096 counts, voltage divider with 100k over 10, result in mV:
+ // level * 3.05/4096. * (100 + 10)/10 * 1000
+ // -> level * 305 * 11 * 1000 / 100 / 4096
+ let value = (level as u32 * 305 * 11 * 1000 / 100 + 2048) / 4096;
+ let value = if value > u16::MAX as u32 { u16::MAX } else { value as u16 };
+ shared_data.vcc_raw.store(level, Ordering::Relaxed);
+ shared_data.vcc.store(value, Ordering::Relaxed);
+ if debug {
+ let vcc = 3.05 * level as f32 / 4096. * (100. + 10.)/10.;
+ info!("ADC measure_vcc: {} cnt -> {} V ({} mV)", level, vcc, value);
+ }
+ }
+
+ let outputs_were_active = outputs_active.load(Ordering::SeqCst);
+ let mut measure_currents = true;
+ if outputs_were_active != 0 && false {
+ // output is active but also en_measure_current -> turn it off
+ // -> Actually, don't do that because there is no harm and it can be useful for the other channel.
+ if en_measure_current.is_set_high() {
+ en_measure_current.set_low();
+ measure_currents = false;
+ }
+ en_measure_current_toggle = 0;
+ } else {
+ match en_measure_current_toggle {
+ 0 | 8 => {
+ en_measure_current.set_level(if en_measure_current_toggle >= 8 { Level::High } else { Level::Low });
+ // skip in this loop to give it time to settle
+ measure_currents = false;
+ }
+ _ => {}
+ }
+ en_measure_current_toggle = (en_measure_current_toggle + 1) % 16;
+ }
+
+ // read another value between looking at output_active and measuring the currents
+ if true {
+ let level = adc.read_temperature().await;
+ // According to chapter 4.9.5. Temperature Sensor in RP2040 datasheet
+ let adc_vcc = 3.0522; // from CJ431
+ let celsius = 27.0 - (level as f32 * adc_vcc / 4096.0 - 0.706) / 0.001721;
+ let value = celsius * 100.;
+ let value = if value > u16::MAX as f32 { u16::MAX } else { value as u16 };
+ shared_data.temp_raw.store(level, Ordering::Relaxed);
+ shared_data.temp_centicelsius.store(value, Ordering::Relaxed);
+ if debug {
+ info!("Temp: {} -> {} deg C", level, celsius);
+ }
+ }
+
+ if measure_currents {
+ let levels = [
+ adc.read(&mut current1).await,
+ adc.read(&mut current2).await,
+ ];
+ let outputs_are_active = outputs_active.load(Ordering::SeqCst);
+ //let en_measure_current_is_active = en_measure_current.is_set_high(); //FIXME I think is_set_high() has a bug.
+ let en_measure_current_is_active = en_measure_current_toggle >= 8;
+ for i in 0..2 {
+ // 3.05 V for 4096 counts, shunt resistor is 0.1 ohms (i.e. 0.1 V/A), amplifier gain is 56, result should be mA/10
+ // (level - offset) * 3.05/4096 / 0.1 * 1/56 * 1e4
+ // -> (level - offset) * 305 * 10 * 1e4 / 56 / 4096 / 100
+ // -> (level - offset) * 305 * 1000 / 56 / 4096
+ // -> (level - offset) * 305 * (1000/8) / 4096 / 7
+ let level = levels[i];
+ let offset = (shared_data.currents[i].zero_offset_raw.load(Ordering::Relaxed)/ZERO_OFFSET_FACTOR) as u16;
+ let level2 = if level > offset { level - offset } else { 0 };
+ //info!("CUR{}: raw: {}, {}, zero={}", i, level, level2, shared_data.currents[i].zero_offset_raw.load(Ordering::Relaxed));
+ let value = (level2 as u32 * 305 * (1000/8) + 7*4096/2) / 4096 / 7;
+ let value = if value > u16::MAX as u32 { u16::MAX } else { value as u16 };
+
+ let output_was_active = (outputs_were_active>>i) & 1 != 0;
+ let output_is_active = (outputs_are_active>>i) & 1 != 0;
+ #[allow(unused_variables, unused_assignments)]
+ let mode : char;
+ match (output_was_active, output_is_active, en_measure_current_is_active) {
+ (true, true, false) => {
+ // output was consistently (hopefully) turned on during our measurement
+ // and en_measure_current is off -> good measurement for current in "on" state
+ shared_data.currents[i].current_on_raw.store(levels[i], Ordering::Relaxed);
+ shared_data.currents[i].current_on_dezi_milliamps.store(value, Ordering::Relaxed);
+ *shared_data.currents[i].current_on_time.lock().await = Instant::now();
+ mode = 'O';
+ }
+ (false, false, false) => {
+ // output and en_measure_current are off -> good measurement for zero level
+
+ // fetch_update is exactly what we want but it doesn't seem to be available...
+ //shared_data.currents[i].zero_offset_raw.fetch_update(Ordering::Relaxed, Ordering::Relaxed,
+ // |x| { x*(ZERO_OFFSET_FACTOR-1)/ZERO_OFFSET_FACTOR + levels[i] });
+ //SAFETY: We are the only writers for zero_offset_raw.
+ let x = shared_data.currents[i].zero_offset_raw.load(Ordering::Relaxed);
+ let x2 = x*(ZERO_OFFSET_FACTOR-1)/ZERO_OFFSET_FACTOR + level as u32;
+ shared_data.currents[i].zero_offset_raw.store(x2, Ordering::Relaxed);
+
+ mode = 'z';
+ }
+ (false, false, true) => {
+ // output is off and en_measure_current is on -> good measurement through 2k resistor
+ shared_data.currents[i].current_2k_raw.store(levels[i], Ordering::Relaxed);
+ shared_data.currents[i].current_2k_dezi_milliamps.store(value, Ordering::Relaxed);
+ mode = 'k';
+ }
+ _ => {
+ // inconsistent state of MOSFETs -> ignore this measurement
+ mode = '!';
+ }
+ }
+
+ if debug {
+ info!("ADC current{}: {}: {} -> {} [10*mA]", i, mode, level, value);
+ }
+ }
+
+ //FIXME remove
+ info!("CUR: 2k={}, {}, {}, zero={}, {} -> {}, {}",
+ en_measure_current_is_active, levels[0], levels[1],
+ shared_data.currents[0].zero_offset_raw.load(Ordering::Relaxed),
+ shared_data.currents[1].zero_offset_raw.load(Ordering::Relaxed),
+ shared_data.currents[1].current_2k_raw.load(Ordering::Relaxed),
+ shared_data.currents[1].current_2k_raw.load(Ordering::Relaxed),
+ );
+ }
+
+ if debug {
+ Timer::after(Duration::from_secs(1)).await;
+ } else {
+ Timer::after(Duration::from_millis(1)).await;
+ }
}
}
+
+
+#[derive(Clone, Copy, PartialEq, Eq)]
+enum ReadType {
+ HoldingRegister,
+ InputRegister,
+}
+
struct ModBusRegs<'a> {
led_g: Output<'a, PIN_5>,
button_boot2: Input<'a, PIN_11>,
@@ -127,6 +313,30 @@ struct ModBusRegs<'a> {
reed2: Input<'a, PIN_21>,
reed1: Input<'a, PIN_22>,
adc_data: &'a AdcData,
+
+ consistent_read_type: ReadType,
+ consistent_read_addr: u16,
+ consistent_read_length: u16,
+ consistent_read_data: [u16; 4],
+}
+
+impl<'a> ModBusRegs<'a> {
+ fn read_reg_u64(self: &mut Self, read_type: ReadType, base_addr: u16, addr: u16, value: u64) -> Result<u16, ModbusErrorCode> {
+ defmt::assert!(addr >= base_addr);
+ defmt::assert!(addr < base_addr + 64/16);
+
+ self.consistent_read_type = read_type;
+ self.consistent_read_addr = base_addr;
+ self.consistent_read_length = 64/16;
+ self.consistent_read_data = [
+ ((value >> 0) & 0xffff) as u16,
+ ((value >> 16) & 0xffff) as u16,
+ ((value >> 32) & 0xffff) as u16,
+ ((value >> 48) & 0xffff) as u16,
+ ];
+
+ Ok(self.consistent_read_data[(addr - base_addr) as usize])
+ }
}
impl<'a> ModbusRegisters for ModBusRegs<'a> {
@@ -142,6 +352,11 @@ impl<'a> ModbusRegisters for ModBusRegs<'a> {
}
fn read_holding_register(self: &mut Self, addr: u16) -> Result<u16, ModbusErrorCode> {
+ if self.consistent_read_type == ReadType::HoldingRegister
+ && addr >= self.consistent_read_addr && addr < self.consistent_read_addr + self.consistent_read_length {
+ return Ok(self.consistent_read_data[(addr - self.consistent_read_addr) as usize]);
+ }
+
if addr == 1 {
return Ok(42)
}
@@ -149,10 +364,39 @@ impl<'a> ModbusRegisters for ModBusRegs<'a> {
}
fn read_input_register(self: &mut Self, addr: u16) -> Result<u16, ModbusErrorCode> {
+ if self.consistent_read_type == ReadType::InputRegister
+ && addr >= self.consistent_read_addr && addr < self.consistent_read_addr + self.consistent_read_length {
+ return Ok(self.consistent_read_data[(addr - self.consistent_read_addr) as usize]);
+ }
+
match addr {
0 => Ok(42),
+
1 => Ok(self.adc_data.analog_in1_raw.load(Ordering::Relaxed)),
2 => Ok(self.adc_data.analog_in1_millivolt.load(Ordering::Relaxed)),
+ 3 => Ok(self.adc_data.vcc_raw.load(Ordering::Relaxed)),
+ 4 => Ok(self.adc_data.vcc.load(Ordering::Relaxed)),
+ 5 => Ok(self.adc_data.temp_raw.load(Ordering::Relaxed)),
+ 6 => Ok(self.adc_data.temp_centicelsius.load(Ordering::Relaxed)),
+
+ 7 => Ok((self.adc_data.currents[0].zero_offset_raw.load(Ordering::Relaxed) / ZERO_OFFSET_FACTOR) as u16),
+ 8 => Ok(self.adc_data.currents[0].current_2k_raw.load(Ordering::Relaxed)),
+ 9 => Ok(self.adc_data.currents[0].current_2k_dezi_milliamps.load(Ordering::Relaxed)),
+ 10 => Ok(self.adc_data.currents[0].current_on_raw.load(Ordering::Relaxed)),
+ 11 => Ok(self.adc_data.currents[0].current_on_dezi_milliamps.load(Ordering::Relaxed)),
+ 12 | 13 | 14 | 15 => self.read_reg_u64(ReadType::InputRegister, 12, addr,
+ self.adc_data.currents[0].current_on_time.try_lock().map(
+ |x| x.as_micros()).unwrap_or(0)),
+
+ 16 => Ok((self.adc_data.currents[1].zero_offset_raw.load(Ordering::Relaxed) / ZERO_OFFSET_FACTOR) as u16),
+ 17 => Ok(self.adc_data.currents[1].current_2k_raw.load(Ordering::Relaxed)),
+ 18 => Ok(self.adc_data.currents[1].current_2k_dezi_milliamps.load(Ordering::Relaxed)),
+ 19 => Ok(self.adc_data.currents[1].current_on_raw.load(Ordering::Relaxed)),
+ 20 => Ok(self.adc_data.currents[1].current_on_dezi_milliamps.load(Ordering::Relaxed)),
+ 21 | 22 | 23 | 24 => self.read_reg_u64(ReadType::InputRegister, 21, addr,
+ self.adc_data.currents[1].current_on_time.try_lock().map(
+ |x| x.as_micros()).unwrap_or(0)),
+
_ => Err(ModbusErrorCode::IllegalDataAddress)
}
}
@@ -213,11 +457,9 @@ async fn main(spawner: Spawner) {
i2c_config.frequency = 400_000;
unwrap!(spawner.spawn(i2c_task(p.I2C0, scl, sda, i2c_irq, i2c_config)));
- static ADC_DATA: AdcData = AdcData {
- current1: AtomicU16::new(0), current2: AtomicU16::new(0), analog_in1_raw: AtomicU16::new(0),
- analog_in1_millivolt: AtomicU16::new(0), vcc: AtomicU16::new(0)
- };
- unwrap!(spawner.spawn(adc_task(&ADC_DATA, p.ADC, en_measure_current, analog_in1, current2, current1, measure_vcc)));
+ static ADC_DATA: AdcData = AdcData::const_default();
+ static OUTPUTS_ACTIVE: AtomicU8 = AtomicU8::new(0);
+ unwrap!(spawner.spawn(adc_task(&ADC_DATA, p.ADC, en_measure_current, analog_in1, current2, current1, measure_vcc, &OUTPUTS_ACTIVE)));
// use 19200 baud in 8E1 mode - not great but it's the Modbus default
let mut uart_config = uart::Config::default();
@@ -231,6 +473,10 @@ async fn main(spawner: Spawner) {
button_boot2,
reed1, reed2, reed3, reed4,
adc_data: &ADC_DATA,
+ consistent_read_type: ReadType::InputRegister,
+ consistent_read_addr: u16::default(),
+ consistent_read_length: u16::default(),
+ consistent_read_data: [0; 4],
}),
);
unwrap!(spawner.spawn(uart_task(rs485)));