From 9355429005e328020adfd243c23c2ab973b42c54 Mon Sep 17 00:00:00 2001
From: Benjamin Koch <bbbsnowball@gmail.com>
Date: Wed, 24 May 2023 04:04:35 +0200
Subject: [PATCH] connect discrete inputs and coils to some actual hardware
---
firmware/rust1/src/bin/heizung.rs | 56 +++++++---
firmware/rust1/src/modbus_server.rs | 156 ++++++++++++++++++++++++----
2 files changed, 178 insertions(+), 34 deletions(-)
diff --git a/firmware/rust1/src/bin/heizung.rs b/firmware/rust1/src/bin/heizung.rs
index 12715b9..fd5b2c9 100644
--- a/firmware/rust1/src/bin/heizung.rs
+++ b/firmware/rust1/src/bin/heizung.rs
@@ -6,7 +6,7 @@ use defmt::*;
use embassy_executor::Spawner;
//use embassy_futures::join::join;
use embassy_futures::select::*;
-use embassy_rp::gpio;
+use embassy_rp::peripherals::*;
use embassy_time::{Duration, Timer};
use embassy_rp::gpio::{Input, Level, Output, Pull, Flex};
use embassy_rp::i2c;
@@ -72,29 +72,53 @@ async fn beeper_task(pwm_channel: embassy_rp::peripherals::PWM_CH3, beeper_pin:
}
#[embassy_executor::task]
-async fn uart_task(this: RS485<ModbusServer<ModBusRegs>>) {
+async fn uart_task(this: RS485<ModbusServer<ModBusRegs<'static>>>) {
this.run_task().await;
}
-struct ModBusRegs {
+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>,
}
-impl ModbusRegisters for ModBusRegs {
- fn read_modbus_holding_register(self: &mut Self, addr: u16) -> Result<u16, ModbusErrorCode> {
+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 addr == 1 {
return Ok(42)
}
Err(ModbusErrorCode::IllegalDataAddress)
}
- fn read_modbus_input_register(self: &mut Self, addr: u16) -> Result<u16, ModbusErrorCode> {
+ fn read_input_register(self: &mut Self, addr: u16) -> Result<u16, ModbusErrorCode> {
if addr == 2 {
return Ok(42)
}
Err(ModbusErrorCode::IllegalDataAddress)
}
- fn write_modbus_register(self: &mut Self, addr: u16, value: u16) -> Result<(), ModbusErrorCode> {
+ 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)
}
}
@@ -112,13 +136,13 @@ async fn main(spawner: Spawner) {
//let LED_Y = p.PIN_3;
let mut _en_measure_current = Output::new(p.PIN_3, Level::Low);
let mut led_b = Output::new(p.PIN_4, Level::Low);
- let mut _led_g = Output::new(p.PIN_5, 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 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);
@@ -126,10 +150,10 @@ async fn main(spawner: Spawner) {
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 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;
@@ -150,7 +174,11 @@ async fn main(spawner: Spawner) {
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 { }),
+ ModbusServer::new(ModBusRegs {
+ led_g,
+ button_boot2,
+ reed1, reed2, reed3, reed4,
+ }),
);
unwrap!(spawner.spawn(uart_task(rs485)));
diff --git a/firmware/rust1/src/modbus_server.rs b/firmware/rust1/src/modbus_server.rs
index ce71a29..e325607 100644
--- a/firmware/rust1/src/modbus_server.rs
+++ b/firmware/rust1/src/modbus_server.rs
@@ -21,9 +21,11 @@ pub enum ModbusErrorCode {
}
pub trait ModbusRegisters {
- fn read_modbus_holding_register(self: &mut Self, addr: u16) -> Result<u16, ModbusErrorCode>;
- fn read_modbus_input_register(self: &mut Self, addr: u16) -> Result<u16, ModbusErrorCode>;
- fn write_modbus_register(self: &mut Self, addr: u16, value: u16) -> Result<(), ModbusErrorCode>;
+ fn read_discrete_input(self: &mut Self, addr: u16) -> Result<bool, ModbusErrorCode>;
+ fn read_holding_register(self: &mut Self, addr: u16) -> Result<u16, ModbusErrorCode>;
+ fn read_input_register(self: &mut Self, addr: u16) -> Result<u16, ModbusErrorCode>;
+ fn write_coil(self: &mut Self, addr: u16, value: bool) -> Result<(), ModbusErrorCode>;
+ fn write_register(self: &mut Self, addr: u16, value: u16) -> Result<u16, ModbusErrorCode>;
}
enum ModbusFrameLength {
@@ -85,8 +87,35 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
let rxbuf = &self.rxbuf;
let txbuf = &mut self.txbuf;
info!("Modbus frame: {:?}", rxbuf.as_slice());
+
+ txbuf.clear();
+ let capacity = txbuf.capacity();
+ let mut push = |x| -> _ { txbuf.push(x).or(Err(ServerDeviceFailure)) };
+ // see https://modbus.org/docs/Modbus_Application_Protocol_V1_1b3.pdf
match rxbuf[1] {
+ 0x02 => {
+ // read discrete inputs
+ let start = ((rxbuf[2] as u16) << 8) | rxbuf[3] as u16;
+ let quantity = ((rxbuf[4] as u16) << 8) | rxbuf[5] as u16;
+ let byte_count = (quantity+7)/8;
+ if quantity < 1 || quantity > 2000 || byte_count as usize > capacity - 5 {
+ return Err(IllegalDataValue);
+ };
+ push(rxbuf[0])?;
+ push(rxbuf[1])?;
+ push(byte_count as u8)?;
+ for i in 0..byte_count {
+ let mut x = 0;
+ for j in 0..7 {
+ if i*8+j < quantity {
+ x |= (if self.regs.read_discrete_input(start + i*8 + j)? {1} else {0}) << j;
+ }
+ }
+ push(x)?;
+ }
+ Ok(())
+ },
0x03 => {
// read holding registers
if rxbuf.len() != 8 {
@@ -95,16 +124,17 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
}
let start = ((rxbuf[2] as u16) << 8) | rxbuf[3] as u16;
let quantity = ((rxbuf[4] as u16) << 8) | rxbuf[5] as u16;
- if quantity as usize > (txbuf.capacity() - 5) / 2 || quantity >= 128 {
- return Err(IllegalDataValue); // is that right?
+ let byte_count = quantity * 2;
+ if byte_count as usize > (capacity - 5) || quantity >= 0x7d {
+ return Err(IllegalDataValue);
}
- txbuf.push(rxbuf[0]).or(Err(ServerDeviceFailure))?;
- txbuf.push(rxbuf[1]).or(Err(ServerDeviceFailure))?;
- txbuf.push((quantity*2) as u8).or(Err(ServerDeviceFailure))?;
+ push(rxbuf[0])?;
+ push(rxbuf[1])?;
+ push(byte_count as u8)?;
for i in 0..quantity {
- let value = self.regs.read_modbus_holding_register(start + i)?;
- txbuf.push((value >> 8) as u8).or(Err(ServerDeviceFailure))?;
- txbuf.push((value & 0xff) as u8).or(Err(ServerDeviceFailure))?;
+ let value = self.regs.read_holding_register(start + i)?;
+ push((value >> 8) as u8)?;
+ push((value & 0xff) as u8)?;
}
Ok(())
},
@@ -116,26 +146,112 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
}
let start = ((rxbuf[2] as u16) << 8) | rxbuf[3] as u16;
let quantity = ((rxbuf[4] as u16) << 8) | rxbuf[5] as u16;
- if quantity as usize > (txbuf.capacity() - 5) / 2 || quantity >= 128 {
+ if quantity as usize > (capacity - 5) / 2 || quantity >= 0x7d {
return Err(IllegalDataValue); // is that right?
}
- txbuf.push(rxbuf[0]).or(Err(ServerDeviceFailure))?;
- txbuf.push(rxbuf[1]).or(Err(ServerDeviceFailure))?;
- txbuf.push((quantity*2) as u8).or(Err(ServerDeviceFailure))?;
+ push(rxbuf[0])?;
+ push(rxbuf[1])?;
+ push((quantity*2) as u8)?;
for i in 0..quantity {
- let value = self.regs.read_modbus_input_register(start + i)?;
- txbuf.push((value >> 8) as u8).or(Err(ServerDeviceFailure))?;
- txbuf.push((value & 0xff) as u8).or(Err(ServerDeviceFailure))?;
+ let value = self.regs.read_input_register(start + i)?;
+ push((value >> 8) as u8)?;
+ push((value & 0xff) as u8)?;
+ }
+ Ok(())
+ },
+ 0x05 => {
+ // write single coil
+ if rxbuf.len() != 8 {
+ // we shouldn't get here
+ return Err(ServerDeviceFailure);
+ }
+ let start = ((rxbuf[2] as u16) << 8) | rxbuf[3] as u16;
+ let value = ((rxbuf[4] as u16) << 8) | rxbuf[5] as u16;
+ let value = if value == 0x0000 {
+ false
+ } else if value == 0xff00 {
+ true
+ } else {
+ return Err(IllegalDataValue);
+ };
+ self.regs.write_coil(start, value)?;
+ for i in 0..6 {
+ push(rxbuf[i])?;
}
Ok(())
},
0x06 => {
// write register
- Err(IllegalDataAddress)
+ if rxbuf.len() != 8 {
+ // we shouldn't get here
+ return Err(ServerDeviceFailure);
+ }
+ let start = ((rxbuf[2] as u16) << 8) | rxbuf[3] as u16;
+ let value = ((rxbuf[4] as u16) << 8) | rxbuf[5] as u16;
+ let actual_new_value = self.regs.write_register(start, value)?;
+ for i in 0..4 {
+ push(rxbuf[i])?;
+ }
+ push((actual_new_value >> 8) as u8)?;
+ push((actual_new_value & 0xff) as u8)?;
+ Ok(())
+ },
+ 0x0F => {
+ // write multiple coils
+ if rxbuf.len() < 9 {
+ // we shouldn't get here
+ return Err(ServerDeviceFailure);
+ }
+ let start = ((rxbuf[2] as u16) << 8) | rxbuf[3] as u16;
+ let quantity = ((rxbuf[4] as u16) << 8) | rxbuf[5] as u16;
+ let byte_count = rxbuf[6];
+ let expected_byte_count = (quantity + 7) / 8;
+ if quantity < 1 || quantity > 0x07b0 || byte_count as u16 != expected_byte_count {
+ return Err(IllegalDataValue);
+ }
+ if rxbuf.len() != 9 + byte_count as usize {
+ // we shouldn't get here
+ return Err(ServerDeviceFailure);
+ }
+ for i in 0..byte_count as u16 {
+ let x = rxbuf[7 + i as usize];
+ for j in 0..7 {
+ if i*8+j < quantity {
+ self.regs.write_coil(start + i*8 + j, (x >> j) != 0)?;
+ }
+ push(x)?;
+ }
+ }
+ for i in 0..6 {
+ push(rxbuf[i])?;
+ }
+ Ok(())
},
0x10 => {
// write multiple registers
- Err(IllegalDataAddress)
+ if rxbuf.len() < 9 {
+ // we shouldn't get here
+ return Err(ServerDeviceFailure);
+ }
+ let start = ((rxbuf[2] as u16) << 8) | rxbuf[3] as u16;
+ let quantity = ((rxbuf[4] as u16) << 8) | rxbuf[5] as u16;
+ let byte_count = rxbuf[6];
+ let expected_byte_count = quantity * 2;
+ if quantity < 1 || quantity > 0x07b || byte_count as u16 != expected_byte_count {
+ return Err(IllegalDataValue);
+ }
+ if rxbuf.len() != 9 + byte_count as usize {
+ // we shouldn't get here
+ return Err(ServerDeviceFailure);
+ }
+ for i in 0..quantity {
+ let value = ((rxbuf[7 + 2 * i as usize] as u16) << 8) | rxbuf[7 + 2 * i as usize + 1] as u16;
+ self.regs.write_register(start + i, value)?;
+ }
+ for i in 0..6 {
+ push(rxbuf[i])?;
+ }
+ Ok(())
},
_ => {
Err(IllegalFunction)
--
GitLab