-
Benjamin Koch authoredBenjamin Koch authored
heizung.rs 20.82 KiB
#![no_std]
#![no_main]
#![feature(type_alias_impl_trait)]
use core::sync::atomic::*;
use defmt::*;
use embassy_executor::Spawner;
//use embassy_futures::join::join;
use embassy_futures::select::*;
use embassy_rp::adc::{self, Adc};
use embassy_rp::peripherals::*;
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;
//use embedded_hal_async::i2c::I2c;
use embassy_embedded_hal::SetConfig;
use embassy_rp::uart::{self, Parity};
use embassy_rp::pwm::{self, Pwm};
use {defmt_rtt as _, panic_probe as _};
use heapless::String;
use heizung::i2c_scan::{self, ScanResultForBus};
use heizung::modbus_server::{ModbusServer, ModbusRegisters, ModbusErrorCode};
use heizung::rs485::RS485;
#[embassy_executor::task]
async fn i2c_task(mut i2c_peri: embassy_rp::peripherals::I2C0,
mut scl: embassy_rp::peripherals::PIN_13, mut sda: embassy_rp::peripherals::PIN_12,
mut i2c_irq: embassy_rp::interrupt::I2C0_IRQ, i2c_config: i2c::Config) {
let mut prev_scan_result = ScanResultForBus::new();
loop {
if true {
let _i2c = i2c::I2c::new_async(&mut i2c_peri, &mut scl, &mut sda, &mut i2c_irq, i2c_config);
}
let scan_result = ScanResultForBus::scan(&mut scl, &mut sda).await;
if prev_scan_result != scan_result {
prev_scan_result = scan_result;
const MSG_LEN : usize = i2c_scan::SCAN_RESULT_STRING_LENGTH;
let mut msg = String::<MSG_LEN>::new();
match scan_result.format(&mut msg) {
Ok(()) => info!("I2C scan result:\n{}", msg),
Err(()) => info!("I2C scan result: too long for our buffer!"),
}
}
Timer::after(Duration::from_secs(1)).await;
}
}
#[embassy_executor::task]
async fn beeper_task(pwm_channel: embassy_rp::peripherals::PWM_CH3, beeper_pin: embassy_rp::peripherals::PIN_6) {
let mut c: pwm::Config = Default::default();
c.top = 0x8000;
c.compare_a = 8;
c.divider = fixed::FixedU16::from_num(20);
let mut pwm = Pwm::new_output_a(pwm_channel, beeper_pin, c.clone());
let mut toggle = false;
loop {
info!("PWM: duty={}/32768, divider={}", c.compare_a, c.divider.to_bits());
Timer::after(Duration::from_millis(200)).await;
//c.compare_a = c.compare_a.rotate_left(4);
c.compare_a = if toggle { c.top/2 } else { 0 };
//toggle = !toggle;
c.divider -= fixed::FixedU16::from_num(0.1); if c.divider < 1 {
c.divider = fixed::FixedU16::from_num(20);
}
toggle = true;
pwm.set_config(&c);
}
}
#[embassy_executor::task]
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 {
analog_in1_raw: AtomicU16,
analog_in1_millivolt: AtomicU16,
vcc_raw: AtomicU16,
vcc: AtomicU16,
temp_raw: AtomicU16,
temp_centicelsius: AtomicU16,
currents: [AdcCurrentData; 2],
}
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, 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 {
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[0].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>,
reed3: Input<'a, PIN_19>,
reed4: Input<'a, PIN_20>,
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> {
fn read_discrete_input(self: &mut Self, addr: u16) -> Result<bool, ModbusErrorCode> {
match addr {
0 => Ok(self.reed1.is_high()),
1 => Ok(self.reed2.is_high()),
2 => Ok(self.reed3.is_high()),
3 => Ok(self.reed4.is_high()),
4 => Ok(self.button_boot2.is_low()),
_ => Err(ModbusErrorCode::IllegalDataAddress), }
}
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)
}
Err(ModbusErrorCode::IllegalDataAddress)
}
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)
}
}
fn write_coil(self: &mut Self, addr: u16, value: bool) -> Result<(), ModbusErrorCode> {
match addr {
0 => { self.led_g.set_level(Level::from(value)); Ok(()) },
_ => Err(ModbusErrorCode::IllegalDataAddress),
}
}
fn write_register(self: &mut Self, addr: u16, value: u16) -> Result<u16, ModbusErrorCode> {
Err(ModbusErrorCode::IllegalDataAddress)
}
}
#[embassy_executor::main]
async fn main(spawner: Spawner) {
let p = embassy_rp::init(Default::default());
info!("starting");
// pinout:
let mut _drive2 = Output::new(p.PIN_0, Level::Low);
let mut _drive1 = Output::new(p.PIN_1, Level::Low);
//let LED_W = p.PIN_2;
let _ws2812_display = p.PIN_2;
//let LED_Y = p.PIN_3;
let en_measure_current = Output::new(p.PIN_3, Level::Low);
let mut led_b = Output::new(p.PIN_4, Level::Low);
let led_g = Output::new(p.PIN_5, Level::Low);
let led_r = p.PIN_6;
let _matrix_in1 = Input::new(p.PIN_7, Pull::Up);
let _sbu1 = p.PIN_8;
let _sbu2 = p.PIN_9;
let _vbus_det = p.PIN_10;
let button_boot2 = Input::new(p.PIN_11, Pull::Up);
let sda = p.PIN_12;
let scl = p.PIN_13;
let _ws2811_red_in = Input::new(p.PIN_14, Pull::None);
let tx_en = p.PIN_15;
let tx = p.PIN_16;
let rx = p.PIN_17;
let _matrix_in2 = Input::new(p.PIN_18, Pull::Up);
let reed3 = Input::new(p.PIN_19, Pull::Up);
let reed4 = Input::new(p.PIN_20, Pull::Up);
let reed2 = Input::new(p.PIN_21, Pull::Up);
let reed1 = Input::new(p.PIN_22, Pull::Up);
let _matrix_in3 = Input::new(p.PIN_23, Pull::Up);
let _digout1 = p.PIN_24;
let _digout2 = p.PIN_25;
let analog_in1 = p.PIN_26;
let current2 = p.PIN_27;
let current1 = p.PIN_28;
let measure_vcc = p.PIN_29;
let i2c_irq = interrupt::take!(I2C0_IRQ);
let mut i2c_config = i2c::Config::default();
i2c_config.frequency = 400_000;
unwrap!(spawner.spawn(i2c_task(p.I2C0, scl, sda, i2c_irq, i2c_config)));
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();
uart_config.baudrate = 19200;
uart_config.parity = Parity::ParityEven;
let rs485 = RS485::new(
p.UART0, rx, tx, tx_en, interrupt::take!(UART0_IRQ), p.DMA_CH1, uart_config,
p.PIO0, p.DMA_CH0, p.DMA_CH2,
ModbusServer::new(ModBusRegs {
led_g,
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)));
if false {
unwrap!(spawner.spawn(beeper_task(p.PWM_CH3, led_r)));
}
loop {
led_b.set_high();
Timer::after(Duration::from_secs(1)).await;
led_b.set_low();
Timer::after(Duration::from_secs(1)).await;
}
}