diff --git a/Cargo.toml b/Cargo.toml index 3cab5b8..49d46a2 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,22 +1,10 @@ [package] -name = "lucy_embedded_firmware" +name = "lucy_embedded_firmware_core" version = "0.1.0" edition = "2024" -[workspace] -resolver = "2" -members = [ - "firmwares/rp2040" -] -exclude = [ -] - -[workspace.dependencies] -lucy_embedded_firmware = { path = "." } - [dependencies] crc = "3.4.0" modbus-core = "0.2.0" [build-dependencies] -builder = { path = "builder" } diff --git a/build.rs b/build.rs index e32fdb3..714b554 100644 --- a/build.rs +++ b/build.rs @@ -1,4 +1,3 @@ -use builder::build_config; fn main() { //build_config("config.yaml".to_string()); diff --git a/drivers/pwmservo.yaml b/drivers/pwmservo.yaml new file mode 100644 index 0000000..285c1f5 --- /dev/null +++ b/drivers/pwmservo.yaml @@ -0,0 +1,17 @@ +PWMServo: + config: + min_angle: {type: float, unit: rad, default: 0.0} + max_angle: {type: float, unit: rad, default: 0.0} + default_angle: {type: float, unit: rad, default: 0.0} + state: + angle: {access: rw, type: float, unit: rad, value: 0.0} + commands: + move: {opcode: 0x01} + reset: {opcode: 0x02} + calibrate: {opcode: 0x03} + +PressureSensor: + state: + value: {access: r, type: float, unit: raw} + commands: + read: {opcode: 0x01} diff --git a/.cargo/config.toml b/firmwares/rp2040/.cargo/config.toml similarity index 65% rename from .cargo/config.toml rename to firmwares/rp2040/.cargo/config.toml index cc2726b..1500dc3 100644 --- a/.cargo/config.toml +++ b/firmwares/rp2040/.cargo/config.toml @@ -4,8 +4,3 @@ rustflags = [ "-C", "link-arg=--nmagic", "-C", "link-arg=-Tlink.x", ] - -[target.avr-none] -rustflags = [ - "-C", "target-cpu=atmega328p" -] diff --git a/firmwares/rp2040/Cargo.toml b/firmwares/rp2040/Cargo.toml index 10fd9cf..c4dc2b5 100644 --- a/firmwares/rp2040/Cargo.toml +++ b/firmwares/rp2040/Cargo.toml @@ -3,8 +3,14 @@ name = "lucy_embedded_firmware_rp2040" version = "0.1.0" edition = "2024" +[profile.dev] +panic = "abort" + +[profile.release] +panic = "abort" + [dependencies] -#app-core = { workspace = true } +lucy_embedded_firmware_core = { path = "../.." } cortex-m = "0.7.9" cortex-m-rt = "0.7.6" embedded-hal = "1.0.0" diff --git a/firmwares/rp2040/src/channel.rs b/firmwares/rp2040/src/channel.rs new file mode 100644 index 0000000..d81ca4d --- /dev/null +++ b/firmwares/rp2040/src/channel.rs @@ -0,0 +1,33 @@ +use lucy_embedded_firmware_core::pwm::{PwmChannel}; + +use embedded_hal::{ + delay::DelayNs, + digital::OutputPin, + pwm::SetDutyCycle, + i2c::I2c, +}; + +use rp2040_hal::{ + fugit::RateExtU32, + fugit::MicrosDuration, + clocks::init_clocks_and_plls, + gpio::{Pins, FunctionPio0, FunctionPwm, FunctionI2C, PullUp}, + pac, + i2c::I2C, + pwm::{Slices, Pwm0, Slice, FreeRunning}, + pio::PIOExt, + sio::Sio, + timer::Timer, + watchdog::Watchdog, + Clock +}; + +pub struct Rp2040PwmChannel { + pub pwm: Slice, +} + +impl PwmChannel for Rp2040PwmChannel { + fn set_pwm(&mut self, pulse: u16) { + self.pwm.channel_a.set_duty_cycle(pulse).unwrap(); + } +} diff --git a/firmwares/rp2040/src/main.rs b/firmwares/rp2040/src/main.rs index 94fdc73..6c4a454 100644 --- a/firmwares/rp2040/src/main.rs +++ b/firmwares/rp2040/src/main.rs @@ -1,21 +1,34 @@ #![no_std] #![no_main] -use cortex_m_rt::entry; -use panic_halt as _; +mod channel; +use channel::Rp2040PwmChannel; + +use lucy_embedded_firmware_core::pwm::{PwmChannel}; +use lucy_embedded_firmware_core::drivers::pwm_servo::{PwmServoDriver, PwmServoModbusAdapter}; +use lucy_embedded_firmware_core::modbus::{ + ModbusError, + ModbusAdapter, + RegisterView, RegisterTable, + Slave, + parse_modbus_frame, route_modbus_request +}; -use embedded_hal::delay::DelayNs; -use embedded_hal::digital::OutputPin; -use embedded_hal::pwm::SetDutyCycle; -use embedded_hal::i2c::I2c; +use embedded_hal::{ + delay::DelayNs, + digital::OutputPin, + pwm::SetDutyCycle, + i2c::I2c, +}; use rp2040_hal::{ fugit::RateExtU32, + fugit::MicrosDuration, clocks::init_clocks_and_plls, gpio::{Pins, FunctionPio0, FunctionPwm, FunctionI2C, PullUp}, pac, i2c::I2C, - pwm::{Slices, Pwm0}, + pwm::{Slices, Pwm0, Slice, FreeRunning}, pio::PIOExt, sio::Sio, timer::Timer, @@ -28,13 +41,14 @@ use ws2812_pio::Ws2812; use usb_device::{class_prelude::*, prelude::*}; use usbd_serial::SerialPort; +use cortex_m_rt::entry; +use panic_halt as _; + #[unsafe(link_section = ".boot2")] #[unsafe(no_mangle)] #[used] pub static BOOT2: [u8; 256] = rp2040_boot2::BOOT_LOADER_GENERIC_03H; - - #[entry] fn main() -> ! { let core = cortex_m::Peripherals::take().unwrap(); @@ -52,34 +66,19 @@ fn main() -> ! { &mut watchdog, ).ok().unwrap(); let mut delay = cortex_m::delay::Delay::new(core.SYST, clocks.system_clock.freq().raw()); - let timer = Timer::new(pac.TIMER, &mut pac.RESETS, &clocks); - let pins = Pins::new(pac.IO_BANK0, pac.PADS_BANK0, sio.gpio_bank0, &mut pac.RESETS); - let sda_pin = pins.gpio4.into_function::().into_pull_type::(); - let scl_pin = pins.gpio5.into_function::().into_pull_type::(); - const ADDR: u8 = 0x40; - let mut buffer = [0u8; 1]; - - - let (mut pio, sm0, _, _, _) = pac.PIO0.split(&mut pac.RESETS); - let led_pin = pins.gpio18.into_function::(); - let mut ws = Ws2812::new( - led_pin, - &mut pio, - sm0, - clocks.peripheral_clock.freq(), - timer.count_down() - ); + /* PWM */ let pwm_slices = Slices::new(pac.PWM, &mut pac.RESETS); let mut pwm = pwm_slices.pwm0; pwm.set_div_int(100); pwm.set_top(25_000 - 1); + pwm.channel_a.output_to(pins.gpio0); pwm.enable(); - let mut channel = pwm.channel_a; - channel.output_to(pins.gpio0); + + /* USB */ let usb_bus = UsbBusAllocator::new(rp2040_hal::usb::UsbBus::new( pac.USBCTRL_REGS, @@ -100,22 +99,100 @@ fn main() -> ! { .device_class(usbd_serial::USB_CLASS_CDC) .build(); - let mut counter = 0; - let mut leds = [RGB8::default(); 6]; - leds[0].r = 255; - ws.write(leds.iter().copied()).unwrap_or(()); + let channel = Rp2040PwmChannel { + pwm: pwm + }; + + let mut driver = PwmServoDriver { + channel: channel, + min_pulse: 1250, + max_pulse: 2500, + min_angle: 0, + max_angle: 180, + default_angle: 90 + }; + + let mut adapter = PwmServoModbusAdapter { + base_register: 0x00, + cmd_reg_off: 0, + angle_reg_off: 1, + driver: &mut driver + }; + + let mut rt = RegisterTable::default(); + let mut rv = RegisterView { + table: &rt, + base_register: 0, + nb_register: 2 + }; + + + let slave = Slave { + address: 0x01, + }; + + + let mut rx_buf = [0u8; 256]; + let mut rx_len = 0; + let mut rx_active_timer = false; + let mut last_rx_micros: u64 = 0; loop { + let now = timer.get_counter().ticks(); + if usb_dev.poll(&mut [&mut serial]) { - let mut buf = [0u8; 64]; - let _ = serial.read(&mut buf); + let mut tmp_buf = [0u8; 64]; + + while let Ok(count) = serial.read(&mut tmp_buf) { + if count == 0 { + break; + } + if rx_len + count <= rx_buf.len() { + rx_buf[rx_len..rx_len + count].copy_from_slice(&tmp_buf[..count]); + rx_len += count; + last_rx_micros = now; + rx_active_timer = true; + } else { + rx_active_timer = false; + rx_len = 0; + break; + } + } + } + if rx_active_timer && (now.saturating_sub(last_rx_micros) >= 3000) { + rx_active_timer = false; + if rx_len > 4 { + let raw_request = parse_modbus_frame(&slave, &rx_buf[..rx_len]); + match raw_request { + Ok(request) => { + serial.write(b"Request received and processed\n"); + route_modbus_request(&rt, request); + } + Err(error) => match error { + ModbusError::InvalidAddress => { + serial.write(b"InvalidAddress\n"); + } + ModbusError::InvalidFrame => { + serial.write(b"InvalidFrame\n"); + } + ModbusError::CrcError => { + serial.write(b"CrcError\n"); + } + ModbusError::UnknownOpcode => { + serial.write(b"UnknownOpcode\n"); + } + _ => { + serial.write(b"Error\n"); + } + } + } + } else { + serial.write(b"Skipping"); + } + rx_len = 0; } - let _ = serial.write(b"Hello! Le RP2040 tourne correctement.\r\n"); - channel.set_duty_cycle(1000).unwrap(); - delay.delay_ms(1000); - channel.set_duty_cycle(2500).unwrap(); - delay.delay_ms(1000); + adapter.tick(&mut rv); } } diff --git a/firmwares/sim/Cargo.toml b/firmwares/sim/Cargo.toml index 78ad5c3..ce273a9 100644 --- a/firmwares/sim/Cargo.toml +++ b/firmwares/sim/Cargo.toml @@ -4,4 +4,7 @@ version = "0.1.0" edition = "2024" [dependencies] -app-core = { workspace = true } +lucy_embedded_firmware_core = { path = "../.." } +crc = "3.4.0" +modbus-core = "0.2.0" +serialport = "4.10.0" diff --git a/firmwares/sim/src/main.rs b/firmwares/sim/src/main.rs index 8c91626..9c6adcc 100644 --- a/firmwares/sim/src/main.rs +++ b/firmwares/sim/src/main.rs @@ -1,3 +1,52 @@ +use std::io::{Read, Write}; +use std::time::Duration; +use modbus_core::{Request, Response, FunctionCode}; +use serialport::{SerialPortType, UsbPortInfo}; + +fn append_crc(frame: &mut Vec) { + let crc = modbus_core::rtu::crc16(&frame); + frame.extend_from_slice(&crc.to_be_bytes()); +} + +fn write_register( + slave_addr: u8, + reg_addr: u16, + reg_value: u16 +) -> Vec { + let mut frame = Vec::new(); + frame.push(slave_addr); + + frame.push(FunctionCode::WriteSingleRegister.value()); + frame.extend_from_slice(®_addr.to_be_bytes()); + frame.extend_from_slice(®_value.to_be_bytes()); + + append_crc(&mut frame); + frame +} + fn main() { - println!("Hello, world sim!"); + let target_vid = 0x16c0; + let target_pid = 0x27dd; + + let ports = serialport::available_ports().unwrap(); + let matching_port = ports.into_iter().find(|p| { + if let SerialPortType::UsbPort(UsbPortInfo { vid, pid, .. }) = p.port_type { + vid == target_vid && pid == target_pid + } else { + false + } + }); + + let port_info = matching_port.ok_or("Périphérique RP2040 introuvable. Est-il branché ?").unwrap(); + println!("Périphérique trouvé sur : {}", port_info.port_name); + + let mut port = serialport::new(&port_info.port_name, 115_200) + .timeout(Duration::from_millis(1000)) + .open().unwrap(); + + let mut angle: u16 = 180; + + let packet = write_register(0x01, 0x01, angle); + let _ = port.write_all(&packet); + println!("Packet send: >{:x?}<", packet); } diff --git a/flake.nix b/flake.nix index 757b0ef..8904d6c 100644 --- a/flake.nix +++ b/flake.nix @@ -27,12 +27,15 @@ name = "Lucy Embedded Firmware"; packages = [ rustToolchain + pkgs.udev + pkgs.systemd pkgs.flip-link pkgs.probe-rs-tools pkgs.elf2uf2-rs pkgs.picotool ]; shellHook = '' + export LD_LIBRARY_PATH="${pkgs.udev}/lib:$LD_LIBRARY_PATH" echo -e "" echo -e "🛡️ \033[1;36mLucy Embedded Firmware\033[1;0m" echo -e "----------------------------" diff --git a/src/drivers/mod.rs b/src/drivers/mod.rs new file mode 100644 index 0000000..6bb5d1d --- /dev/null +++ b/src/drivers/mod.rs @@ -0,0 +1 @@ +pub mod pwm_servo; diff --git a/src/drivers/pwm_servo.rs b/src/drivers/pwm_servo.rs new file mode 100644 index 0000000..14977bf --- /dev/null +++ b/src/drivers/pwm_servo.rs @@ -0,0 +1,59 @@ +use crate::{pwm::PwmChannel}; +use crate::{modbus::RegisterView, modbus::ModbusAdapter, utils::map_range}; + +pub struct PwmServoDriver { + pub channel: C, + pub min_pulse: u16, + pub max_pulse: u16, + pub min_angle: u16, + pub max_angle: u16, + pub default_angle: u16 +} + +impl PwmServoDriver { + pub fn move_angle(&mut self, angle: u16) { + let angle = angle.clamp(self.min_angle, self.max_angle); + let pulse = map_range(angle as f32, self.min_angle as f32, self.max_angle as f32, self.min_pulse as f32, self.max_pulse as f32) as u16; + self.channel.set_pwm(pulse); + } + + pub fn reset_angle(&mut self) { + let angle = self.default_angle.clamp(self.min_angle, self.max_angle); + let pulse = map_range(angle as f32, self.min_angle as f32, self.max_angle as f32, self.min_pulse as f32, self.max_pulse as f32) as u16; + self.channel.set_pwm(pulse); + } +} + +pub struct PwmServoModbusAdapter<'a, C> { + pub base_register: u16, + pub cmd_reg_off: u16, + pub angle_reg_off: u16, + pub driver: &'a mut PwmServoDriver, +} + +impl<'a, C: PwmChannel> ModbusAdapter for PwmServoModbusAdapter<'a, C> { + fn tick(&mut self, rv: &mut RegisterView) { + let cmd = rv.read_register(self.cmd_reg_off); + rv.write_register(self.cmd_reg_off, 0); + match cmd { + 1 => { + let angle = rv.read_register(self.angle_reg_off); + self.driver.move_angle(angle); + }, + 2 => { + self.driver.reset_angle(); + }, + _ => { + + } + } + } + + fn get_nb_register(&self) -> u16 { + 2 + } + + fn get_base_register(&self) -> u16 { + self.base_register + } +} diff --git a/src/lib.rs b/src/lib.rs index 3774d3a..2a08da3 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,2 +1,9 @@ +#![no_std] + pub mod modbus; pub mod transport; +pub mod pwm; +pub mod utils; + +pub mod drivers; + diff --git a/src/modbus.rs b/src/modbus.rs index 97e337f..4847c4c 100644 --- a/src/modbus.rs +++ b/src/modbus.rs @@ -1,35 +1,74 @@ -use modbus_core::Request; -use modbus_core::Response; +use modbus_core::{Request, Response}; use crc::{Crc, CRC_16_MODBUS}; +use core::{ + cell::Cell, + error::Error, + result::Result::{self, Err, Ok}, + option::Option::{self, None, Some}, +}; + const MODBUS_CRC: Crc = Crc::::new(&CRC_16_MODBUS); +/* Trait definition */ + +pub trait ModbusAdapter { + fn tick(&mut self, rv: &mut RegisterView); + fn get_nb_register(&self) -> u16; + fn get_base_register(&self) -> u16; +} + +/* Registers structure definition */ pub struct RegisterTable { - pub registers: [u16; 0xFF] + pub registers: [Cell; 0xFF], +} + +impl RegisterTable { + pub const fn new() -> Self { + Self { + registers: [const { Cell::new(0) }; 0xFF], + } + } +} + +impl Default for RegisterTable { + fn default() -> Self { + Self::new() + } } pub struct RegisterView<'a> { - pub table: &'a mut RegisterTable + pub table: &'a RegisterTable, + pub base_register: u16, + pub nb_register: u16, } impl<'a> RegisterView<'a> { - pub fn new(table: &'a mut RegisterTable) -> Self { - RegisterView { table } + pub fn new(table: &'a RegisterTable, base_register: u16, nb_register: u16) -> Self { + RegisterView { table, base_register, nb_register } } pub fn read_register(&self, index: u16) -> u16 { - if index < self.table.registers.len() as u16 { - self.table.registers[index as usize] + if index > self.nb_register { + return 0; + } + let reg = self.base_register + index; + if reg < self.table.registers.len() as u16 { + self.table.registers[reg as usize].get() } else { 0 } } pub fn write_register(&mut self, index: u16, value: u16) { - if index < self.table.registers.len() as u16 { - self.table.registers[index as usize] = value; + if index > self.nb_register { + return; + } + let reg = self.base_register + index; + if reg < self.table.registers.len() as u16 { + self.table.registers[reg as usize].set(value); } } } @@ -47,6 +86,15 @@ pub enum ModbusError { UnknownOpcode } +pub fn check_crc(frame: &[u8]) -> bool { + let len = frame.len(); + let payload = &frame[..len - 2]; + let crc = MODBUS_CRC.checksum(payload); + let crc_received = u16::from_le_bytes([frame[len - 2], frame[len - 1]]); + + crc == crc_received +} + pub fn parse_modbus_frame<'a>(slave: &'a Slave, frame: &'a[u8]) -> Result, ModbusError> { let len = frame.len(); @@ -56,11 +104,7 @@ pub fn parse_modbus_frame<'a>(slave: &'a Slave, frame: &'a[u8]) -> Result(slave: &'a Slave, frame: &'a[u8]) -> Result) -> Result<(), ModbusError> { +pub fn route_modbus_request(register_table: &RegisterTable, request: Request<'_>) -> Result<(), ModbusError> { match request { Request::ReadHoldingRegisters(addr, quantity) => { let registers = ®ister_table.registers[addr as usize..(addr + quantity) as usize]; Ok(()) }, Request::WriteSingleRegister(addr, value) => { - register_table.registers[addr as usize] = value; + register_table.registers[addr as usize].set(value); Ok(()) }, Request::WriteMultipleRegisters(addr, data) => { @@ -89,7 +133,7 @@ pub fn route_modbus_request(register_table: &mut RegisterTable, request: Request } for i in 0..data.len() { if let Some(value) = data.get(i) { - register_table.registers[addr as usize + i] = value; + register_table.registers[addr as usize + i].set(value); } } Ok(()) diff --git a/src/pwm/mod.rs b/src/pwm/mod.rs new file mode 100644 index 0000000..a13a6d3 --- /dev/null +++ b/src/pwm/mod.rs @@ -0,0 +1,2 @@ +pub mod pwm_channel; +pub use pwm_channel::*; diff --git a/src/pwm/pwm_channel.rs b/src/pwm/pwm_channel.rs new file mode 100644 index 0000000..1c7c402 --- /dev/null +++ b/src/pwm/pwm_channel.rs @@ -0,0 +1,3 @@ +pub trait PwmChannel { + fn set_pwm(&mut self, pulse: u16); +} diff --git a/src/transport.rs b/src/transport.rs index c44d07a..ad505b7 100644 --- a/src/transport.rs +++ b/src/transport.rs @@ -1,13 +1,4 @@ -use std::fs::File; -use std::io::{self, Write, Read}; - -pub trait Transport { - type Error; - - fn write(&mut self, data: &[u8]) -> std::result::Result<(), Self::Error>; - fn read(&mut self, buffer: &mut [u8]) -> std::result::Result; - fn flush(&mut self) -> std::result::Result<(), Self::Error>; -} +use core::result::Result; pub enum TransportError { WriteError, @@ -15,81 +6,10 @@ pub enum TransportError { FlushError, } -pub struct DummyTransport { -} - -impl Transport for DummyTransport { - type Error = TransportError; - - fn write(&mut self, data: &[u8]) -> std::result::Result<(), Self::Error> { - Ok(()) - } - - fn read(&mut self, buffer: &mut [u8]) -> std::result::Result { - let mut ibuffer = String::new(); - io::stdin().read_line(&mut ibuffer).expect("Failed to read line"); - let len = usize::min(ibuffer.len(), buffer.len()); - buffer[..len].copy_from_slice(&ibuffer[..len].as_bytes()); - Ok(len) - } - - fn flush(&mut self) -> std::result::Result<(), Self::Error> { - Ok(()) - } -} - -pub struct NamedPipeTransport { - path: String, - file: Option -} - -impl NamedPipeTransport { - pub fn new(path: String) -> Self { - NamedPipeTransport { - path, - file: None - } - } - - pub fn open(&mut self) -> io::Result<()> { - let file = File::open(&self.path)?; - self.file = Some(file); - Ok(()) - } - - pub fn close(&mut self) { - self.file = None; - } -} - -impl Transport for NamedPipeTransport { - type Error = io::Error; - - fn write(&mut self, data: &[u8]) -> std::result::Result<(), Self::Error> { - if let Some(file) = &mut self.file { - file.write_all(data)?; - file.flush()?; - Ok(()) - } else { - Err(io::Error::new(io::ErrorKind::NotConnected, "Pipe not open")) - } - } - - fn read(&mut self, buffer: &mut [u8]) -> std::result::Result { - if let Some(file) = &mut self.file { - let bytes_read = file.read(buffer)?; - Ok(bytes_read) - } else { - Err(io::Error::new(io::ErrorKind::NotConnected, "Pipe not open")) - } - } +pub trait Transport { + type Error; - fn flush(&mut self) -> std::result::Result<(), Self::Error> { - if let Some(file) = &mut self.file { - file.flush()?; - Ok(()) - } else { - Err(io::Error::new(io::ErrorKind::NotConnected, "Pipe not open")) - } - } + fn write(&mut self, data: &[u8]) -> Result<(), Self::Error>; + fn read(&mut self, buffer: &mut [u8]) -> Result; + fn flush(&mut self) -> Result<(), Self::Error>; } diff --git a/src/utils.rs b/src/utils.rs new file mode 100644 index 0000000..a587dfb --- /dev/null +++ b/src/utils.rs @@ -0,0 +1,3 @@ +pub fn map_range(val: f32, in_min: f32, in_max: f32, out_min: f32, out_max: f32) -> f32 { + out_min + (val - in_min) * (out_max - out_min) / (in_max - in_min) +}