mbo/feat-rp2040

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" }

build.rs

@@ -1,4 +1,3 @@
-use builder::build_config;
 
 fn main() {
     //build_config("config.yaml".to_string());

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}

.cargo/config.toml → 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"
-]

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"

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<Pwm0, FreeRunning>,
+}
+
+impl PwmChannel for Rp2040PwmChannel {
+    fn set_pwm(&mut self, pulse: u16) {
+        self.pwm.channel_a.set_duty_cycle(pulse).unwrap();
+    }
+}

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::<FunctionI2C>().into_pull_type::<PullUp>();
-    let scl_pin = pins.gpio5.into_function::<FunctionI2C>().into_pull_type::<PullUp>();
-    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::<FunctionPio0>();
-    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);
     }
 }

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"

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<u8>) {
+    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<u8> {
+    let mut frame = Vec::new();
+    frame.push(slave_addr);
+
+    frame.push(FunctionCode::WriteSingleRegister.value());
+    frame.extend_from_slice(&reg_addr.to_be_bytes());
+    frame.extend_from_slice(&reg_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);
 }

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 "----------------------------"

src/drivers/mod.rs

@@ -0,0 +1 @@
+pub mod pwm_servo;