mirror of
https://github.com/saymrwulf/xous-core.git
synced 2026-11-16 08:38:26 +00:00
commit
53de9cc273
10 changed files with 604 additions and 170 deletions
|
|
@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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",
|
||||
|
|
|
|||
|
|
@ -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());
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
154
services/bao-video/src/modules.rs
Normal file
154
services/bao-video/src/modules.rs
Normal 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
|
||||
}
|
||||
|
|
@ -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)
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue