#![no_std]
#[cfg(feature = "rttdebug")]
use panic_rtt_core::rprintln;
#[derive(Debug)]
pub enum Error<CommE> {
Comm(CommE),
Timeout,
}
pub const CAM_PERIPH_ADDRESS_00: u8 = 0x90 >> 1;
pub const CAM_PERIPH_ADDRESS_01: u8 = 0x98 >> 1;
pub const CAM_PERIPH_ADDRESS_10: u8 = 0xB0 >> 1;
pub const CAM_PERIPH_ADDRESS_11: u8 = 0xB8 >> 1;
pub const PX4FLOW_CAM_ADDRESS: u8 = CAM_PERIPH_ADDRESS_11;
pub const ARDUCAM_BREAKOUT_ADDRESS: u8 = CAM_PERIPH_ADDRESS_00;
pub struct Mt9v034<I2C> {
base_address: u8,
i2c: I2C,
win_width_a: u16,
win_height_a: u16,
win_width_b: u16,
win_height_b: u16,
col_bin_factor_a: BinningFactor,
row_bin_factor_a: BinningFactor,
col_bin_factor_b: BinningFactor,
row_bin_factor_b: BinningFactor,
}
impl<I2C, CommE> Mt9v034<I2C>
where
I2C: embedded_hal::blocking::i2c::Write<Error = CommE>
+ embedded_hal::blocking::i2c::Read<Error = CommE>
+ embedded_hal::blocking::i2c::WriteRead<Error = CommE>,
{
pub fn new(i2c: I2C, address: u8) -> Self {
Self {
base_address: address,
i2c,
win_width_a: 0,
win_height_a: 0,
win_width_b: 0,
win_height_b: 0,
col_bin_factor_a: BinningFactor::None,
row_bin_factor_a: BinningFactor::None,
col_bin_factor_b: BinningFactor::None,
row_bin_factor_b: BinningFactor::None,
}
}
pub fn set_context(
&mut self,
context: ParamContext,
) -> Result<(), crate::Error<CommE>> {
self.write_general_reg(GeneralRegister::Control, context as u16)
}
pub fn setup(&mut self) -> Result<(), crate::Error<CommE>> {
#[cfg(feature = "rttdebug")]
rprintln!("mt9v034-i2c setup start 0x{:x}", self.base_address);
let _version = self.read_reg_u16(GeneralRegister::ChipVersion as u8)?;
self.set_context_b_default_dimensions()?;
self.set_context_b_shutter_defaults()?;
self.set_context_a_default_dimensions()?;
self.set_context_a_shutter_defaults()?;
self.set_general_defaults(4096)?;
self.set_context(ParamContext::ContextA)?;
self.write_general_reg(GeneralRegister::SoftReset, 0x01)?;
let _verify_version =
self.read_reg_u16(GeneralRegister::ChipVersion as u8)?;
#[cfg(feature = "rttdebug")]
rprintln!("mt9v034-i2c setup done, vers: 0x{:x}", _verify_version);
Ok(())
}
pub fn setup_with_dimensions(
&mut self,
win_width_a: u16,
win_height_a: u16,
col_bin_a: BinningFactor,
row_bin_a: BinningFactor,
win_width_b: u16,
win_height_b: u16,
col_bin_b: BinningFactor,
row_bin_b: BinningFactor,
default_context: ParamContext,
) -> Result<(), crate::Error<CommE>> {
#[cfg(feature = "rttdebug")]
rprintln!("mt9v034-i2c setup start 0x{:x}", self.base_address);
let _version = self.read_reg_u16(GeneralRegister::ChipVersion as u8)?;
self.set_context_dimensions(
ParamContext::ContextB,
win_height_b,
win_width_b,
col_bin_b,
row_bin_b,
)?;
self.set_context_b_shutter_defaults()?;
self.set_context_dimensions(
ParamContext::ContextA,
win_height_a,
win_width_a,
col_bin_a,
row_bin_a,
)?;
self.set_context_a_shutter_defaults()?;
let max_pixels = match default_context {
ParamContext::ContextA => {
(self.win_height_a / self.row_bin_factor_a as u16)
* (self.win_width_a / self.col_bin_factor_a as u16)
}
ParamContext::ContextB => {
(self.win_height_b / self.row_bin_factor_b as u16)
* (self.win_width_b / self.col_bin_factor_b as u16)
}
};
self.set_general_defaults(max_pixels as u32)?;
self.set_context(default_context)?;
self.write_general_reg(GeneralRegister::SoftReset, 0x01)?;
let _verify_version =
self.read_reg_u16(GeneralRegister::ChipVersion as u8)?;
#[cfg(feature = "rttdebug")]
rprintln!("mt9v034-i2c setup done, vers: 0x{:x}", _verify_version);
Ok(())
}
fn write_general_reg(
&mut self,
reg: GeneralRegister,
data: u16,
) -> Result<(), crate::Error<CommE>> {
self.write_reg_u16(reg as u8, data)
}
fn write_context_a_reg(
&mut self,
reg: ContextARegister,
data: u16,
) -> Result<(), crate::Error<CommE>> {
self.write_reg_u16(reg as u8, data)
}
fn write_context_b_reg(
&mut self,
reg: ContextBRegister,
data: u16,
) -> Result<(), crate::Error<CommE>> {
self.write_reg_u16(reg as u8, data)
}
pub fn set_agc_pixel_count(
&mut self,
max_pixels: u32,
) -> Result<(), crate::Error<CommE>> {
let agc_pixels: u16 = if max_pixels > 65535 {
65535
} else {
max_pixels as u16
};
self.write_general_reg(GeneralRegister::AgcAecPixelCount, agc_pixels)
}
pub fn set_general_defaults(
&mut self,
max_pixel_count: u32,
) -> Result<(), crate::Error<CommE>> {
self.write_reg_u8(GeneralRegister::RowNoiseConstant as u8, 0x00)?;
self.write_reg_u16(0x13, 0x2D2E)?;
self.write_reg_u16(0x20, 0x03C7)?;
self.write_reg_u16(0x24, 0x001B)?;
self.write_reg_u16(0x2B, 0x0003)?;
self.write_reg_u16(0x2F, 0x0003)?;
self.write_general_reg(GeneralRegister::TestPattern, 0x0000)?;
self.write_general_reg(GeneralRegister::RowNoiseCorrCtrl, 0x0101)?;
self.write_general_reg(GeneralRegister::AecAgcEnable, 0x0011)?;
self.write_general_reg(GeneralRegister::HdrEnable, 0x0001)?;
self.write_general_reg(GeneralRegister::MinExposure, 0x0001)?;
self.write_general_reg(GeneralRegister::MaxExposure, 0x1F4)?;
self.write_general_reg(GeneralRegister::AgcMaxGain, 0x0010)?;
self.set_agc_pixel_count(max_pixel_count)?;
self.write_general_reg(GeneralRegister::AgcAecDesiredBin, 20)?;
self.write_general_reg(GeneralRegister::AdcResCtrl, 0x0303)?;
self.write_general_reg(GeneralRegister::AecUpdate, 0x02)?;
self.write_general_reg(GeneralRegister::AecLowpass, 0x01)?;
self.write_general_reg(GeneralRegister::AgcUpdate, 0x02)?;
self.write_general_reg(GeneralRegister::AgcLowpass, 0x02)?;
Ok(())
}
pub fn set_context_b_default_dimensions(
&mut self,
) -> Result<(), crate::Error<CommE>> {
const VIDEO_IMG_HEIGHT: u16 = 480 / 2;
const VIDEO_IMG_WIDTH: u16 = 752 / 2;
const COLUMN_BINNING: BinningFactor = BinningFactor::Two;
const ROW_BINNING: BinningFactor = BinningFactor::Two;
const WINDOW_W: u16 = VIDEO_IMG_WIDTH * 2;
const WINDOW_H: u16 = VIDEO_IMG_HEIGHT * 2;
self.set_context_dimensions(
ParamContext::ContextB,
WINDOW_H,
WINDOW_W,
COLUMN_BINNING,
ROW_BINNING,
)
}
pub fn set_context_a_default_dimensions(
&mut self,
) -> Result<(), crate::Error<CommE>> {
const FLOW_IMG_HEIGHT: u16 = 64;
const FLOW_IMG_WIDTH: u16 = 64;
const COLUMN_BINNING: BinningFactor = BinningFactor::Four;
const ROW_BINNING: BinningFactor = BinningFactor::Four;
const WINDOW_W: u16 = FLOW_IMG_WIDTH * 4;
const WINDOW_H: u16 = FLOW_IMG_HEIGHT * 4;
self.set_context_dimensions(
ParamContext::ContextA,
WINDOW_H,
WINDOW_W,
COLUMN_BINNING,
ROW_BINNING,
)
}
pub fn set_context_dimensions(
&mut self,
context: ParamContext,
window_h: u16,
window_w: u16,
col_bin_factor: BinningFactor,
row_bin_factor: BinningFactor,
) -> Result<(), crate::Error<CommE>> {
let min_h_blank: u16 = match col_bin_factor {
BinningFactor::None => 61,
BinningFactor::Two => 71,
BinningFactor::Four => 91,
};
let col_binning = binning_factor_to_selector(col_bin_factor);
let row_binning = binning_factor_to_selector(row_bin_factor);
let h_blank: u16 = 425 + min_h_blank;
const V_BLANK: u16 = 10;
const MIN_COL_START: u16 = 1;
const MIN_ROW_START: u16 = 4;
let col_start: u16 = (MAX_FRAME_WIDTH - window_w) / 2 + MIN_COL_START;
let row_start: u16 = (MAX_FRAME_HEIGHT - window_h) / 2 + MIN_ROW_START;
let read_mode: u16 =
0x300 | ((col_binning as u16) << 2) | (row_binning as u16);
match context {
ParamContext::ContextA => {
self.win_width_a = window_w;
self.win_height_a = window_h;
self.col_bin_factor_a = col_bin_factor;
self.row_bin_factor_a = row_bin_factor;
self.write_context_a_reg(
ContextARegister::WindowWidth,
window_w,
)?;
self.write_context_a_reg(
ContextARegister::WindowHeight,
window_h,
)?;
self.write_context_a_reg(ContextARegister::HBlanking, h_blank)?;
self.write_context_a_reg(ContextARegister::VBlanking, V_BLANK)?;
self.write_context_a_reg(
ContextARegister::ReadMode,
read_mode,
)?;
self.write_context_a_reg(
ContextARegister::ColumnStart,
col_start,
)?;
self.write_context_a_reg(
ContextARegister::RowStart,
row_start,
)?;
}
ParamContext::ContextB => {
self.win_width_b = window_w;
self.win_height_b = window_h;
self.col_bin_factor_b = col_bin_factor;
self.row_bin_factor_b = row_bin_factor;
self.write_context_b_reg(
ContextBRegister::WindowWidth,
window_w,
)?;
self.write_context_b_reg(
ContextBRegister::WindowHeight,
window_h,
)?;
self.write_context_b_reg(ContextBRegister::HBlanking, h_blank)?;
self.write_context_b_reg(ContextBRegister::VBlanking, V_BLANK)?;
self.write_context_b_reg(
ContextBRegister::ReadMode,
read_mode,
)?;
self.write_context_b_reg(
ContextBRegister::ColumnStart,
col_start,
)?;
self.write_context_b_reg(
ContextBRegister::RowStart,
row_start,
)?;
}
}
Ok(())
}
pub fn set_context_a_shutter_defaults(
&mut self,
) -> Result<(), crate::Error<CommE>> {
self.write_context_a_reg(ContextARegister::CoarseShutter1, 443)?;
self.write_context_a_reg(ContextARegister::CoarseShutter2, 473)?;
self.write_context_a_reg(ContextARegister::CoarseShutterCtrl, 0x0164)?;
self.write_context_a_reg(ContextARegister::CoarseShutterTotal, 0x01E0)?;
Ok(())
}
pub fn set_context_b_shutter_defaults(
&mut self,
) -> Result<(), crate::Error<CommE>> {
self.write_context_b_reg(ContextBRegister::CoarseShutter1, 443)?;
self.write_context_b_reg(ContextBRegister::CoarseShutter2, 473)?;
self.write_context_b_reg(ContextBRegister::CoarseShutterCtrl, 0x0164)?;
self.write_context_b_reg(ContextBRegister::CoarseShutterTotal, 0x01E0)?;
Ok(())
}
#[cfg(feature = "rttdebug")]
pub fn dump_context_a_settings(
&mut self,
) -> Result<(), crate::Error<CommE>> {
rprintln!("-- Context A settings:");
self.dump_register_setting(ContextARegister::WindowWidth as u8)?;
self.dump_register_setting(ContextARegister::WindowHeight as u8)?;
self.dump_register_setting(ContextARegister::HBlanking as u8)?;
self.dump_register_setting(ContextARegister::VBlanking as u8)?;
self.dump_register_setting(ContextARegister::ReadMode as u8)?;
self.dump_register_setting(ContextARegister::ColumnStart as u8)?;
self.dump_register_setting(ContextARegister::RowStart as u8)?;
self.dump_register_setting(ContextARegister::CoarseShutter1 as u8)?;
self.dump_register_setting(ContextARegister::CoarseShutter2 as u8)?;
self.dump_register_setting(ContextARegister::CoarseShutterCtrl as u8)?;
self.dump_register_setting(ContextARegister::CoarseShutterTotal as u8)?;
Ok(())
}
#[cfg(feature = "rttdebug")]
pub fn dump_context_b_settings(
&mut self,
) -> Result<(), crate::Error<CommE>> {
rprintln!("-- Context B settings:");
self.dump_register_setting(ContextBRegister::WindowWidth as u8)?;
self.dump_register_setting(ContextBRegister::WindowHeight as u8)?;
self.dump_register_setting(ContextBRegister::HBlanking as u8)?;
self.dump_register_setting(ContextBRegister::VBlanking as u8)?;
self.dump_register_setting(ContextBRegister::ReadMode as u8)?;
self.dump_register_setting(ContextBRegister::ColumnStart as u8)?;
self.dump_register_setting(ContextBRegister::RowStart as u8)?;
self.dump_register_setting(ContextBRegister::CoarseShutter1 as u8)?;
self.dump_register_setting(ContextBRegister::CoarseShutter2 as u8)?;
self.dump_register_setting(ContextBRegister::CoarseShutterCtrl as u8)?;
self.dump_register_setting(ContextBRegister::CoarseShutterTotal as u8)?;
Ok(())
}
#[cfg(feature = "rttdebug")]
pub fn dump_general_settings(&mut self) -> Result<(), crate::Error<CommE>> {
rprintln!("-- General settings:");
self.dump_register_setting(GeneralRegister::Control as u8)?;
self.dump_register_setting(GeneralRegister::RowNoiseConstant as u8)?;
self.dump_register_setting(0x13)?;
self.dump_register_setting(0x20)?;
self.dump_register_setting(0x24)?;
self.dump_register_setting(0x2B)?;
self.dump_register_setting(0x2F)?;
self.dump_register_setting(GeneralRegister::RowNoiseCorrCtrl as u8)?;
self.dump_register_setting(GeneralRegister::TestPattern as u8)?;
self.dump_register_setting(GeneralRegister::AecAgcEnable as u8)?;
self.dump_register_setting(GeneralRegister::HdrEnable as u8)?;
self.dump_register_setting(GeneralRegister::MinExposure as u8)?;
self.dump_register_setting(GeneralRegister::MaxExposure as u8)?;
self.dump_register_setting(GeneralRegister::AgcMaxGain as u8)?;
self.dump_register_setting(GeneralRegister::AgcAecPixelCount as u8)?;
self.dump_register_setting(GeneralRegister::AgcAecDesiredBin as u8)?;
self.dump_register_setting(GeneralRegister::AdcResCtrl as u8)?;
self.dump_register_setting(GeneralRegister::AecUpdate as u8)?;
self.dump_register_setting(GeneralRegister::AecLowpass as u8)?;
self.dump_register_setting(GeneralRegister::AgcUpdate as u8)?;
self.dump_register_setting(GeneralRegister::AgcLowpass as u8)?;
Ok(())
}
#[cfg(feature = "rttdebug")]
pub fn dump_register_setting(
&mut self,
reg: u8,
) -> Result<(), crate::Error<CommE>> {
let val = self.read_reg_u16(reg)?;
rprintln!("0x{:X} = 0x{:x} {}", reg, val, val);
Ok(())
}
pub fn enable_pixel_test_pattern(
&mut self,
enable: bool,
pattern: PixelTestPattern,
) -> Result<(), crate::Error<CommE>> {
if enable {
self.write_general_reg(
GeneralRegister::TestPattern,
(pattern as u16) | 0x2000,
)?;
self.write_general_reg(GeneralRegister::RowNoiseCorrCtrl, 0x0000)?;
} else {
self.write_general_reg(GeneralRegister::TestPattern, 0x0000)?;
self.write_general_reg(GeneralRegister::RowNoiseCorrCtrl, 0x0101)?;
}
Ok(())
}
pub fn protect_all_registers(
&mut self,
protect: bool,
) -> Result<(), crate::Error<CommE>> {
self.write_general_reg(
GeneralRegister::RegisterLock,
if protect { 0xDEAD } else { 0xBEEF },
)
}
pub fn read_reg_u8(&mut self, reg: u8) -> Result<u8, crate::Error<CommE>> {
let cmd_buf = [reg];
let mut recv_buf = [0u8];
self.i2c
.write_read(self.base_address, &cmd_buf, &mut recv_buf)
.map_err(Error::Comm)?;
Ok(recv_buf[0])
}
pub fn read_reg_u16(
&mut self,
reg: u8,
) -> Result<u16, crate::Error<CommE>> {
let upper = (self.read_reg_u8(reg)? as u16) << 8;
let lower = self.read_reg_u8(FOLLOW_UP_ADDRESS)? as u16;
Ok(upper | lower)
}
pub fn write_reg_u8(
&mut self,
reg: u8,
val: u8,
) -> Result<(), crate::Error<CommE>> {
let write_buf = [reg, val];
self.i2c
.write(self.base_address, &write_buf)
.map_err(Error::Comm)?;
Ok(())
}
pub fn write_reg_u16(
&mut self,
reg: u8,
data: u16,
) -> Result<(), crate::Error<CommE>> {
self.write_reg_u8(reg, (data >> 8) as u8)?;
self.write_reg_u8(FOLLOW_UP_ADDRESS, (data & 0xFF) as u8)?;
Ok(())
}
}
const FOLLOW_UP_ADDRESS: u8 = 0xF0;
pub const MAX_FRAME_HEIGHT: u16 = 480;
pub const MAX_FRAME_WIDTH: u16 = 752;
#[repr(u8)]
pub enum GeneralRegister {
ChipVersion = 0x00,
Control = 0x07,
SoftReset = 0x0c,
HdrEnable = 0x0f,
AdcResCtrl = 0x1c,
RowNoiseCorrCtrl = 0x70,
RowNoiseConstant = 0x72,
TestPattern = 0x7f,
TiledDigitalGain = 0x80,
AgcAecDesiredBin = 0xa5,
AecUpdate = 0xa6,
AecLowpass = 0xa8,
AgcUpdate = 0xa9,
AgcLowpass = 0xaa,
AgcMaxGain = 0xab,
MinExposure = 0xac,
MaxExposure = 0xad,
AecAgcEnable = 0xaf,
AgcAecPixelCount = 0xb0,
RegisterLock = 0xfe,
}
#[repr(u16)]
pub enum ParamContext {
ContextA = 0x0188,
ContextB = 0x8188,
}
#[repr(u8)]
pub enum ContextARegister {
ColumnStart = 0x01,
RowStart = 0x02,
WindowHeight = 0x03,
WindowWidth = 0x04,
HBlanking = 0x05,
VBlanking = 0x06,
CoarseShutter1 = 0x08,
CoarseShutter2 = 0x09,
CoarseShutterCtrl = 0x0A,
CoarseShutterTotal = 0x0B,
ReadMode = 0x0D,
V1Ctrl = 0x31,
V2Ctrl = 0x32,
V3Ctrl = 0x33,
V4Ctrl = 0x34,
AnalogGainCtrl = 0x35,
FineShutter1 = 0xD3,
FineShutter2 = 0xD4,
FineShutterTotal = 0xD5,
}
#[repr(u8)]
pub enum ContextBRegister {
ColumnStart = 0xC9,
RowStart = 0xCA,
WindowHeight = 0xCB,
WindowWidth = 0xCC,
HBlanking = 0xCD,
VBlanking = 0xCE,
CoarseShutter1 = 0xCF,
CoarseShutter2 = 0xD0,
CoarseShutterCtrl = 0xD1,
CoarseShutterTotal = 0xD2,
ReadMode = 0x0E,
V1Ctrl = 0x39,
V2Ctrl = 0x3A,
V3Ctrl = 0x3B,
V4Ctrl = 0x3C,
AnalogGainCtrl = 0x36,
FineShutter1 = 0xD6,
FineShutter2 = 0xD7,
FineShutterTotal = 0xD8,
}
#[repr(u16)]
pub enum PixelTestPattern {
None = 0x0000,
VerticalShade = 0x0800,
HorizontalShade = 0x1000,
DiagonalShade = 0x1800,
}
#[repr(u16)]
#[derive(Copy, Clone, Debug)]
pub enum BinningFactor {
None = 1,
Two = 2,
Four = 4,
}
#[repr(u8)]
#[derive(Copy, Clone, Debug)]
enum BinningSelector {
None = 0b00,
Two = 0b01,
Four = 0b10,
}
fn binning_factor_to_selector(factor: BinningFactor) -> BinningSelector {
match factor {
BinningFactor::None => BinningSelector::None,
BinningFactor::Two => BinningSelector::Two,
BinningFactor::Four => BinningSelector::Four,
}
}