Skip to content
Snippets Groups Projects
Commit 8e0305f4 authored by Benjamin Koch's avatar Benjamin Koch
Browse files

add more Modbus registers with ADC data

parent e118b151
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
firmware/rust1/src/bin/heizung.rs
+ 278
− 32
View file @ 8e0305f4
......@@ -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)));
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment