diff --git a/libs/cramium-hal/src/udma/mod.rs b/libs/cramium-hal/src/udma/mod.rs index f43a8e96d..0e92bc727 100644 --- a/libs/cramium-hal/src/udma/mod.rs +++ b/libs/cramium-hal/src/udma/mod.rs @@ -415,6 +415,10 @@ pub trait Udma { } fn udma_busy(&self, bank: Bank) -> bool { // Safety: only safe when used in the context of UDMA registers. - unsafe { self.csr().base().add(bank as usize).add(DmaReg::Saddr.into()).read_volatile() != 0 } + unsafe { + let saddr = self.csr().base().add(bank as usize).add(DmaReg::Saddr.into()).read_volatile(); + // crate::println!("brx: {:x}", saddr); + saddr != 0 + } } } diff --git a/libs/cramium-hal/src/udma/spim.rs b/libs/cramium-hal/src/udma/spim.rs index 5478b8e40..0f480ac1b 100644 --- a/libs/cramium-hal/src/udma/spim.rs +++ b/libs/cramium-hal/src/udma/spim.rs @@ -608,17 +608,44 @@ impl Spim { /// Wait for the pending tx/rx cycle to finish, returns a pointer to the Rx buffer when done. pub fn txrx_await(&mut self, _use_yield: bool) -> Result<&[u8], xous::Error> { + #[cfg(not(target_os = "xous"))] if let Some(pending) = self.pending_txrx.take() { - while self.udma_busy(Bank::Tx) || self.udma_busy(Bank::Rx) || self.udma_busy(Bank::Custom) { - #[cfg(feature = "std")] - if _use_yield { - xous::yield_slice(); - } - } + while self.udma_busy(Bank::Tx) || self.udma_busy(Bank::Rx) || self.udma_busy(Bank::Custom) {} Ok(&self.rx_buf()[..pending]) } else { Err(xous::Error::UseBeforeInit) } + #[cfg(target_os = "xous")] + { + if let Some(pending) = self.pending_txrx.take() { + let tt = xous_api_ticktimer::Ticktimer::new().unwrap(); + let start = tt.elapsed_ms(); + let mut now = tt.elapsed_ms(); + const TIMEOUT_MS: u64 = 500; + while (self.udma_busy(Bank::Tx) || self.udma_busy(Bank::Rx) || self.udma_busy(Bank::Custom)) + && ((now - start) < TIMEOUT_MS) + { + now = tt.elapsed_ms(); + } + if now - start >= TIMEOUT_MS { + log::warn!( + "Timeout in txrx_await(): Tx {:?} Rx {:?} Custom {:?}, Rx SA: {:x}, Rx Cfg: {:x}", + self.udma_busy(Bank::Tx), + self.udma_busy(Bank::Rx), + self.udma_busy(Bank::Custom), + unsafe { + self.csr().base().add(Bank::Rx as usize).add(DmaReg::Saddr.into()).read_volatile() + }, + unsafe { + self.csr().base().add(Bank::Rx as usize).add(DmaReg::Cfg.into()).read_volatile() + }, + ); + } + Ok(&self.rx_buf()[..pending]) + } else { + Err(xous::Error::UseBeforeInit) + } + } } /// `txrx_data_async` will return as soon as all the pending operations are queued. This will error @@ -810,9 +837,9 @@ impl Spim { // read back the ID result let cmd_list = [SpimCmd::RxData(self.mode, SpimWordsPerXfer::Words1, 8, SpimEndian::MsbFirst, 3)]; - self.send_cmd_list(&cmd_list); // safety: this is safe because rx_buf_phys() slice is only used as a base/bounds reference unsafe { self.udma_enqueue(Bank::Rx, &self.rx_buf_phys::()[..3], CFG_EN | CFG_SIZE_8) }; + self.send_cmd_list(&cmd_list); while self.udma_busy(Bank::Rx) { #[cfg(feature = "std")] xous::yield_slice(); @@ -835,9 +862,9 @@ impl Spim { // The ID requires 24 bits "dummy" address field, then followed by 2 bytes ID + KGD, and then // 48 bits of unique ID -- we only retrieve the top 16 of that here. let cmd_list = [SpimCmd::RxData(self.mode, SpimWordsPerXfer::Words1, 8, SpimEndian::MsbFirst, 7)]; - self.send_cmd_list(&cmd_list); // safety: this is safe because rx_buf_phys() slice is only used as a base/bounds reference unsafe { self.udma_enqueue(Bank::Rx, &self.rx_buf_phys::()[..7], CFG_EN | CFG_SIZE_8) }; + self.send_cmd_list(&cmd_list); while self.udma_busy(Bank::Rx) { #[cfg(feature = "std")] xous::yield_slice(); @@ -881,10 +908,10 @@ impl Spim { // setup the command list for data to send let cmd_list = [SpimCmd::TxData(self.mode, SpimWordsPerXfer::Words1, 8 as u8, SpimEndian::MsbFirst, 2 as u32)]; - self.send_cmd_list(&cmd_list); self.tx_buf_mut()[..2].copy_from_slice(&[status, config]); // safety: this is safe because tx_buf_phys() slice is only used as a base/bounds reference unsafe { self.udma_enqueue(Bank::Tx, &self.tx_buf_phys::()[..2], CFG_EN | CFG_SIZE_8) } + self.send_cmd_list(&cmd_list); while self.udma_busy(Bank::Tx) { #[cfg(feature = "std")] @@ -919,7 +946,6 @@ impl Spim { // the remaining bytes are junk self.tx_buf_mut()[3..6].copy_from_slice(&[0xFFu8, 0xFFu8, 0xFFu8]); self.mem_cs(true); - self.send_cmd_list(&cmd_list); // safety: this is safe because tx_buf_phys() slice is only used as a base/bounds reference unsafe { self.udma_enqueue( @@ -928,6 +954,7 @@ impl Spim { CFG_EN | CFG_SIZE_8, ) } + self.send_cmd_list(&cmd_list); let rd_cmd = [SpimCmd::RxData( self.mode, SpimWordsPerXfer::Words1, @@ -941,11 +968,11 @@ impl Spim { xous::yield_slice(); } } - self.send_cmd_list(&rd_cmd); // safety: this is safe because rx_buf_phys() slice is only used as a base/bounds reference unsafe { self.udma_enqueue(Bank::Rx, &self.rx_buf_phys::()[..chunk.len()], CFG_EN | CFG_SIZE_8) }; + self.send_cmd_list(&rd_cmd); while self.udma_busy(Bank::Rx) { // TODO: figure out why this timeout detection code is necessary. // It seems that some traffic during the UDMA access can cause the UDMA @@ -1012,9 +1039,9 @@ impl Spim { let a = chunk_addr.to_be_bytes(); self.tx_buf_mut()[..3].copy_from_slice(&[a[1], a[2], a[3]]); self.mem_cs(true); - self.send_cmd_list(&cmd_list); // safety: this is safe because tx_buf_phys() slice is only used as a base/bounds reference unsafe { self.udma_enqueue(Bank::Tx, &self.tx_buf_phys::()[..3], CFG_EN | CFG_SIZE_8) } + self.send_cmd_list(&cmd_list); let wr_cmd = [SpimCmd::TxData( self.mode, SpimWordsPerXfer::Words1, @@ -1028,12 +1055,12 @@ impl Spim { xous::yield_slice(); } } - self.send_cmd_list(&wr_cmd); self.tx_buf_mut()[..chunk.len()].copy_from_slice(chunk); // safety: this is safe because tx_buf_phys() slice is only used as a base/bounds reference unsafe { self.udma_enqueue(Bank::Tx, &self.tx_buf_phys::()[..chunk.len()], CFG_EN | CFG_SIZE_8) }; + self.send_cmd_list(&wr_cmd); while self.udma_busy(Bank::Tx) { #[cfg(feature = "std")] if _use_yield { diff --git a/libs/xous-pl230/src/pl230_tests/mod.rs b/libs/xous-pl230/src/pl230_tests/mod.rs index 47e6dfe0e..759bbf83b 100644 --- a/libs/xous-pl230/src/pl230_tests/mod.rs +++ b/libs/xous-pl230/src/pl230_tests/mod.rs @@ -5,7 +5,7 @@ pub fn report_api(desc: &str, d: u32) { { use core::fmt::Write; let mut uart = crate::debug::Uart {}; - writeln!(uart, "pl230: [{}] 0x{:x}", desc, d).ok(); + writeln!(uart, "pl230: [{}] 0x{:x}\r", desc, d).ok(); } #[cfg(not(feature = "baremetal"))] log::info!("pl230: [{}] 0x{:x}", desc, d); diff --git a/libs/xous-pl230/src/pl230_tests/units.rs b/libs/xous-pl230/src/pl230_tests/units.rs index 2ab798d69..342949cbd 100644 --- a/libs/xous-pl230/src/pl230_tests/units.rs +++ b/libs/xous-pl230/src/pl230_tests/units.rs @@ -69,12 +69,18 @@ pub fn basic_tests(pl230: &mut Pl230) -> bool { // report_api("dma_len", DMA_LEN as u32); report_api("baseptr", cc_struct.channels.as_ptr() as u32); report_api("src start", region_a.as_ptr() as u32); - // report_api("baseptr[0]", unsafe{cc_struct.channels.as_ptr().read()}.src_end_ptr); + report_api("baseptr[0]", unsafe { cc_struct.channels.as_ptr().read() }.src_end_ptr); report_api("dst start", region_b.as_ptr() as u32); - // report_api("baseptr[1]", unsafe{cc_struct.channels.as_ptr().read()}.dst_end_ptr); - // report_api("baseptr[2]", unsafe{cc_struct.channels.as_ptr().read()}.control); - // report_api("baseptr[3]", unsafe{cc_struct.channels.as_ptr().read()}.reserved); - // report_api("baseptr reg", pl230.csr.r(utra::pl230::CTRLBASEPTR)); + report_api("baseptr[1]", unsafe { cc_struct.channels.as_ptr().read() }.dst_end_ptr); + report_api("baseptr[2]", unsafe { cc_struct.channels.as_ptr().read() }.control); + report_api("baseptr[3]", unsafe { cc_struct.channels.as_ptr().read() }.reserved); + report_api("baseptr reg", pl230.csr.r(utra::pl230::CTRLBASEPTR)); + + unsafe { + for i in 0..16 { + report_api("pl230 reg ", pl230.csr.base().add(i as usize).read_volatile()); + } + } // this should kick off the DMA pl230.csr.wo(utra::pl230::CHNLSWREQUEST, 1); @@ -123,6 +129,22 @@ pub fn basic_tests(pl230: &mut Pl230) -> bool { } report_api("basic dma result (1=pass)", if passing { 1 } else { 0 }); report_api("errs: ", errs); + + report_api("baseptr", cc_struct.channels.as_ptr() as u32); + report_api("src start", region_a.as_ptr() as u32); + report_api("baseptr[0]", unsafe { cc_struct.channels.as_ptr().read() }.src_end_ptr); + report_api("dst start", region_b.as_ptr() as u32); + report_api("baseptr[1]", unsafe { cc_struct.channels.as_ptr().read() }.dst_end_ptr); + report_api("baseptr[2]", unsafe { cc_struct.channels.as_ptr().read() }.control); + report_api("baseptr[3]", unsafe { cc_struct.channels.as_ptr().read() }.reserved); + report_api("baseptr reg", pl230.csr.r(utra::pl230::CTRLBASEPTR)); + + unsafe { + for i in 0..16 { + report_api("pl230 reg ", pl230.csr.base().add(i as usize).read_volatile()); + } + } + passing } diff --git a/loader/Cargo.toml b/loader/Cargo.toml index 7da26eb81..ff529c4be 100644 --- a/loader/Cargo.toml +++ b/loader/Cargo.toml @@ -23,6 +23,7 @@ xous-pl230 = { path = "../libs/xous-pl230", optional = true, features = [ "pio", "cramium-soc", "baremetal", + # "dma-mainram", # PL-230 option - this fails on MPW ] } xous-pio = { path = "../libs/xous-pio", optional = true, features = [ "tests", diff --git a/loader/src/platform/cramium/cramium.rs b/loader/src/platform/cramium/cramium.rs index 305372ce3..6fb4ba564 100644 --- a/loader/src/platform/cramium/cramium.rs +++ b/loader/src/platform/cramium/cramium.rs @@ -246,6 +246,11 @@ pub fn early_init() -> u32 { clock_tests(&mut udma_uart); } + // do a PL230/PIO test. Toggles PB15 (PIO0) with an LFSR sequence. + // let mut pl230 = xous_pl230::Pl230::new(); + // xous_pl230::pl230_tests::units::basic_tests(&mut pl230); + // xous_pl230::pl230_tests::units::pio_test(&mut pl230); + // Setup some global control registers that will allow the TRNG to operate once the kernel is // booted. This is done so the kernel doesn't have to exclusively have rights to the SCE global // registers just for this purpose. @@ -692,11 +697,6 @@ pub fn early_init() -> u32 { let trng_csr = CSR::new(HW_TRNG_BASE as *mut u32); crate::println!("trng status: {:x}", trng_csr.r(utra::trng::SFR_SR)); - // do a PL230/PIO test. Toggles PB15 (PIO0) with an LFSR sequence. - // let mut pl230 = xous_pl230::Pl230::new(); - // xous_pl230::pl230_tests::units::basic_tests(&mut pl230); - // xous_pl230::pl230_tests::units::pio_test(&mut pl230); - const BANNER: &'static str = "\n\rKeep pressing keys to continue boot...\r\n"; udma_uart.write(BANNER.as_bytes()); diff --git a/services/bao-video/src/homography.rs b/services/bao-video/src/homography.rs index a1c8cb7eb..80539f51a 100644 --- a/services/bao-video/src/homography.rs +++ b/services/bao-video/src/homography.rs @@ -52,7 +52,7 @@ pub fn find_homography(src_points: [(f32, f32); 4], dst_points: [(f32, f32); 4]) } } -const HOM_FP_SHIFT: i32 = 16; +const HOM_FP_SHIFT: i32 = 18; const HOM_FP_SCALE: i32 = 1 << HOM_FP_SHIFT; pub fn matrix3_to_fixp(h: Matrix3) -> Matrix3 { @@ -78,8 +78,22 @@ pub fn apply_fixp_homography(homography: &Matrix3, point: (i32, i32)) -> (i let tw = homography[(2, 0)] * x + homography[(2, 1)] * y + homography[(2, 2)]; // Normalize by w to get the final coordinates - let x_transformed = tx / tw; - let y_transformed = ty / tw; + if tw != 0 { + let x_transformed = tx / tw; + let y_transformed = ty / tw; - (x_transformed, y_transformed) + (x_transformed, y_transformed) + } else { + log::info!( + "Homography transformation would divide by 0: possible fixed-point underflow!\n\r {}, {}\n\r {} * {} + {} * {} + {}", + tx, + ty, + homography[(2, 0)], + x, + homography[(2, 1)], + y, + homography[(2, 2)] + ); + (0, 0) + } } diff --git a/services/bao-video/src/main.rs b/services/bao-video/src/main.rs index dda5d2d91..53689008f 100644 --- a/services/bao-video/src/main.rs +++ b/services/bao-video/src/main.rs @@ -1,20 +1,19 @@ -use cramium_hal::iox::IoGpio; use cramium_hal::iox::{IoxPort, IoxValue}; +use cramium_hal::minigfx::*; use cramium_hal::sh1107::{Mono, Oled128x128}; -use cramium_hal::udma::PeriphId; -use cramium_hal::{minigfx::*, sh1107}; -use nalgebra::QR; +use cramium_hal::udma::{PeriphId, Udma}; mod gfx; mod homography; +mod modules; mod qr; -const QR_WIDTH: usize = 256; -const QR_HEIGHT: usize = 240; +const IMAGE_WIDTH: usize = 256; +const IMAGE_HEIGHT: usize = 240; const BW_THRESH: u8 = 128; pub fn blit_to_display(sh1107: &mut Oled128x128, frame: &[u8], display_cleared: bool) { - for (y, row) in frame.chunks(QR_WIDTH).enumerate() { + for (y, row) in frame.chunks(IMAGE_WIDTH).enumerate() { if y & 1 == 0 { for (x, &pixval) in row.iter().enumerate() { if x & 1 == 0 { @@ -23,21 +22,13 @@ pub fn blit_to_display(sh1107: &mut Oled128x128, frame: &[u8], display_cleared: { let luminance = pixval & 0xff; if luminance > BW_THRESH { - // flip on y to adjust for sensor orientation. Lower left is (0, 0) - // on the display. - sh1107.put_pixel( - Point::new(x as isize / 2, (sh1107.dimensions().y - 1) - (y as isize / 2)), - Mono::White.into(), - ); + sh1107.put_pixel(Point::new(x as isize / 2, y as isize / 2), Mono::White.into()); } else { // optimization to avoid some computation if we're blitting to an already-black // buffer if !display_cleared { sh1107.put_pixel( - Point::new( - x as isize / 2, - (sh1107.dimensions().y - 1) - (y as isize / 2), - ), + Point::new(x as isize / 2, y as isize / 2), Mono::Black.into(), ); } @@ -61,7 +52,6 @@ fn wrapped_main() -> ! { log::set_max_level(log::LevelFilter::Info); let tt = ticktimer::Ticktimer::new().unwrap(); - let xns = xous_names::XousNames::new().unwrap(); let iox = cram_hal_service::IoxHal::new(); let udma_global = cram_hal_service::UdmaGlobal::new(); @@ -90,57 +80,40 @@ fn wrapped_main() -> ! { tt.sleep_ms(1).ok(); let (cols, _rows) = cam.resolution(); - let border = (cols - QR_WIDTH) / 2; - cam.set_slicing((border, 0), (cols - border, QR_HEIGHT)); + let border = (cols - IMAGE_WIDTH) / 2; + cam.set_slicing((border, 0), (cols - border, IMAGE_HEIGHT)); log::info!("320x240 resolution setup with 256x240 slicing"); #[cfg(feature = "decongest-udma")] log::info!("Decongest udma option enabled."); let mut frames = 0; - let mut frame = [0u8; QR_WIDTH * QR_HEIGHT]; + let mut frame = [0u8; IMAGE_WIDTH * IMAGE_HEIGHT]; + let mut decode_success; // while iox.get_gpio_pin_value(IoxPort::PB, 9) == IoxValue::High {} loop { #[cfg(not(feature = "decongest-udma"))] cam.capture_async(); let mut candidates = Vec::::new(); - log::info!("------------- SEARCH -----------"); - let finder_width = qr::find_finders(&mut candidates, &frame, BW_THRESH, QR_WIDTH) as isize; - const CROSSHAIR_LEN: isize = 3; + decode_success = false; + log::info!("------------- SEARCH {} -----------", frames); + let finder_width = qr::find_finders(&mut candidates, &frame, BW_THRESH, IMAGE_WIDTH) as isize; if candidates.len() == 3 { - gfx::msg(&mut sh1107, "Aligning...", Point::new(0, 0), Mono::White.into(), Mono::Black.into()); - for c in candidates.iter() { - log::info!("****** candidate: {}, {} ******", c.x, c.y); - // remap image to screen coordinates (it's 2:1) - let mut c_screen = *c / 2; - // flip coordinates to match the camera data - c_screen = Point::new(c_screen.x, sh1107.dimensions().y - 1 - c_screen.y); - qr::draw_crosshair(&mut sh1107, c_screen); - } - + let candidates_orig = candidates.clone(); + let mut x_candidates: [Point; 3] = [Point::new(0, 0); 3]; + // apply homography to generate a new buffer for processing + let mut aligned = Vec::new(); + let mut qr_pixels: Option = None; if let Some(mut qr_corners) = qr::QrCorners::from_finders( &candidates.try_into().unwrap(), - Point::new(QR_WIDTH as isize, QR_HEIGHT as isize), + Point::new(IMAGE_WIDTH as isize, IMAGE_HEIGHT as isize), // add a search margin on the finder width (finder_width + (qr::FINDER_SEARCH_MARGIN * finder_width) / (1 + 1 + 3 + 1 + 1)) as usize, ) { - let dims = Point::new(QR_WIDTH as isize, QR_HEIGHT as isize); + let dims = Point::new(IMAGE_WIDTH as isize, IMAGE_HEIGHT as isize); let mut il = qr::ImageRoi::new(&mut frame, dims, BW_THRESH); let (src, dst) = qr_corners.mapping(&mut il, qr::HOMOGRAPHY_MARGIN); - for s in src.iter() { - if let Some(p) = s { - log::info!("src {:?}", p); - qr::draw_crosshair(&mut sh1107, *p / 2); - } - } - for d in dst.iter() { - if let Some(p) = d { - log::info!("dst {:?}", p); - qr::draw_crosshair(&mut sh1107, *p / 2); - } - } - let mut src_f: [(f32, f32); 4] = [(0.0, 0.0); 4]; let mut dst_f: [(f32, f32); 4] = [(0.0, 0.0); 4]; let mut all_found = true; @@ -158,6 +131,7 @@ fn wrapped_main() -> ! { all_found = false; } } + if all_found { if let Some(h) = homography::find_homography(src_f, dst_f) { if let Some(h_inv) = h.try_inverse() { @@ -165,11 +139,10 @@ fn wrapped_main() -> ! { let h_inv_fp = homography::matrix3_to_fixp(h_inv); log::info!("{:?}", h_inv_fp); - // apply homography to generate a new buffer for processing - let mut aligned = [0u8; QR_WIDTH * QR_HEIGHT]; + aligned = vec![0u8; qr_corners.qr_pixels() * qr_corners.qr_pixels()]; // iterate through pixels and apply homography - for y in 0..dims.y { - for x in 0..dims.x { + for y in 0..qr_corners.qr_pixels() { + for x in 0..qr_corners.qr_pixels() { let (x_src, y_src) = homography::apply_fixp_homography(&h_inv_fp, (x as i32, y as i32)); if (x_src as i32 >= 0) @@ -178,75 +151,226 @@ fn wrapped_main() -> ! { && ((y_src as i32) < dims.y as i32) { // println!("{},{} -> {},{}", x_src as i32, y_src as i32, x, y); - aligned[QR_WIDTH * y as usize + x as usize] = - frame[QR_WIDTH * y_src as usize + x_src as usize]; + aligned[qr_corners.qr_pixels() * y as usize + x as usize] = + frame[IMAGE_WIDTH * y_src as usize + x_src as usize]; } else { - aligned[QR_WIDTH * y as usize + x as usize] = 255; + aligned[qr_corners.qr_pixels() * y as usize + x as usize] = 255; } } } - blit_to_display(&mut sh1107, &aligned, true); - let mut search_img = - rqrr::PreparedImage::prepare_from_greyscale(QR_WIDTH, QR_HEIGHT, |x, y| { - aligned[y * QR_WIDTH + x] - }); - let grids = search_img.detect_grids(); - log::info!("grids len {}", grids.len()); - if grids.len() > 0 { - let rawdata = grids[0].get_raw_data(); - match rawdata { - Ok((md, rd)) => { - log::info!( - "{:?}, {}:{:x?}", - md, - rd.len, - &rd.data[..(rd.len / 8) + 1] - ); - } - Err(e) => { - log::info!("Error: {:?}", e); - } - } - log::info!("{:?}", grids[0].decode()); + // we can also know the location of the finders by transforming them + let h_fp = homography::matrix3_to_fixp(h); + for (i, &c) in candidates_orig.iter().enumerate() { + let (x, y) = + homography::apply_fixp_homography(&h_fp, (c.x as i32, c.y as i32)); + x_candidates[i] = Point::new(x as isize, y as isize); } + qr_pixels = Some(qr_corners.qr_pixels()); } } } } + + if let Some(qr_width) = qr_pixels { + // show the transformed/aligned frame + frame.fill(255); + for (dst_line, src_line) in frame.chunks_mut(IMAGE_WIDTH).zip(aligned.chunks(qr_width)) { + // "center up" the QR in the middle by the estimated margin + // this is just a perceptual trick to prevent users from shifting the position of the + // camera + for (dst, &src) in + dst_line[qr::HOMOGRAPHY_MARGIN.abs() as usize..].iter_mut().zip(src_line.iter()) + { + *dst = src; + } + } + blit_to_display(&mut sh1107, &frame, true); + + // we now have a QR code in "canonical" orientation, with a + // known width in pixels + for &x in x_candidates.iter() { + log::info!("transformed finder location {:?}", x); + } + + // Confirm that the finders coordinates are valid + let mut checked_candidates = Vec::::new(); + let x_finder_width = + qr::find_finders(&mut checked_candidates, &aligned, BW_THRESH, qr_width as _) as isize; + log::info!("x_finder width: {}", x_finder_width); + + // check that the new coordinates are within delta pixels of the original + const XFORM_DELTA: isize = 2; + let mut deltas = Vec::::new(); + for c in checked_candidates { + log::info!("x_point: {:?}", c); + for &xformed in x_candidates.iter() { + let delta = xformed - c; + log::info!("delta: {:?}", delta); + if delta.x.abs() <= XFORM_DELTA && delta.y.abs() <= XFORM_DELTA { + deltas.push(delta); + } + } + } + if deltas.len() == 3 { + let (version, modules) = qr::guess_code_version( + x_finder_width as usize, + (qr_width as isize + qr::HOMOGRAPHY_MARGIN * 2) as usize, + ); + + log::info!("image dims: {}", qr_width); + log::info!("guessed version: {}, modules: {}", version, modules); + log::info!( + "QR symbol width in pixels: {}", + qr_width - 2 * (qr::HOMOGRAPHY_MARGIN.abs() as usize) + ); + + let qr = + qr::ImageRoi::new(&mut aligned, Point::new(qr_width as _, qr_width as _), BW_THRESH); + let grid = + modules::stream_to_grid(&qr, qr_width, modules, qr::HOMOGRAPHY_MARGIN.abs() as usize); + + println!("grid len {}", grid.len()); + for y in 0..modules { + for x in 0..modules { + if grid[y * modules + x] { + print!("X"); + } else { + print!(" "); + } + } + println!(" {:2}", y); + } + let simple = + rqrr::SimpleGrid::from_func(modules, |x, y| grid[(modules - 1) - x + y * modules]); + let grid = rqrr::Grid::new(simple); + match grid.decode() { + Ok((meta, content)) => { + log::info!("meta: {:?}", meta); + log::info!("************ {} ***********", content); + decode_success = true; + gfx::msg( + &mut sh1107, + &format!("{:?}", meta), + Point::new(0, 0), + Mono::White.into(), + Mono::Black.into(), + ); + gfx::msg( + &mut sh1107, + &format!("{:?}", content), + Point::new(0, 64), + Mono::White.into(), + Mono::Black.into(), + ); + } + Err(e) => { + log::info!("{:?}", e); + gfx::msg( + &mut sh1107, + &format!("{:?}", e), + Point::new(0, 0), + Mono::White.into(), + Mono::Black.into(), + ); + } + } + } else { + log::info!("Transformed image did not survive sanity check!"); + gfx::msg( + &mut sh1107, + "Hold device steady...", + Point::new(0, 0), + Mono::White.into(), + Mono::Black.into(), + ); + } + } else { + blit_to_display(&mut sh1107, &frame, true); + for c in candidates_orig.iter() { + log::debug!("****** candidate: {}, {} ******", c.x, c.y); + // remap image to screen coordinates (it's 2:1) + let c_screen = *c / 2; + // flip coordinates to match the camera data + // c_screen = Point::new(c_screen.x, sh1107.dimensions().y - 1 - c_screen.y); + qr::draw_crosshair(&mut sh1107, c_screen); + } + gfx::msg( + &mut sh1107, + "Align the QR code...", + Point::new(0, 0), + Mono::White.into(), + Mono::Black.into(), + ); + } } else { - // blit fb to sh1107 + // blit raw camera fb to sh1107 blit_to_display(&mut sh1107, &frame, true); - gfx::msg(&mut sh1107, "Searching...", Point::new(0, 0), Mono::White.into(), Mono::Black.into()); + gfx::msg( + &mut sh1107, + "Scan QR code...", + Point::new(0, 0), + Mono::White.into(), + Mono::Black.into(), + ); } // swap the double buffer and update to the display sh1107.buffer_swap(); sh1107.draw(); + if decode_success { + tt.sleep_ms(2000).ok(); + } // clear the front buffer sh1107.clear(); + let mut start = tt.elapsed_ms(); + let mut now = tt.elapsed_ms(); + const TIMEOUT_MS: u64 = 100; #[cfg(feature = "decongest-udma")] { // don't parallelize the camera capture to avoid triggering a hardware bug // in the SPIM block. - while iox.get_gpio_pin_value(IoxPort::PB, 9) == IoxValue::High {} + while iox.get_gpio_pin_value(IoxPort::PB, 9) == IoxValue::High && ((now - start) < TIMEOUT_MS) { + now = tt.elapsed_ms(); + } + if now - start >= TIMEOUT_MS { + log::info!("Timeout before capture_async()!"); + } cam.capture_async(); } // wait for the transfer to finish - cam.capture_await(true); + start = tt.elapsed_ms(); + now = tt.elapsed_ms(); + while cam.udma_busy(cramium_hal::udma::Bank::Rx) && ((now - start) < TIMEOUT_MS) { + now = tt.elapsed_ms(); + // busy-wait to get better time resolution on when the frame ends + } + if now - start >= TIMEOUT_MS { + log::info!("Timeout before rx_buf()!"); + } let fb: &[u32] = cam.rx_buf(); - // fb is non-cacheable, slow memory. If we stride through it in u16 chunks, we end - // up fetching each location *twice*, because the native width of the bus is a u32 - // Stride through the slice as a u32, allowing us to make the most out of each slow - // read from IFRAM, and unpack the values into fast SRAM. - for (&u32src, u8dest) in fb.iter().zip(frame.chunks_mut(2)) { - u8dest[0] = (u32src & 0xff) as u8; - u8dest[1] = ((u32src >> 16) & 0xff) as u8; + // fb is an array of IMAGE_WIDTH x IMAGE_HEIGHT x u16 + // frame is an array of IMAGE_WIDTH x IMAGE_HEIGHT x u8 + // Take only the "Y" channel out of the fb array and write it to frame, but do it + // such that we are fetching a u32 each read from fb as this matches the native + // width of the bus (because fb is non-cacheable reading u16 ends up fetching the + // same word twice, then masking it at the CPU side in hardware). Also, the fb + // is slow to access relative to main memory. + // + // Also, commit the data to `frame` in inverse line order, e.g. flip the image + // vertically. + for (y_src, line) in fb.chunks(IMAGE_WIDTH / 2).enumerate() { + for (x_src, &u32src) in line.iter().enumerate() { + frame[(IMAGE_HEIGHT - y_src - 1) * IMAGE_WIDTH + 2 * x_src] = (u32src & 0xff) as u8; + frame[(IMAGE_HEIGHT - y_src - 1) * IMAGE_WIDTH + 2 * x_src + 1] = + ((u32src >> 16) & 0xff) as u8; + } } + frames += 1; } } diff --git a/services/bao-video/src/modules.rs b/services/bao-video/src/modules.rs new file mode 100644 index 000000000..26acbbaf7 --- /dev/null +++ b/services/bao-video/src/modules.rs @@ -0,0 +1,154 @@ +use crate::Point; +use crate::qr::*; + +/// This function takes a fraction described as `m.n`, where n is a fraction out of 1024, as well +/// as an `end` value, and returns numbers going from `0..end` such that with each +/// call of `next()` you produce a usize index in the range of `0..end` that is the +/// nearest algebraic integer that would result from adding the value `m.n` to the +/// current iteration count. +/// +/// Thus, a value of 1.5 would be passed as m=1, n=512 (as 512/1024 = 0.5). +/// +/// This "iterator" is not guaranteed to return every element in the range of `0..end`, +/// and in fact some numbers may not be returned at all, including `0` or `end-1`. However, +/// it does guarantee that the output is monatomically increasing, unless an error signal +/// is injected that is big enough to cause the count to go backwards. +/// +/// Features that we might need: +/// - A report of the "error sign", expressed as +1, 0, -1, which expresses the direction +/// in which we're rounding. For example, 6.3 would be returned as 6 with an error of +1. +/// whereas 6.9 would be returned as 7 with an error of -1. +/// - The ability to inject an error correction signal into the fractional counter. This +/// if the loop internally concludes that an error signal would produce a more accurate +/// sub-pixel sampling, the error count would be applied at that iteration so that subsequent +/// indices absorb the delta into their future counts. + +pub const FIXED_POINT_SHIFT: usize = 10; +pub const FIXED_POINT: usize = 1 << FIXED_POINT_SHIFT; // 1024 + +pub struct FracIter { + stride: usize, + current: usize, + end: usize, + count: usize, + finished: bool, +} +impl FracIter { + pub fn new(m: usize, n: usize, end: usize) -> Self { + assert!(n < FIXED_POINT); + // start the iterator at half the stride of a module + Self { + stride: (m << FIXED_POINT_SHIFT | n), + current: (m << FIXED_POINT_SHIFT | n) / 2, + end, + count: 0, + finished: false, + } + } + + pub fn next(&mut self) -> Option { + // short circuit computation if we've hit the end of the iterator + if self.finished { + return None; + } + if self.count < self.end { + let rounded_m = if (self.current & (FIXED_POINT - 1)) >= (FIXED_POINT / 2) { + (self.current >> FIXED_POINT_SHIFT) + 1 + } else { + self.current >> FIXED_POINT_SHIFT + }; + self.current += self.stride; + self.count += 1; + Some(rounded_m) + } else { + self.finished = true; + None + } + } + + pub fn reset(&mut self) { + self.finished = false; + self.current = self.stride / 2; + self.count = 0; + } + + #[allow(dead_code)] + /// Untested, but reserved API for adjusting grid streaming if we need better performance on + /// higher resolution QR codes + pub fn error(&self) -> isize { + if (self.current & (FIXED_POINT - 1)) == 0 { + 0 + } else if (self.current & (FIXED_POINT - 1)) >= (FIXED_POINT / 2) { + 1 + } else { + -1 + } + } + + #[allow(dead_code)] + /// Untested, but reserved API for adjusting grid streaming if we need better performance on + /// higher resolution QR codes + /// + /// The sign on m applies to n, but n is represented as a sign-less quantity + pub fn nudge(&mut self, m: isize, n: usize) { + assert!(n < FIXED_POINT); + if m >= 0 { + self.current += ((m as usize) << FIXED_POINT_SHIFT) | n; + } else { + // nudge down case + self.current -= ((-m as usize) << FIXED_POINT_SHIFT) | n; + } + } +} + +pub fn stream_to_grid( + image: &ImageRoi, + qr_size_pixels: usize, + qr_size_modules: usize, + margin: usize, +) -> Vec { + let pix_per_module = ((qr_size_pixels - margin * 2) << FIXED_POINT_SHIFT) / qr_size_modules; + let mut x_frac = FracIter::new( + pix_per_module >> FIXED_POINT_SHIFT, + pix_per_module & ((FIXED_POINT) - 1), + qr_size_modules, + ); + let mut y_frac = FracIter::new( + pix_per_module >> FIXED_POINT_SHIFT, + pix_per_module & ((FIXED_POINT) - 1), + qr_size_modules, + ); + let mut grid = Vec::::new(); + let mut i = 0; + log::info!("{}", qr_size_modules); + while let Some(y) = y_frac.next() { + const FUDGE_X: usize = 0; + const FUDGE_Y: usize = 0; + while let Some(x) = x_frac.next() { + if true { + // seems to work better with the simple threshold? + if image.data[(y + margin - FUDGE_Y) * image.width + (x + margin + FUDGE_X)] + < crate::BW_THRESH + { + grid.push(true); + } else { + grid.push(false); + } + } else { + if image + .neighbor_luma(Point::new(x as isize + margin as isize, y as isize + margin as isize)) + .unwrap() + < crate::BW_THRESH + { + grid.push(true); + } else { + grid.push(false); + } + } + } + x_frac.reset(); + print!("{} ", i); + i += 1; + } + grid +} diff --git a/services/bao-video/src/qr.rs b/services/bao-video/src/qr.rs index 5084a93b2..6044b36f3 100644 --- a/services/bao-video/src/qr.rs +++ b/services/bao-video/src/qr.rs @@ -18,9 +18,6 @@ const UPPER_1: usize = 2 << SEQ_FP_SHIFT; const LOWER_3: usize = 2 << SEQ_FP_SHIFT; const UPPER_3: usize = 4 << SEQ_FP_SHIFT; -pub const STORAGE: usize = 92; - -pub const BW_THRESH: u8 = 128; /// Finder search margin, as defined by expected QR code code widths (so this scales with the effective /// resolution of the code) pub const FINDER_SEARCH_MARGIN: isize = 2; @@ -44,6 +41,8 @@ pub fn draw_crosshair(image: &mut dyn FrameBuffer, p: Point) { ); } +#[allow(dead_code)] +/// Used for debugging QR code algorithms pub fn draw_line(image: &mut dyn FrameBuffer, l: &LineDerivation, color: ColorNative) { let axis = l.independent_axis; let (m, b) = l.equation.unwrap(); @@ -177,79 +176,55 @@ fn point_from_hv_lines(hline: &LineDerivation, vline: &LineDerivation) -> Option } // Threshold to reject points if they don't fit on the best-fit line -const OUTLIER_THRESHOLD: f32 = 1.0; +const OUTLIER_THRESHOLD: f32 = 0.5; const OUTLIER_ITERS: usize = 5; -#[derive(Copy, Clone)] +#[derive(Clone)] pub struct LineDerivation { pub equation: Option<(f32, f32)>, pub independent_axis: Axis, - pub data_points: [Point; STORAGE], - pub data_index: usize, + pub data_points: Vec, } impl LineDerivation { pub fn new(axis: Axis) -> Self { - LineDerivation { - equation: None, - independent_axis: axis, - data_points: [Point::new(0, 0); STORAGE], - data_index: 0, - } + LineDerivation { equation: None, independent_axis: axis, data_points: Vec::new() } } - pub fn push(&mut self, p: Point) { - if self.data_index < STORAGE { - self.data_points[self.data_index] = p; - self.data_index += 1; - } else { - assert!(false, "Static storage exceeded"); - } - } + pub fn push(&mut self, p: Point) { self.data_points.push(p); } /// This implementation heavily relies on f32, so it is slow on an embedded processor; however, /// we need the precision and the solving should be done only rarely. pub fn solve(&mut self) { - let mut points = [Point::default(); STORAGE]; - let mut filtered_points = [Point::default(); STORAGE]; - let mut residuals = [0.0f32; STORAGE]; - let mut sorted_residuals = [0.0f32; STORAGE]; - let mut filtered_index; - let mut count = self.data_index; let mut m_guess: f32 = 0.0; let mut b_guess: f32 = 0.0; - let mut converged_in = 0; - points[..count].copy_from_slice(&self.data_points[..count]); + let mut points = self.data_points.clone(); for guesses in 0..OUTLIER_ITERS { + let mut filtered_points = Vec::::new(); + let mut residuals = Vec::::new(); converged_in = guesses; // guess a best-fit line - (m_guess, b_guess) = least_squares_fit(&points[..count], self.independent_axis); + (m_guess, b_guess) = least_squares_fit(&points, self.independent_axis); // compute the residuals of the points to the guessed line - for (&p, residual) in points[..count].iter().zip(residuals.iter_mut()) { + for &p in points.iter() { let (x, y) = adjust_axis(p, self.independent_axis); let predicted_y = m_guess * x + b_guess; - *residual = y - predicted_y; - if *residual < 0.0 { - *residual = -*residual; - } + residuals.push((y - predicted_y).abs()); } // extract the median residual - sorted_residuals[..count].copy_from_slice(&residuals[..count]); - sorted_residuals[..count] + let mut sorted_residuals = residuals.clone(); + sorted_residuals .sort_unstable_by(|a, b| a.partial_cmp(b).unwrap_or(core::cmp::Ordering::Greater)); - let threshold = (sorted_residuals[count / 2] * 1.5).max(OUTLIER_THRESHOLD); + let threshold = (sorted_residuals[points.len() / 2] * 1.5).max(OUTLIER_THRESHOLD); - filtered_index = 0; - for (i, &p) in points[..count].iter().enumerate() { + for (i, &p) in points.iter().enumerate() { if residuals[i] <= threshold { - filtered_points[filtered_index] = p; - filtered_index += 1; + filtered_points.push(p); } } - if filtered_index == count { + if filtered_points.len() == points.len() { break; } else { - count = filtered_index; - points[..count].copy_from_slice(&filtered_points[..count]); + points = filtered_points; } } log::info!("Solver converged in {} iterations", converged_in); @@ -277,6 +252,8 @@ pub struct QrCorners { height: isize, derived_corner: Direction, finder_width: usize, + // destination QR image in pixels per side (as a perfect square) + dst_pixels: usize, } impl QrCorners { pub fn from_finders(points: &[Point; 3], dimensions: Point, finder_width: usize) -> Option { @@ -287,6 +264,7 @@ impl QrCorners { qrc.width = dimensions.x; qrc.height = dimensions.y; qrc.finder_width = finder_width; + qrc.dst_pixels = dimensions.x.min(dimensions.y) as usize; for &p in points { if p.x < x_half && p.y < y_half { @@ -313,8 +291,10 @@ impl QrCorners { } } + #[allow(dead_code)] pub fn derived_corner(&self) -> Direction { self.derived_corner } + #[allow(dead_code)] pub fn center_point(&self, dir: Direction) -> Option { self.corners[dir as usize].finder_ref } fn outline_search(&mut self, ir: &mut ImageRoi) { @@ -402,12 +382,26 @@ impl QrCorners { /// was made, with a margin added. /// /// The margin should be negative for the corners to go in toward the center. + /// + /// Canonical orientation for QR code should be like this: + /// +------+ + /// |O .| + /// | | + /// ^ |O O| + /// | +______+ + /// (0,0) -> + /// + /// Thus we want to orient so the discovered corner is in the NorthEast direction. pub fn mapping(&mut self, ir: &mut ImageRoi, margin: isize) -> ([Option; 4], [Option; 4]) { + // adjust the final target image size by the specified margin + self.dst_pixels = (self.dst_pixels as isize + 2 * margin) as usize; + // first, search for the lines that define the outline of the QR code self.outline_search(ir); let mut src = [None; 4]; let mut dst = [None; 4]; + let mut uk_direction: Option = None; for (i, corner) in self.corners.iter().enumerate() { if let Some(_p) = corner.finder_ref { @@ -415,6 +409,7 @@ impl QrCorners { // derive the corner from the extracted h and v lines along the finder pattern src[i] = point_from_hv_lines(&corner.h_line, &corner.v_line); } else { + uk_direction = Direction::try_from(i).ok(); // This is the unknown corner: // derive the corner from the h and v lines from the nearest finders' lines let h_line = match Direction::try_from(i) { @@ -431,19 +426,39 @@ impl QrCorners { Ok(Direction::SouthEast) => &self.corners[Direction::NorthEast as usize].v_line, _ => panic!("Bad index"), }; + // draw_line(debug, &h_line, [255, 255, 0]); + // draw_line(debug, &v_line, [255, 255, 0]); src[i] = point_from_hv_lines(h_line, v_line); } + let min = -margin; + let max = self.dst_pixels as isize + margin; dst[i] = match Direction::try_from(i) { - Ok(Direction::NorthWest) => Some(Point::new(-margin, self.height + margin)), - Ok(Direction::NorthEast) => Some(Point::new(self.width + margin, self.height + margin)), - Ok(Direction::SouthWest) => Some(Point::new(-margin, -margin)), - Ok(Direction::SouthEast) => Some(Point::new(self.width + margin, -margin)), + Ok(Direction::NorthWest) => Some(Point::new(min, max)), + Ok(Direction::NorthEast) => Some(Point::new(max, max)), + Ok(Direction::SouthWest) => Some(Point::new(min, min)), + Ok(Direction::SouthEast) => Some(Point::new(max, min)), _ => None, }; } - (src, dst) + // now shuffle the dst points such that the unknown corner is in the + // NorthEast direction + let mut shuffle: [Option; 4] = [None; 4]; + shuffle.copy_from_slice(&dst); + if let Some(uk_corner) = uk_direction { + let corner_index: usize = uk_corner.into(); + if corner_index != 0 { + for _ in 0..corner_index { + shuffle[1..].copy_from_slice(&dst[..3]); + shuffle[0] = dst[3]; + dst.copy_from_slice(&shuffle); + } + } + } + (src, shuffle) } + + pub fn qr_pixels(&self) -> usize { self.dst_pixels } } #[derive(Copy, Clone, Default, Debug)] @@ -569,10 +584,36 @@ impl TryFrom for Direction { } } } +impl Direction { + pub fn eight_way_iter() -> EightConnectedIter { EightConnectedIter { index: 0 } } +} +pub struct EightConnectedIter { + index: usize, +} + +impl Iterator for EightConnectedIter { + type Item = Direction; + + fn next(&mut self) -> Option { + let direction = match self.index { + 0 => Some(Direction::NorthWest), + 1 => Some(Direction::NorthEast), + 2 => Some(Direction::SouthWest), + 3 => Some(Direction::SouthEast), + 4 => Some(Direction::North), + 5 => Some(Direction::West), + 6 => Some(Direction::East), + 7 => Some(Direction::South), + _ => None, + }; + self.index += 1; + direction + } +} /// (0, 0) is at the lower left corner pub struct ImageRoi<'a> { - data: &'a mut [u8], + pub data: &'a mut [u8], pub width: usize, pub height: usize, thresh: u8, @@ -649,6 +690,7 @@ impl<'a> ImageRoi<'a> { } } + #[allow(dead_code)] pub fn absolute_to_roi(&self, point: Point) -> Option { let x = point.x - self.x0 as isize; let y = point.y - self.y0 as isize; @@ -658,6 +700,22 @@ impl<'a> ImageRoi<'a> { None } } + + pub fn neighbor_luma(&self, point: Point) -> Option { + let x = point.x; + let y = point.y; + let mut accumulated_luma = 0; + let mut total_counted = 0; + if x >= 0 && x < self.width as isize && y >= 0 && y < self.height as isize { + for direction in Direction::eight_way_iter() { + total_counted += 1; + let d: Point = direction.into(); + let p: Point = d + point; + accumulated_luma += self.data[(p.y * (self.width as isize) + p.x).max(0) as usize] as usize; + } + } + if total_counted > 0 { Some((accumulated_luma / total_counted) as u8) } else { None } + } } impl<'a> Iterator for ImageRoi<'_> { @@ -822,3 +880,33 @@ pub fn find_finders(candidates: &mut Vec, image: &[u8], thresh: u8, strid } if candidates.len() != 0 { candidate_width / candidates.len() } else { 0 } } + +const MODULES_BY_VERSION: [usize; 40] = [ + 21, 25, 29, 33, 37, 41, 45, 49, 53, 57, 61, 65, 69, 73, 77, 81, 85, 89, 93, 97, 101, 105, 109, 113, 117, + 121, 125, 129, 133, 137, 141, 145, 149, 153, 157, 161, 165, 169, 173, 177, +]; +const FP_SHIFT: usize = 1 << 16; +/// Returns tuple of (version, expected_modules) +pub fn guess_code_version(finder_width_pix: usize, qr_width_pix: usize) -> (usize, usize) { + let incoming_ratio = ((qr_width_pix * FP_SHIFT) / finder_width_pix) as isize; + let mut candidate_ratio = usize::MAX; + let mut candidate = 0; + let mut returned_modules = 0; + for (i, &modules) in MODULES_BY_VERSION.iter().enumerate() { + let version_ratio = ((modules * FP_SHIFT) / 7) as isize; + log::debug!( + "{}: version_ratio {} incoming_ratio {} diff {}", + i + 1, + version_ratio, + incoming_ratio, + version_ratio - incoming_ratio + ); + let prev_candidate = candidate_ratio; + candidate_ratio = candidate_ratio.min((version_ratio - incoming_ratio).abs() as usize); + if prev_candidate != candidate_ratio { + candidate = i + 1; + returned_modules = modules; + } + } + (candidate, returned_modules) +}