Merge pull request #593 from betrusted-io/mpw-dev

Mpw dev
This commit is contained in:
bunnie 2024-11-18 18:41:56 +08:00 committed by GitHub
commit 53de9cc273
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
10 changed files with 604 additions and 170 deletions

View file

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

View file

@ -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::<u8>()[..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::<u8>()[..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::<u8>()[..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::<u8>()[..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::<u8>()[..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::<u8>()[..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 {

View file

@ -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);

View file

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

View file

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

View file

@ -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());

View file

@ -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<f32>) -> Matrix3<i32> {
@ -78,8 +78,22 @@ pub fn apply_fixp_homography(homography: &Matrix3<i32>, 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)
}
}

View file

@ -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::<Point>::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<usize> = 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::<Point>::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::<Point>::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;
}
}

View file

@ -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<usize> {
// 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<bool> {
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::<bool>::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
}

View file

@ -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<Point>,
}
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::<Point>::new();
let mut residuals = Vec::<f32>::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<Self> {
@ -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<Point> { 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<Point>; 4], [Option<Point>; 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<Direction> = 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<Point>; 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<usize> 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<Self::Item> {
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<Point> {
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<u8> {
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<Point>, 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)
}