From bc920af1cb48dd1a646edd57c127e936e9408fd0 Mon Sep 17 00:00:00 2001 From: Jaci Brunning Date: Thu, 4 Jan 2024 20:17:25 +0800 Subject: [PATCH] Initial move to a unified impl --- lasercan-bootloader/Cargo.lock | 42 +-- lasercan-bootloader/Cargo.toml | 4 +- lasercan-bootloader/src/main.rs | 182 ++++++------ lasercan-firmware/Cargo.toml | 1 + lasercan-firmware/src/configuration.rs | 42 --- lasercan-impl/.gitignore | 1 + lasercan-impl/Cargo.lock | 244 ++++++++++++++++ lasercan-impl/Cargo.toml | 9 + lasercan-impl/src/lib.rs | 386 +++++++++++++++++++++++++ 9 files changed, 758 insertions(+), 153 deletions(-) delete mode 100644 lasercan-firmware/src/configuration.rs create mode 100644 lasercan-impl/.gitignore create mode 100644 lasercan-impl/Cargo.lock create mode 100644 lasercan-impl/Cargo.toml create mode 100644 lasercan-impl/src/lib.rs diff --git a/lasercan-bootloader/Cargo.lock b/lasercan-bootloader/Cargo.lock index 698a17a..32d1f9f 100644 --- a/lasercan-bootloader/Cargo.lock +++ b/lasercan-bootloader/Cargo.lock @@ -4,9 +4,9 @@ version = 3 [[package]] name = "anyhow" -version = "1.0.76" +version = "1.0.79" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "59d2a3357dde987206219e78ecfbbb6e8dad06cbb65292758d3270e6254f7355" +checksum = "080e9890a082662b09c1ad45f567faeeb47f22b5fb23895fbe1e651e718e25ca" [[package]] name = "atomic-polyfill" @@ -40,9 +40,9 @@ checksum = "f8fe8f5a8a398345e52358e18ff07cc17a568fbca5c6f73873d3a62056309603" [[package]] name = "binmarshal" -version = "0.2.8" +version = "1.0.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ef52819335fe738d04a0941a49ccd3b54c12660611456e2919a6bbb86787bc7a" +checksum = "3f9334da3cc3c1f73c4cb5849f784cfe380a12d604670f91f24cfd9bfa0b3b35" dependencies = [ "anyhow", "binmarshal-macros", @@ -50,14 +50,14 @@ dependencies = [ [[package]] name = "binmarshal-macros" -version = "0.2.7" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a28cee2e730b6ddc4ea2afe66696682e79957e6f687f28a440d9124a532d4ddb" +checksum = "9a928be789849c207d60b342918373711573e1a18635e132297b35f4cf2b2c7d" dependencies = [ "darling", "proc-macro2", "quote", - "syn 2.0.43", + "syn 2.0.46", ] [[package]] @@ -178,7 +178,7 @@ dependencies = [ "proc-macro2", "quote", "strsim", - "syn 2.0.43", + "syn 2.0.46", ] [[package]] @@ -189,7 +189,7 @@ checksum = "836a9bbc7ad63342d6d6e7b815ccab164bc77a2d95d84bc3117a8c0d5c98e2d5" dependencies = [ "darling_core", "quote", - "syn 2.0.43", + "syn 2.0.46", ] [[package]] @@ -254,9 +254,9 @@ checksum = "1d758ba1b47b00caf47f24925c0074ecb20d6dfcffe7f6d53395c0465674841a" [[package]] name = "grapple-frc-msgs" -version = "2024.0.6" +version = "2024.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7873a0302eaf78396f0f8def395b470f6ebe36649e770fa3312d7ad31f073072" +checksum = "3d123327d9ef5e72c352a06ae93df8319b54b65940bbf9d22d80ebadf1fd828b" dependencies = [ "anyhow", "binmarshal", @@ -316,7 +316,7 @@ dependencies = [ [[package]] name = "lasercan-bootloader" -version = "0.3.1" +version = "0.3.2" dependencies = [ "bxcan", "cortex-m", @@ -403,18 +403,18 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.71" +version = "1.0.74" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "75cb1540fadbd5b8fbccc4dddad2734eba435053f725621c070711a14bb5f4b8" +checksum = "2de98502f212cfcea8d0bb305bd0f49d7ebdd75b64ba0a68f937d888f4e0d6db" dependencies = [ "unicode-ident", ] [[package]] name = "quote" -version = "1.0.33" +version = "1.0.35" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5267fca4496028628a95160fc423a33e8b2e6af8a5302579e322e4b520293cae" +checksum = "291ec9ab5efd934aaf503a6466c5d5251535d108ee747472c3977cc5acc868ef" dependencies = [ "proc-macro2", ] @@ -458,7 +458,7 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366" dependencies = [ - "semver 1.0.20", + "semver 1.0.21", ] [[package]] @@ -484,9 +484,9 @@ dependencies = [ [[package]] name = "semver" -version = "1.0.20" +version = "1.0.21" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "836fa6a3e1e547f9a2c4040802ec865b5d85f4014efe00555d7090a3dcaa1090" +checksum = "b97ed7a9823b74f99c7742f5336af7be5ecd3eeafcb1507d1fa93347b1d589b0" [[package]] name = "semver-parser" @@ -590,9 +590,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.43" +version = "2.0.46" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ee659fb5f3d355364e1f3e5bc10fb82068efbf824a1e9d1c9504244a6469ad53" +checksum = "89456b690ff72fddcecf231caedbe615c59480c93358a93dfae7fc29e3ebbf0e" dependencies = [ "proc-macro2", "quote", diff --git a/lasercan-bootloader/Cargo.toml b/lasercan-bootloader/Cargo.toml index 037c832..469cdd3 100644 --- a/lasercan-bootloader/Cargo.toml +++ b/lasercan-bootloader/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "lasercan-bootloader" -version = "0.3.1" +version = "0.3.2" edition = "2021" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html @@ -12,7 +12,7 @@ cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } cortex-m-rt = "0.7.1" panic-halt = "0.2.0" stm32f1xx-hal = { version = "0.10.0", features = ["rt", "stm32f103", "medium"] } -grapple-frc-msgs = { version = "~2024.0.5", default-features = false } +grapple-frc-msgs = { version = "~2024.2.0", default-features = false } bxcan = "0.7.0" embedded-alloc = "0.5.1" lasercan-common = { path = "../lasercan-common" } diff --git a/lasercan-bootloader/src/main.rs b/lasercan-bootloader/src/main.rs index ffdd8b2..cb88db7 100644 --- a/lasercan-bootloader/src/main.rs +++ b/lasercan-bootloader/src/main.rs @@ -34,11 +34,11 @@ const WATCHDOG_COUNTER_ADDR: usize = 0x2000_0000; #[rtic::app(device = stm32f1xx_hal::pac, peripherals = true)] mod app { - use alloc::{collections::VecDeque, borrow::ToOwned}; + use alloc::collections::VecDeque; use bxcan::{ExtendedId, filter::Mask32, Interrupts, Rx0, Tx}; use core::ops::Deref; use cortex_m::peripheral::SCB; - use grapple_frc_msgs::{DEVICE_TYPE_FIRMWARE_UPGRADE, binmarshal::{BinMarshal, rw::BitView}, grapple::{MANUFACTURER_GRAPPLE, GrappleBroadcastMessage, device_info::GrappleDeviceInfo, GrappleDeviceMessage, firmware::GrappleFirmwareMessage, TaggedGrappleMessage, fragments::FragmentReassembler}, DEVICE_ID_BROADCAST, DEVICE_TYPE_BROADCAST, ManufacturerMessage, Message, MessageId}; + use grapple_frc_msgs::{DEVICE_TYPE_FIRMWARE_UPGRADE, binmarshal::{Demarshal, rw::BitView}, grapple::{MANUFACTURER_GRAPPLE, GrappleBroadcastMessage, device_info::GrappleDeviceInfo, GrappleDeviceMessage, firmware::GrappleFirmwareMessage, TaggedGrappleMessage, fragments::{FragmentReassembler, FragmentReassemblerRx, FragmentReassemblerTx}}, DEVICE_ID_BROADCAST, DEVICE_TYPE_BROADCAST, ManufacturerMessage, MessageId}; use lasercan_common::bootutil::{is_firmware_update_in_progress, is_user_code_present, FLASH_USER, FLASH_META_FIRMWARE_RESET_LOC}; use stm32f1xx_hal::{prelude::*, watchdog::IndependentWatchdog, can::Can, pac::{CAN1, Interrupt}, gpio::{ErasedPin, Output}, flash::{self, FLASH_START}}; use tiny_rng::Rand; @@ -50,13 +50,15 @@ mod app { can_tx_queue: VecDeque, status_led: ErasedPin, device_id: u8, + #[lock_free] + reassemble_tx: FragmentReassemblerTx } #[local] struct LocalResources { can_rx: Rx0>, can_tx: Tx>, - reassemble: FragmentReassembler, + reassemble_rx: FragmentReassemblerRx, flash: flash::Parts, offset: usize, rng: tiny_rng::Rng @@ -145,7 +147,9 @@ mod app { // Perform arbitration let mut queue = VecDeque::with_capacity(16); - enqueue(TaggedGrappleMessage::new( + let (reassemble_rx, reassemble_tx) = FragmentReassembler::new(1000, 8).split(); + + enqueue(&&reassemble_tx, TaggedGrappleMessage::new( 0, GrappleDeviceMessage::Broadcast( GrappleBroadcastMessage::DeviceInfo(GrappleDeviceInfo::ArbitrationRequest) @@ -156,11 +160,12 @@ mod app { SharedResources { device_id: 0, can_tx_queue: queue, - status_led: status_led.erase() + status_led: status_led.erase(), + reassemble_tx }, LocalResources { can_rx, can_tx, flash, - reassemble: FragmentReassembler::new(1000), + reassemble_rx, offset: 0, rng }, @@ -169,7 +174,7 @@ mod app { } #[idle(shared = [], local = [])] - fn idle(ctx: idle::Context) -> ! { + fn idle(_: idle::Context) -> ! { loop { } } @@ -199,7 +204,7 @@ mod app { }); } - #[task(binds = USB_LP_CAN_RX0, shared = [can_tx_queue, status_led, device_id], local = [can_rx, flash, reassemble, offset, rng])] + #[task(binds = USB_LP_CAN_RX0, shared = [can_tx_queue, status_led, device_id, reassemble_tx], local = [can_rx, flash, reassemble_rx, offset, rng])] fn can_rx(mut ctx: can_rx::Context) { let my_serial = lasercan_common::get_serial_hash(); @@ -214,94 +219,95 @@ mod app { if id.device_id == DEVICE_ID_BROADCAST || id.device_id == my_id { let d = data.map(|x| x.deref()).unwrap_or(&[]); match ManufacturerMessage::read(&mut BitView::new(d), id.clone()) { - Some(ManufacturerMessage::Grapple(grpl_msg)) => { - match ctx.local.reassemble.defragment(0 as i64, &id, grpl_msg) { - Some(GrappleDeviceMessage::Broadcast(bmsg)) => match bmsg { - GrappleBroadcastMessage::DeviceInfo(di) => match di { - GrappleDeviceInfo::ArbitrationRequest if id.device_id == my_id => { - // Reply to the arbitration request with a rejection, since this ID belongs to us. - let msg = TaggedGrappleMessage::new( - my_id, - GrappleDeviceMessage::Broadcast( - GrappleBroadcastMessage::DeviceInfo(GrappleDeviceInfo::ArbitrationReject) - ) - ); - ctx.shared.can_tx_queue.lock(|q| enqueue(msg, q)); - }, - GrappleDeviceInfo::ArbitrationReject if id.device_id == my_id => { - // Retry Arbitration. - let msg = ctx.shared.device_id.lock(|did| { - *did = ctx.local.rng.rand_u8() & 0b11111; - TaggedGrappleMessage::new( - *did, + Ok(ManufacturerMessage::Grapple(grpl_msg)) => { + ctx.local.reassemble_rx.defragment(0 as i64, &id, grpl_msg, |msg| { + match msg { + GrappleDeviceMessage::Broadcast(bmsg) => match bmsg { + GrappleBroadcastMessage::DeviceInfo(di) => match di { + GrappleDeviceInfo::ArbitrationRequest if id.device_id == my_id => { + // Reply to the arbitration request with a rejection, since this ID belongs to us. + let msg = TaggedGrappleMessage::new( + my_id, GrappleDeviceMessage::Broadcast( - GrappleBroadcastMessage::DeviceInfo(GrappleDeviceInfo::ArbitrationRequest) + GrappleBroadcastMessage::DeviceInfo(GrappleDeviceInfo::ArbitrationReject) ) - ) - }); - - ctx.shared.can_tx_queue.lock(|q| enqueue(msg, q)); + ); + ctx.shared.can_tx_queue.lock(|q| enqueue(ctx.shared.reassemble_tx, msg, q)); + }, + GrappleDeviceInfo::ArbitrationReject if id.device_id == my_id => { + // Retry Arbitration. + let msg = ctx.shared.device_id.lock(|did| { + *did = ctx.local.rng.rand_u8() & 0b11111; + TaggedGrappleMessage::new( + *did, + GrappleDeviceMessage::Broadcast( + GrappleBroadcastMessage::DeviceInfo(GrappleDeviceInfo::ArbitrationRequest) + ) + ) + }); + + ctx.shared.can_tx_queue.lock(|q| enqueue(&ctx.shared.reassemble_tx, msg, q)); + }, + GrappleDeviceInfo::EnumerateRequest => { + let new_msg = TaggedGrappleMessage::new( + my_id, + GrappleDeviceMessage::Broadcast( + GrappleBroadcastMessage::DeviceInfo(GrappleDeviceInfo::EnumerateResponse { + model_id: META_MODEL_ID.clone(), + serial: my_serial, + is_dfu: true, + is_dfu_in_progress: *ctx.local.offset != 0, + version: META_BOOTLOADER_VERSION, + name: "" + }) + ) + ); + ctx.shared.can_tx_queue.lock(|q| enqueue(&ctx.shared.reassemble_tx, new_msg, q)); + }, + _ => () }, - GrappleDeviceInfo::EnumerateRequest => { + }, + GrappleDeviceMessage::FirmwareUpdate(fwupdate) => match fwupdate { + GrappleFirmwareMessage::StartFieldUpgrade { .. } => (), + GrappleFirmwareMessage::UpdatePart(data) => { + ctx.shared.status_led.lock(|led| led.toggle()); + + let mut flash_writer = ctx.local.flash.writer(flash::SectorSize::Sz1K, flash::FlashSize::Sz64K); + let user_code_offset = FLASH_USER as usize - FLASH_START as usize; + + // Erase the sector if required. Obviously, this requires our data step size to be a factor of 1024. + if (user_code_offset + *ctx.local.offset) as u32 % 1024 == 0 { + // Erase the flash region + flash_writer.erase((user_code_offset + *ctx.local.offset) as u32, 1024).unwrap(); + } + + // Write the flash data (must be aligned to 16 bits) + flash_writer.write((user_code_offset + *ctx.local.offset) as u32, &data[..]).unwrap(); + + *ctx.local.offset += data.len() as usize; + + // Reply with an ACK let new_msg = TaggedGrappleMessage::new( my_id, - GrappleDeviceMessage::Broadcast( - GrappleBroadcastMessage::DeviceInfo(GrappleDeviceInfo::EnumerateResponse { - model_id: META_MODEL_ID.clone(), - serial: my_serial, - is_dfu: true, - is_dfu_in_progress: *ctx.local.offset != 0, - version: META_BOOTLOADER_VERSION.to_owned(), - name: "".to_owned() - }) + GrappleDeviceMessage::FirmwareUpdate( + GrappleFirmwareMessage::UpdatePartAck ) ); - ctx.shared.can_tx_queue.lock(|q| enqueue(new_msg, q)); + ctx.shared.can_tx_queue.lock(|q| enqueue(&ctx.shared.reassemble_tx, new_msg, q)); }, - _ => () - }, - }, - Some(GrappleDeviceMessage::FirmwareUpdate(fwupdate)) => match fwupdate { - GrappleFirmwareMessage::StartFieldUpgrade { .. } => (), - GrappleFirmwareMessage::UpdatePart(data) => { - ctx.shared.status_led.lock(|led| led.toggle()); - - let mut flash_writer = ctx.local.flash.writer(flash::SectorSize::Sz1K, flash::FlashSize::Sz64K); - let user_code_offset = FLASH_USER as usize - FLASH_START as usize; - - // Erase the sector if required. Obviously, this requires our data step size to be a factor of 1024. - if (user_code_offset + *ctx.local.offset) as u32 % 1024 == 0 { - // Erase the flash region - flash_writer.erase((user_code_offset + *ctx.local.offset) as u32, 1024).unwrap(); - } - - // Write the flash data (must be aligned to 16 bits) - flash_writer.write((user_code_offset + *ctx.local.offset) as u32, &data[..]).unwrap(); - - *ctx.local.offset += data.len() as usize; - - // Reply with an ACK - let new_msg = TaggedGrappleMessage::new( - my_id, - GrappleDeviceMessage::FirmwareUpdate( - GrappleFirmwareMessage::UpdatePartAck - ) - ); - ctx.shared.can_tx_queue.lock(|q| enqueue(new_msg, q)); - }, - GrappleFirmwareMessage::UpdateDone => { - let mut flash_writer = ctx.local.flash.writer(flash::SectorSize::Sz1K, flash::FlashSize::Sz64K); - ctx.shared.status_led.lock(|led| led.set_high()); + GrappleFirmwareMessage::UpdateDone => { + let mut flash_writer = ctx.local.flash.writer(flash::SectorSize::Sz1K, flash::FlashSize::Sz64K); + ctx.shared.status_led.lock(|led| led.set_high()); - // /* Mark the firmware as good to go */ - flash_writer.write(FLASH_META_FIRMWARE_RESET_LOC as u32 - FLASH_START, &[0xDE, 0xAD, 0xBE, 0xEF]).unwrap(); + // /* Mark the firmware as good to go */ + flash_writer.write(FLASH_META_FIRMWARE_RESET_LOC as u32 - FLASH_START, &[0xDE, 0xAD, 0xBE, 0xEF]).unwrap(); - cortex_m::peripheral::SCB::sys_reset(); + cortex_m::peripheral::SCB::sys_reset(); + }, + _ => () }, - _ => () - }, - _ => () - } + } + }).ok(); }, _ => () } @@ -327,11 +333,11 @@ mod app { }; } - fn enqueue(mut msg: TaggedGrappleMessage, q: &mut VecDeque) { + fn enqueue(reassembler: &FragmentReassemblerTx, msg: TaggedGrappleMessage, q: &mut VecDeque) { static FRAG_ID: core::sync::atomic::AtomicU8 = core::sync::atomic::AtomicU8::new(0); let frag_id = FRAG_ID.load(core::sync::atomic::Ordering::Relaxed); - FragmentReassembler::maybe_fragment(msg.device_id, msg.msg, frag_id, &mut |id, buf| { + reassembler.maybe_fragment(msg.device_id, msg.msg, frag_id, &mut |id, buf| { let frame = unsafe { bxcan::Frame::new_data( ExtendedId::new_unchecked(Into::::into(id.clone())), @@ -339,7 +345,7 @@ mod app { ) }; q.push_back(frame); - }); + }).ok(); rtic::pend(Interrupt::USB_HP_CAN_TX); FRAG_ID.store(frag_id.wrapping_add(1), core::sync::atomic::Ordering::Relaxed); } diff --git a/lasercan-firmware/Cargo.toml b/lasercan-firmware/Cargo.toml index 77c4959..9e26836 100644 --- a/lasercan-firmware/Cargo.toml +++ b/lasercan-firmware/Cargo.toml @@ -13,6 +13,7 @@ cortex-m-rt = "0.7.1" panic-halt = "0.2.0" stm32f1xx-hal = { version = "0.10.0", features = ["rt", "stm32f103", "medium"] } grapple-frc-msgs = { version = "~2024.0.5", default-features = false, features = ["lasercan_nop_patch", "grapple_lasercan"] } +grapple-lasercan = { version = "^2024.0.2", path = "../lasercan-impl" } bxcan = "0.7.0" embedded-alloc = "0.5.1" lasercan-common = { path = "../lasercan-common" } diff --git a/lasercan-firmware/src/configuration.rs b/lasercan-firmware/src/configuration.rs deleted file mode 100644 index ca3f304..0000000 --- a/lasercan-firmware/src/configuration.rs +++ /dev/null @@ -1,42 +0,0 @@ -use alloc::string::String; -use grapple_frc_msgs::binmarshal::BinMarshal; -use grapple_frc_msgs::binmarshal; -use alloc::borrow::ToOwned; -use grapple_frc_msgs::grapple::lasercan::{LaserCanRoi, LaserCanRoiU4}; - -#[derive(Clone, BinMarshal)] -#[marshal(magic = b"LCAN//0.1.2")] -pub struct LaserCanConfiguration { - #[marshal(bits = "7")] - pub device_id: u8, - #[marshal(bits = "1")] - pub long: bool, - pub roi: LaserCanRoi, - pub timing_budget: u8, - pub led_threshold: u16, - pub name: String -} - -impl Default for LaserCanConfiguration { - fn default() -> Self { - Self { - device_id: 0, - long: false, - roi: LaserCanRoi { x: LaserCanRoiU4(8), y: LaserCanRoiU4(8), w: LaserCanRoiU4(16), h: LaserCanRoiU4(16) }, - timing_budget: 33, - led_threshold: 1000, - name: "LaserCAN".to_owned() - } - } -} - -impl LaserCanConfiguration { - pub fn validate(&self) -> bool { - if self.device_id > 0x3F { return false } - if self.timing_budget <= 20 { return false } - if self.roi.w.0 > 16 || self.roi.h.0 > 16 { return false } - // TODO: Complex ROI calcs - - true - } -} \ No newline at end of file diff --git a/lasercan-impl/.gitignore b/lasercan-impl/.gitignore new file mode 100644 index 0000000..eb5a316 --- /dev/null +++ b/lasercan-impl/.gitignore @@ -0,0 +1 @@ +target diff --git a/lasercan-impl/Cargo.lock b/lasercan-impl/Cargo.lock new file mode 100644 index 0000000..9c30292 --- /dev/null +++ b/lasercan-impl/Cargo.lock @@ -0,0 +1,244 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 3 + +[[package]] +name = "anyhow" +version = "1.0.79" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "080e9890a082662b09c1ad45f567faeeb47f22b5fb23895fbe1e651e718e25ca" + +[[package]] +name = "binmarshal" +version = "0.2.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "56123c76502290a6e8a4d9124c3be15b463657a86270d9775e7d94fa652f5d05" +dependencies = [ + "binmarshal-macros 0.2.7", +] + +[[package]] +name = "binmarshal" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f9334da3cc3c1f73c4cb5849f784cfe380a12d604670f91f24cfd9bfa0b3b35" +dependencies = [ + "anyhow", + "binmarshal-macros 1.0.0", +] + +[[package]] +name = "binmarshal-macros" +version = "0.2.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a28cee2e730b6ddc4ea2afe66696682e79957e6f687f28a440d9124a532d4ddb" +dependencies = [ + "darling", + "proc-macro2", + "quote", + "syn 2.0.46", +] + +[[package]] +name = "binmarshal-macros" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a928be789849c207d60b342918373711573e1a18635e132297b35f4cf2b2c7d" +dependencies = [ + "darling", + "proc-macro2", + "quote", + "syn 2.0.46", +] + +[[package]] +name = "darling" +version = "0.20.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0209d94da627ab5605dcccf08bb18afa5009cfbef48d8a8b7d7bdbc79be25c5e" +dependencies = [ + "darling_core", + "darling_macro", +] + +[[package]] +name = "darling_core" +version = "0.20.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "177e3443818124b357d8e76f53be906d60937f0d3a90773a664fa63fa253e621" +dependencies = [ + "fnv", + "ident_case", + "proc-macro2", + "quote", + "strsim", + "syn 2.0.46", +] + +[[package]] +name = "darling_macro" +version = "0.20.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "836a9bbc7ad63342d6d6e7b815ccab164bc77a2d95d84bc3117a8c0d5c98e2d5" +dependencies = [ + "darling_core", + "quote", + "syn 2.0.46", +] + +[[package]] +name = "embedded-hal" +version = "0.2.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "35949884794ad573cf46071e41c9b60efb0cb311e3ca01f7af807af1debc66ff" +dependencies = [ + "nb 0.1.3", + "void", +] + +[[package]] +name = "fnv" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" + +[[package]] +name = "grapple-config" +version = "0.1.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5d74a07fdd4b60864ca11d1e1dbde0743de0a275ddf2413603d0ea1cd0303da8" +dependencies = [ + "binmarshal 0.2.9", + "embedded-hal", +] + +[[package]] +name = "grapple-frc-msgs" +version = "2024.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1c89fbb3e8d1a976316c3f70024a23bb2b13b626151b3449cb1bd26fede33e2" +dependencies = [ + "anyhow", + "binmarshal 1.0.2", + "smallvec", + "strum_macros", +] + +[[package]] +name = "grapple-lasercan" +version = "2024.0.2" +dependencies = [ + "grapple-config", + "grapple-frc-msgs", + "smallvec", +] + +[[package]] +name = "heck" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "95505c38b4572b2d910cecb0281560f54b440a19336cbbcb27bf6ce6adc6f5a8" + +[[package]] +name = "ident_case" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" + +[[package]] +name = "nb" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "801d31da0513b6ec5214e9bf433a77966320625a37860f910be265be6e18d06f" +dependencies = [ + "nb 1.1.0", +] + +[[package]] +name = "nb" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" + +[[package]] +name = "proc-macro2" +version = "1.0.74" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2de98502f212cfcea8d0bb305bd0f49d7ebdd75b64ba0a68f937d888f4e0d6db" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quote" +version = "1.0.35" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "291ec9ab5efd934aaf503a6466c5d5251535d108ee747472c3977cc5acc868ef" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "rustversion" +version = "1.0.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7ffc183a10b4478d04cbbbfc96d0873219d962dd5accaff2ffbd4ceb7df837f4" + +[[package]] +name = "smallvec" +version = "1.11.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4dccd0940a2dcdf68d092b8cbab7dc0ad8fa938bf95787e1b916b0e3d0e8e970" + +[[package]] +name = "strsim" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" + +[[package]] +name = "strum_macros" +version = "0.24.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e385be0d24f186b4ce2f9982191e7101bb737312ad61c1f2f984f34bcf85d59" +dependencies = [ + "heck", + "proc-macro2", + "quote", + "rustversion", + "syn 1.0.109", +] + +[[package]] +name = "syn" +version = "1.0.109" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "syn" +version = "2.0.46" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "89456b690ff72fddcecf231caedbe615c59480c93358a93dfae7fc29e3ebbf0e" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "unicode-ident" +version = "1.0.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" + +[[package]] +name = "void" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d" diff --git a/lasercan-impl/Cargo.toml b/lasercan-impl/Cargo.toml new file mode 100644 index 0000000..4efb6d7 --- /dev/null +++ b/lasercan-impl/Cargo.toml @@ -0,0 +1,9 @@ +[package] +name = "grapple-lasercan" +version = "2024.0.2" +edition = "2021" + +[dependencies] +grapple-frc-msgs = { version = "^2024.2.2", default-features = false, features = ["lasercan_nop_patch", "grapple_lasercan"] } +grapple-config = { version = "^0.1.1" } +smallvec = "1.11.2" diff --git a/lasercan-impl/src/lib.rs b/lasercan-impl/src/lib.rs new file mode 100644 index 0000000..933592c --- /dev/null +++ b/lasercan-impl/src/lib.rs @@ -0,0 +1,386 @@ +#![no_std] + +extern crate alloc; + +use alloc::{string::String, borrow::ToOwned}; +use grapple_config::{ConfigurationMarshal, ConfigurationProvider, GenericConfigurationProvider}; +use grapple_frc_msgs::{grapple::{lasercan::{LaserCanRoi, LaserCanRangingMode, LaserCanMeasurement, LaserCanRoiU4, LaserCanTimingBudget, self}, errors::{GrappleResult, GrappleError}, TaggedGrappleMessage, fragments::{FragmentReassemblerRx, FragmentReassemblerTx, FragmentReassembler}, GrappleDeviceMessage, firmware::GrappleFirmwareMessage, GrappleBroadcastMessage, device_info::{GrappleDeviceInfo, GrappleModelId}, Request, MANUFACTURER_GRAPPLE, DEVICE_TYPE_DISTANCE_SENSOR}, MessageId, ManufacturerMessage, binmarshal::{Marshal, Demarshal, CowStr, BitView}, DEVICE_ID_BROADCAST, DEVICE_TYPE_FIRMWARE_UPGRADE, DEVICE_TYPE_BROADCAST}; +use grapple_frc_msgs::binmarshal; + +// CONFIGURATION + +#[derive(Debug, Clone, Marshal, Demarshal)] +#[marshal(magic = b"LCAN//0.2.0")] +pub struct LaserCanConfiguration { + pub device_id: u8, + + #[marshal(align = "1")] + pub ranging_mode: LaserCanRangingMode, + + #[marshal(align = "1")] + pub roi: LaserCanRoi, + + #[marshal(align = "1")] + pub timing_budget: LaserCanTimingBudget, + + #[marshal(align = "1")] + pub led_threshold: u16, + + #[marshal(align = "1")] + pub name: String +} + +impl Default for LaserCanConfiguration { + fn default() -> Self { + Self { + device_id: 0, + ranging_mode: LaserCanRangingMode::Short, + roi: LaserCanRoi { x: LaserCanRoiU4(8), y: LaserCanRoiU4(8), w: LaserCanRoiU4(16), h: LaserCanRoiU4(16) }, + timing_budget: LaserCanTimingBudget::TB33ms, + led_threshold: 1000, + name: "LaserCAN".to_owned() + } + } +} + +impl LaserCanConfiguration { + pub fn validate(&self) -> bool { + if self.device_id > 0x3F { return false } + if self.roi.w.0 > 16 || self.roi.h.0 > 16 { return false } + true + } +} + +// TRAITS + +pub trait Sensor { + fn data_ready(&mut self) -> GrappleResult<'static, bool>; + fn get_result(&mut self) -> GrappleResult<'static, LaserCanMeasurement>; + fn stop_ranging(&mut self) -> GrappleResult<'static, ()>; + fn start_ranging(&mut self) -> GrappleResult<'static, ()>; + fn set_ranging_mode(&mut self, mode: LaserCanRangingMode) -> GrappleResult<'static, ()>; + fn set_roi(&mut self, roi: LaserCanRoi) -> GrappleResult<'static, ()>; + fn set_timing_budget_ms(&mut self, budget: LaserCanTimingBudget) -> GrappleResult<'static, ()>; +} + +pub trait Watchdog { + fn feed(&mut self); +} + +pub trait SysTick { + fn time(&self) -> u32; +} + +pub trait DropToBootloader { + fn drop_to_bootloader(&mut self); +} + +pub trait CanBus { + const MAX_MSG_SIZE: usize; + + fn subscribe(&mut self, id: MessageId, mask: MessageId); + fn enqueue_msg(&mut self, id: MessageId, buf: &[u8]); +} + +pub trait InputOutput { + fn set_led(&mut self, value: bool); // false = high + fn toggle_led(&mut self); +} + +// IMPL + +pub const LED_TICK_PERIOD_MS: usize = 50; +pub const LED_TICK_IRQ_PRIORITY: usize = 1; + +pub const STATUS_TICK_PERIOD_MS: usize = 20; +pub const STATUS_TICK_IRQ_PRIORITY: usize = 15; + +pub struct LaserCANImpl { + firmware_version: &'static str, + serial_number: u32, + + watchdog: WDG, + systick: SYST, + to_bootloader: DTB, + can: CAN, + sensor: SEN, + config: ConfigurationProvider, + io: IO, + + reassemble: (FragmentReassemblerRx, FragmentReassemblerTx), + led_counter: u32, + blink_counter: u32, + last_result: Option, +} + +impl< + WDG: Watchdog, SYST: SysTick, DTB: DropToBootloader, + CAN: CanBus, SEN: Sensor, MRSHL: ConfigurationMarshal, + IO: InputOutput +> LaserCANImpl { + pub fn new( + firmware_version: &'static str, + serial_number: u32, + watchdog: WDG, + systick: SYST, + to_bootloader: DTB, + can: CAN, + sensor: SEN, + marshal: MRSHL, + io: IO + ) -> GrappleResult<'static, Self> { + let provider: ConfigurationProvider = ConfigurationProvider::new(marshal) + .map_err(|_| GrappleError::Generic(CowStr::Borrowed("Configuration Provider Initialisation Failed")))?; + + let mut s = Self { + firmware_version, + serial_number, + + watchdog, systick, to_bootloader, can, + sensor, config: provider, io, + + reassemble: FragmentReassembler::new(1000, CAN::MAX_MSG_SIZE).split(), + led_counter: 0, blink_counter: 40, + last_result: None + }; + + s.init()?; + + Ok(s) + } + + fn init(&mut self) -> GrappleResult<'static, ()> { + // Try to apply the configuration, but get rid of it if the configuration is invalid. + let cfg = self.config.current().clone(); + match self.apply_configuration(&cfg) { + Ok(_) => (), + Err(_) => { + *self.config.current_mut() = LaserCanConfiguration::default(); + self.config.commit(); + let cfg = self.config.current().clone(); + self.apply_configuration(&cfg).ok(); + }, + }; + + // Firmware Update Messages + self.can.subscribe( + MessageId { device_type: DEVICE_TYPE_FIRMWARE_UPGRADE, manufacturer: MANUFACTURER_GRAPPLE, api_class: 0x00, api_index: 0x00, device_id: DEVICE_ID_BROADCAST }, + MessageId { device_type: 0xFF, manufacturer: 0xFF, api_class: 0x00, api_index: 0x00, device_id: 0xFF } + ); + + // Broadcast Messages + self.can.subscribe( + MessageId { device_type: DEVICE_TYPE_BROADCAST, manufacturer: MANUFACTURER_GRAPPLE, api_class: 0x00, api_index: 0x00, device_id: DEVICE_ID_BROADCAST }, + MessageId { device_type: 0xFF, manufacturer: 0xFF, api_class: 0x00, api_index: 0x00, device_id: 0xFF } + ); + + // Specific to this sensor. Note we don't check for Device ID here, since it may change whilst we're still booted. + self.can.subscribe( + MessageId { device_type: DEVICE_TYPE_DISTANCE_SENSOR, manufacturer: MANUFACTURER_GRAPPLE, api_class: 0x00, api_index: 0x00, device_id: 0x00 }, + MessageId { device_type: 0xFF, manufacturer: 0xFF, api_class: 0x00, api_index: 0x00, device_id: 0x00 } + ); + + Ok(()) + } + + // ============================ + // INTERRUPTS + // ============================ + + pub fn on_led_tick(&mut self) { + if self.blink_counter > 0 { + if self.led_counter & 2 == 0 { + self.io.toggle_led(); + } + self.blink_counter -= 1; + } else if let Some(lr) = &self.last_result { + let led_thresh = self.config.current().led_threshold; + if lr.status == 0 && lr.distance_mm < led_thresh && led_thresh > 20 { + let partial = ((lr.distance_mm as u32) * 20) / (led_thresh as u32) + 1; // Plus one so we don't modulo by zero and crash the MCU :) + if self.led_counter % partial == 0 { + self.io.toggle_led(); + } + } else { + self.io.set_led(false); + } + } else { + self.io.set_led(false); + } + + self.led_counter = self.led_counter.wrapping_add(1); + } + + // TODO: Move this to be interrupt driven, by TOF_INT + pub fn on_status_tick(&mut self) { + self.watchdog.feed(); + + if Ok(true) == self.sensor.data_ready() { + if let Ok(r) = self.sensor.get_result() { + self.enqueue_can_msg(TaggedGrappleMessage::new( + self.config.current().device_id, + GrappleDeviceMessage::DistanceSensor( + lasercan::LaserCanMessage::Measurement(lasercan::LaserCanMeasurement { + status: r.status, + distance_mm: r.distance_mm, + ambient: r.ambient, + mode: self.config.current().ranging_mode.clone(), + budget: self.config.current().timing_budget.clone(), + roi: self.config.current().roi.clone() + }) + ) + )).ok(); + + self.last_result = Some(r); + } + } + } + + pub fn on_can_message(&mut self, id: MessageId, buf: &[u8]) -> /* was something processed? */ bool { + if id.device_id != DEVICE_ID_BROADCAST && id.device_id != self.config.current().device_id { + return false; + } + + match ManufacturerMessage::read(&mut BitView::new(buf), id.clone()) { + Ok(ManufacturerMessage::Grapple(grpl_msg)) => { + let mut storage: smallvec::SmallVec<[u8; 64]> = smallvec::SmallVec::new(); + + match self.reassemble.0.defragment(self.systick.time() as i64, &id, grpl_msg, &mut storage) { + Ok(Some(msg)) => match msg { + GrappleDeviceMessage::Broadcast(bcast) => match bcast { + GrappleBroadcastMessage::DeviceInfo(di) => match di { + GrappleDeviceInfo::EnumerateRequest => { + let name = self.config.current().name.clone(); + + let new_msg = TaggedGrappleMessage::new( + self.config.current().device_id, + GrappleDeviceMessage::Broadcast( + GrappleBroadcastMessage::DeviceInfo(GrappleDeviceInfo::EnumerateResponse { + model_id: GrappleModelId::LaserCan, + serial: self.serial_number, + is_dfu: false, + is_dfu_in_progress: false, + version: self.firmware_version, + name: &name + }) + ) + ); + + self.enqueue_can_msg(new_msg).ok(); + }, + GrappleDeviceInfo::Blink { serial } if serial == self.serial_number => { + self.blink_counter = 60; + }, + GrappleDeviceInfo::SetName { serial, name } if serial == self.serial_number => { + self.config.current_mut().name = name.to_owned(); + self.config.commit(); + }, + GrappleDeviceInfo::SetId { serial, new_id } if serial == self.serial_number => { + self.config.current_mut().device_id = new_id; + self.config.commit(); + }, + GrappleDeviceInfo::CommitConfig { serial } if serial == self.serial_number => { + self.config.commit(); + }, + _ => () + } + }, + GrappleDeviceMessage::DistanceSensor(dist) => match dist { + lasercan::LaserCanMessage::SetRange(Request::Request(range)) => { + let response = self.try_configuration_change(|cfg| cfg.ranging_mode = range); + + self.enqueue_can_msg(TaggedGrappleMessage::new( + self.config.current().device_id, + GrappleDeviceMessage::DistanceSensor(lasercan::LaserCanMessage::SetRange( + Request::Ack(response) + )) + )).ok(); + }, + lasercan::LaserCanMessage::SetRoi(Request::Request(roi)) => { + let response = self.try_configuration_change(|cfg| cfg.roi = roi); + + self.enqueue_can_msg(TaggedGrappleMessage::new( + self.config.current().device_id, + GrappleDeviceMessage::DistanceSensor(lasercan::LaserCanMessage::SetRoi( + Request::Ack(response) + )) + )).ok(); + }, + lasercan::LaserCanMessage::SetTimingBudget(Request::Request(budget)) => { + let response = self.try_configuration_change(|cfg| cfg.timing_budget = budget); + + self.enqueue_can_msg(TaggedGrappleMessage::new( + self.config.current().device_id, + GrappleDeviceMessage::DistanceSensor(lasercan::LaserCanMessage::SetTimingBudget( + Request::Ack(response) + )) + )).ok(); + }, + lasercan::LaserCanMessage::SetLedThreshold(Request::Request(thresh)) => { + self.config.current_mut().led_threshold = thresh; + self.config.commit(); + + self.enqueue_can_msg(TaggedGrappleMessage::new( + self.config.current().device_id, + GrappleDeviceMessage::DistanceSensor(lasercan::LaserCanMessage::SetLedThreshold( + Request::Ack(Ok(())) + )) + )).ok(); + }, + _ => () + }, + GrappleDeviceMessage::FirmwareUpdate(GrappleFirmwareMessage::StartFieldUpgrade { serial }) if serial == self.serial_number => { + self.to_bootloader.drop_to_bootloader(); + }, + _ => () + }, + _ => () + } + }, + Err(_) => ( /* Ignore any malformed messages */ ), + } + + true + } + + // ============================ + // INTERNAL + // ============================ + + fn try_configuration_change(&mut self, f: F) -> GrappleResult<'static, ()> { + let mut candidate = self.config.current().clone(); + f(&mut candidate); + + match self.apply_configuration(&candidate) { + Ok(()) => { + *self.config.current_mut() = candidate; + self.config.commit(); + Ok(()) + }, + Err(e) => { + let cfg = self.config.current().clone(); + self.apply_configuration(&cfg).ok(); + Err(e) + } + } + } + + fn apply_configuration(&mut self, cfg: &LaserCanConfiguration) -> GrappleResult<'static, ()> { + if !cfg.validate() { + Err(GrappleError::FailedAssertion(CowStr::Borrowed("Invalid Configuration!")))?; + } + + self.sensor.stop_ranging()?; + self.sensor.set_ranging_mode(cfg.ranging_mode.clone())?; + self.sensor.set_roi(cfg.roi.clone())?; + self.sensor.set_timing_budget_ms(cfg.timing_budget.clone())?; + self.sensor.start_ranging()?; + Ok(()) + } + + fn enqueue_can_msg(&mut self, msg: TaggedGrappleMessage) -> GrappleResult<'static, ()> { + self.reassemble.1.maybe_fragment(msg.device_id, msg.msg, &mut |id, buf| { + self.can.enqueue_msg(id, buf) + }).map_err(|_| GrappleError::Generic(CowStr::Borrowed("Failed to encode message")))?; + Ok(()) + } +}