rework camera main loop to be interrupt driven

this will free up some time for concurrent processing instead of
spin-waiting on new frames
This commit is contained in:
bunnie 2024-12-04 16:15:58 +08:00
parent 02b2b86184
commit a57f3f1977

View file

@ -1,7 +1,9 @@
use cramium_hal::iox::{IoxPort, IoxValue}; use cramium_hal::iox::{IoxPort, IoxValue};
use cramium_hal::minigfx::*; use cramium_hal::minigfx::*;
use cramium_hal::sh1107::{Mono, Oled128x128}; use cramium_hal::sh1107::{Mono, Oled128x128};
use cramium_hal::udma::{PeriphId, Udma}; use cramium_hal::udma::PeriphId;
use num_traits::ToPrimitive;
use utralib::utra;
mod gfx; mod gfx;
mod homography; mod homography;
@ -51,6 +53,32 @@ pub fn blit_to_display(sh1107: &mut Oled128x128, frame: &[u8], display_cleared:
} }
} }
#[derive(num_derive::FromPrimitive, num_derive::ToPrimitive, Debug)]
enum Opcode {
CamIrq,
InvalidCall,
}
#[repr(align(32))]
struct CamIrq {
csr: utralib::CSR<u32>,
cid: u32,
}
fn handle_irq(_irq_no: usize, arg: *mut usize) {
let cam_irq: &mut CamIrq = unsafe { &mut *(arg as *mut CamIrq) };
// clear the pending interrupt - assume it's just the camera for now
let pending = cam_irq.csr.r(utra::irqarray8::EV_PENDING);
cam_irq.csr.wo(utra::irqarray8::EV_PENDING, pending);
// activate the handler
xous::try_send_message(
cam_irq.cid,
xous::Message::new_scalar(Opcode::CamIrq.to_usize().unwrap(), pending as usize, 0, 0, 0),
)
.ok();
}
fn main() -> ! { fn main() -> ! {
let stack_size = 1 * 1024 * 1024; let stack_size = 1 * 1024 * 1024;
std::thread::Builder::new().stack_size(stack_size).spawn(wrapped_main).unwrap().join().unwrap() std::thread::Builder::new().stack_size(stack_size).spawn(wrapped_main).unwrap().join().unwrap()
@ -60,6 +88,9 @@ fn wrapped_main() -> ! {
log_server::init_wait().unwrap(); log_server::init_wait().unwrap();
log::set_max_level(log::LevelFilter::Info); log::set_max_level(log::LevelFilter::Info);
let xns = xous_names::XousNames::new().unwrap();
let sid = xns.register_name("bao video subsystem", None).expect("can't register server");
let tt = ticktimer::Ticktimer::new().unwrap(); let tt = ticktimer::Ticktimer::new().unwrap();
let iox = cram_hal_service::IoxHal::new(); let iox = cram_hal_service::IoxHal::new();
@ -96,14 +127,65 @@ fn wrapped_main() -> ! {
#[cfg(feature = "decongest-udma")] #[cfg(feature = "decongest-udma")]
log::info!("Decongest udma option enabled."); log::info!("Decongest udma option enabled.");
// register interrupt handler
let irq = xous::syscall::map_memory(
xous::MemoryAddress::new(utra::irqarray8::HW_IRQARRAY8_BASE),
None,
4096,
xous::MemoryFlags::R | xous::MemoryFlags::W,
)
.expect("couldn't map IRQ CSR range");
let mut irq_csr = utralib::CSR::new(irq.as_mut_ptr() as *mut u32);
irq_csr.wo(utra::irqarray8::EV_PENDING, 0xFFFF); // clear any pending interrupts
let cid = xous::connect(sid).unwrap(); // self-connection always succeeds
let cam_irq = CamIrq { csr: utralib::CSR::new(irq.as_mut_ptr() as *mut u32), cid };
let irq_arg = &cam_irq as *const CamIrq as *mut usize;
log::debug!("irq_arg: {:x}", irq_arg as usize);
xous::claim_interrupt(utra::irqarray8::IRQARRAY8_IRQ, handle_irq, irq_arg).expect("couldn't claim IRQ8");
// enable camera Rx IRQ
irq_csr.wfo(utra::irqarray8::EV_ENABLE_CAM_RX, 1);
// kick off the first acquisition synchronous to the frame sync. With NTO/DAR-704 fix we
// could turn on the sync_sof field and skip this step.
while iox.get_gpio_pin_value(IoxPort::PB, 9) == IoxValue::High {}
cam.capture_async();
let mut frames = 0; let mut frames = 0;
let mut frame = [0u8; IMAGE_WIDTH * IMAGE_HEIGHT]; let mut frame = [0u8; IMAGE_WIDTH * IMAGE_HEIGHT];
let mut decode_success; let mut decode_success;
while iox.get_gpio_pin_value(IoxPort::PB, 9) == IoxValue::High {} let mut msg_opt = None;
loop { loop {
xous::reply_and_receive_next(sid, &mut msg_opt).unwrap();
let msg = msg_opt.as_mut().unwrap();
let opcode = num_traits::FromPrimitive::from_usize(msg.body.id()).unwrap_or(Opcode::InvalidCall);
log::debug!("{:?}", opcode);
match opcode {
Opcode::CamIrq => {
#[cfg(not(feature = "decongest-udma"))] #[cfg(not(feature = "decongest-udma"))]
cam.capture_async(); cam.capture_async();
// copy the camera data to our FB
let fb: &[u32] = cam.rx_buf();
// 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;
let mut candidates = Vec::<Point>::new(); let mut candidates = Vec::<Point>::new();
decode_success = false; decode_success = false;
log::info!("------------- SEARCH {} -----------", frames); log::info!("------------- SEARCH {} -----------", frames);
@ -118,7 +200,8 @@ fn wrapped_main() -> ! {
&candidates.try_into().unwrap(), &candidates.try_into().unwrap(),
Point::new(IMAGE_WIDTH as isize, IMAGE_HEIGHT as isize), Point::new(IMAGE_WIDTH as isize, IMAGE_HEIGHT as isize),
// add a search margin on the finder width // add a search margin on the finder width
(finder_width + (qr::FINDER_SEARCH_MARGIN * finder_width) / (1 + 1 + 3 + 1 + 1)) as usize, (finder_width + (qr::FINDER_SEARCH_MARGIN * finder_width) / (1 + 1 + 3 + 1 + 1))
as usize,
) { ) {
let dims = Point::new(IMAGE_WIDTH as isize, IMAGE_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 mut il = qr::ImageRoi::new(&mut frame, dims, BW_THRESH);
@ -152,18 +235,22 @@ fn wrapped_main() -> ! {
// iterate through pixels and apply homography // iterate through pixels and apply homography
for y in 0..qr_corners.qr_pixels() { for y in 0..qr_corners.qr_pixels() {
for x in 0..qr_corners.qr_pixels() { for x in 0..qr_corners.qr_pixels() {
let (x_src, y_src) = let (x_src, y_src) = homography::apply_fixp_homography(
homography::apply_fixp_homography(&h_inv_fp, (x as i32, y as i32)); &h_inv_fp,
(x as i32, y as i32),
);
if (x_src as i32 >= 0) if (x_src as i32 >= 0)
&& ((x_src as i32) < dims.x as i32) && ((x_src as i32) < dims.x as i32)
&& (y_src as i32 >= 0) && (y_src as i32 >= 0)
&& ((y_src as i32) < dims.y as i32) && ((y_src as i32) < dims.y as i32)
{ {
// println!("{},{} -> {},{}", x_src as i32, y_src as i32, x, y); // println!("{},{} -> {},{}", x_src as i32, y_src as i32, x,
// y);
aligned[qr_corners.qr_pixels() * y as usize + x as usize] = aligned[qr_corners.qr_pixels() * y as usize + x as usize] =
frame[IMAGE_WIDTH * y_src as usize + x_src as usize]; frame[IMAGE_WIDTH * y_src as usize + x_src as usize];
} else { } else {
aligned[qr_corners.qr_pixels() * y as usize + x as usize] = 255; aligned[qr_corners.qr_pixels() * y as usize + x as usize] =
255;
} }
} }
} }
@ -171,8 +258,10 @@ fn wrapped_main() -> ! {
// we can also know the location of the finders by transforming them // we can also know the location of the finders by transforming them
let h_fp = homography::matrix3_to_fixp(h); let h_fp = homography::matrix3_to_fixp(h);
for (i, &c) in candidates_orig.iter().enumerate() { for (i, &c) in candidates_orig.iter().enumerate() {
let (x, y) = let (x, y) = homography::apply_fixp_homography(
homography::apply_fixp_homography(&h_fp, (c.x as i32, c.y as i32)); &h_fp,
(c.x as i32, c.y as i32),
);
x_candidates[i] = Point::new(x as isize, y as isize); x_candidates[i] = Point::new(x as isize, y as isize);
} }
qr_pixels = Some(qr_corners.qr_pixels()); qr_pixels = Some(qr_corners.qr_pixels());
@ -184,12 +273,15 @@ fn wrapped_main() -> ! {
if let Some(qr_width) = qr_pixels { if let Some(qr_width) = qr_pixels {
// show the transformed/aligned frame // show the transformed/aligned frame
frame.fill(255); frame.fill(255);
for (dst_line, src_line) in frame.chunks_mut(IMAGE_WIDTH).zip(aligned.chunks(qr_width)) { 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 // "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 // this is just a perceptual trick to prevent users from shifting the position of
// camera // the camera
for (dst, &src) in for (dst, &src) in dst_line[qr::HOMOGRAPHY_MARGIN.abs() as usize..]
dst_line[qr::HOMOGRAPHY_MARGIN.abs() as usize..].iter_mut().zip(src_line.iter()) .iter_mut()
.zip(src_line.iter())
{ {
*dst = src; *dst = src;
} }
@ -205,7 +297,8 @@ fn wrapped_main() -> ! {
// Confirm that the finders coordinates are valid // Confirm that the finders coordinates are valid
let mut checked_candidates = Vec::<Point>::new(); let mut checked_candidates = Vec::<Point>::new();
let x_finder_width = let x_finder_width =
qr::find_finders(&mut checked_candidates, &aligned, BW_THRESH, qr_width as _) as isize; qr::find_finders(&mut checked_candidates, &aligned, BW_THRESH, qr_width as _)
as isize;
log::info!("x_finder width: {}", x_finder_width); log::info!("x_finder width: {}", x_finder_width);
// check that the new coordinates are within delta pixels of the original // check that the new coordinates are within delta pixels of the original
@ -234,10 +327,17 @@ fn wrapped_main() -> ! {
qr_width - 2 * (qr::HOMOGRAPHY_MARGIN.abs() as usize) qr_width - 2 * (qr::HOMOGRAPHY_MARGIN.abs() as usize)
); );
let qr = let qr = qr::ImageRoi::new(
qr::ImageRoi::new(&mut aligned, Point::new(qr_width as _, qr_width as _), BW_THRESH); &mut aligned,
let grid = Point::new(qr_width as _, qr_width as _),
modules::stream_to_grid(&qr, qr_width, modules, qr::HOMOGRAPHY_MARGIN.abs() as usize); BW_THRESH,
);
let grid = modules::stream_to_grid(
&qr,
qr_width,
modules,
qr::HOMOGRAPHY_MARGIN.abs() as usize,
);
println!("grid len {}", grid.len()); println!("grid len {}", grid.len());
for y in 0..modules { for y in 0..modules {
@ -250,8 +350,9 @@ fn wrapped_main() -> ! {
} }
println!(" {:2}", y); println!(" {:2}", y);
} }
let simple = let simple = rqrr::SimpleGrid::from_func(modules, |x, y| {
rqrr::SimpleGrid::from_func(modules, |x, y| grid[(modules - 1) - x + y * modules]); grid[(modules - 1) - x + y * modules]
});
let grid = rqrr::Grid::new(simple); let grid = rqrr::Grid::new(simple);
match grid.decode() { match grid.decode() {
Ok((meta, content)) => { Ok((meta, content)) => {
@ -334,14 +435,25 @@ fn wrapped_main() -> ! {
// clear the front buffer // clear the front buffer
sh1107.clear(); sh1107.clear();
// re-initiate the capture. This is done at the bottom of the loop because UDMA
// congestion leads to system instability. When this problem is solved, we would
// actually want to re-initiate the capture immediately (or leave it on continuous mode)
// to allow capture to process concurrently with the code. However, there is a bug
// in the SPIM block that prevents proper usage with high bus contention that should
// be fixed in NTO.
const TIMEOUT_MS: u64 = 100; const TIMEOUT_MS: u64 = 100;
#[cfg(feature = "decongest-udma")] #[cfg(feature = "decongest-udma")]
{ {
let start = tt.elapsed_ms(); let start = tt.elapsed_ms();
let mut now = tt.elapsed_ms(); let mut now = tt.elapsed_ms();
// don't parallelize the camera capture to avoid triggering a hardware bug // this is required because if we initiate the capture in the middle
// in the SPIM block. // of a frame, we get an offset result. This should be fixed by DAR-704
while iox.get_gpio_pin_value(IoxPort::PB, 9) == IoxValue::High && ((now - start) < TIMEOUT_MS) { // on NTO if the pull request is accepted; in which case, we can just rely
// on setting bit 30 of the CFG_GLOBAL register which will cause any
// RX start request to align to the beginning of a frame automatically.
while iox.get_gpio_pin_value(IoxPort::PB, 9) == IoxValue::High
&& ((now - start) < TIMEOUT_MS)
{
now = tt.elapsed_ms(); now = tt.elapsed_ms();
} }
if now - start >= TIMEOUT_MS { if now - start >= TIMEOUT_MS {
@ -350,9 +462,12 @@ fn wrapped_main() -> ! {
cam.capture_async(); cam.capture_async();
} }
// this is no longer the case because we're relying on interrupts to wake us up.
/*
// wait for the transfer to finish // wait for the transfer to finish
let start = tt.elapsed_ms(); let start = tt.elapsed_ms();
let mut now = tt.elapsed_ms(); let mut now = tt.elapsed_ms();
use cramium_hal::udma::Udma;
while cam.udma_busy(cramium_hal::udma::Bank::Rx) && ((now - start) < TIMEOUT_MS) { while cam.udma_busy(cramium_hal::udma::Bank::Rx) && ((now - start) < TIMEOUT_MS) {
now = tt.elapsed_ms(); now = tt.elapsed_ms();
// busy-wait to get better time resolution on when the frame ends // busy-wait to get better time resolution on when the frame ends
@ -360,26 +475,11 @@ fn wrapped_main() -> ! {
if now - start >= TIMEOUT_MS { if now - start >= TIMEOUT_MS {
log::info!("Timeout before rx_buf()!"); log::info!("Timeout before rx_buf()!");
} }
let fb: &[u32] = cam.rx_buf(); */
}
// fb is an array of IMAGE_WIDTH x IMAGE_HEIGHT x u16 Opcode::InvalidCall => {
// frame is an array of IMAGE_WIDTH x IMAGE_HEIGHT x u8 log::error!("Invalid call to bao video server: {:?}", msg);
// 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;
} }
} }