diff --git a/firmware/rust1/src/bin/heizung.rs b/firmware/rust1/src/bin/heizung.rs index d03b9575936da9c45941c4e243d5389b9171bec3..6e8762b66da0426485e3e908c4897ad20f9331b9 100644 --- a/firmware/rust1/src/bin/heizung.rs +++ b/firmware/rust1/src/bin/heizung.rs @@ -9,7 +9,8 @@ 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_rp::flash::Flash; +use embassy_rp::peripherals::{*, self}; use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex; use embassy_sync::mutex::Mutex; use embassy_time::{Duration, Timer, Instant}; @@ -310,6 +311,8 @@ enum ReadType { InputRegister, } +const FLASH_SIZE: usize = 2 * 1024 * 1024; + struct ModBusRegs<'a> { led_g: Output<'a, PIN_5>, button_boot2: Input<'a, PIN_11>, @@ -318,6 +321,7 @@ struct ModBusRegs<'a> { reed2: Input<'a, PIN_21>, reed1: Input<'a, PIN_22>, adc_data: &'a AdcData, + flash: Flash<'a, peripherals::FLASH, FLASH_SIZE>, consistent_read_type: ReadType, consistent_read_addr: u16, @@ -444,6 +448,15 @@ impl<'a> ModbusRegisters for ModBusRegs<'a> { pusher.push_bytes(&value[start .. end])?; Ok(()) }, + (6, 1) => { + pusher.push_fn(record_length as usize * 2, |buf| { + self.flash.read(record_number as u32 * 2, buf) + .map_err(|err| { + info!("Error reading flash: {:?}", err); + ModbusErrorCode::ServerDeviceFailure + }) + }) + }, _ => { Err(ModbusErrorCode::IllegalDataAddress) } @@ -511,6 +524,7 @@ async fn main2(spawner: Spawner) { button_boot2, reed1, reed2, reed3, reed4, adc_data: &ADC_DATA, + flash: embassy_rp::flash::Flash::<_, FLASH_SIZE>::new(p.FLASH), consistent_read_type: ReadType::InputRegister, consistent_read_addr: u16::default(), consistent_read_length: u16::default(), diff --git a/firmware/rust1/src/modbus_server.rs b/firmware/rust1/src/modbus_server.rs index 94d7f72a6441e56a230f7abf166d110e7e2530bb..cb5e37ccb12a6157162d8c54060d5d25b3f31db5 100644 --- a/firmware/rust1/src/modbus_server.rs +++ b/firmware/rust1/src/modbus_server.rs @@ -62,6 +62,15 @@ impl<'a, const N: usize> U16Pusher<'a, N> { pub fn push(self: &mut Self, x: u16) -> Result<(), ModbusErrorCode> { self.push_bytes(&x.to_be_bytes()) } + + pub fn push_fn<F: FnOnce(&mut [u8]) -> Result<(), ModbusErrorCode>>(self: &mut Self, len_bytes: usize, f: F) -> Result<(), ModbusErrorCode> { + let len_before = self.buf.len(); + for _ in 0..len_bytes { + self.buf.push(0).or(Err(ModbusErrorCode::ServerDeviceFailure))?; + } + let len_after = self.buf.len(); + f(&mut self.buf[len_before .. len_after]) + } }