1#![no_std]
7
8#[cfg(feature = "rttdebug")]
16use panic_rtt_core::rprintln;
17
18#[derive(Debug)]
20pub enum Error<CommE> {
21 Comm(CommE),
23
24 Timeout,
26}
27
28pub const CAM_PERIPH_ADDRESS_00: u8 = 0x90 >> 1;
32pub const CAM_PERIPH_ADDRESS_01: u8 = 0x98 >> 1;
33pub const CAM_PERIPH_ADDRESS_10: u8 = 0xB0 >> 1;
34pub const CAM_PERIPH_ADDRESS_11: u8 = 0xB8 >> 1;
35
36pub const PX4FLOW_CAM_ADDRESS: u8 = CAM_PERIPH_ADDRESS_11;
38
39pub const ARDUCAM_BREAKOUT_ADDRESS: u8 = CAM_PERIPH_ADDRESS_00;
41
42pub struct Mt9v034<I2C> {
44 base_address: u8,
45 i2c: I2C,
46 win_width_a: u16,
47 win_height_a: u16,
48 win_width_b: u16,
49 win_height_b: u16,
50 col_bin_factor_a: BinningFactor,
51 row_bin_factor_a: BinningFactor,
52 col_bin_factor_b: BinningFactor,
53 row_bin_factor_b: BinningFactor,
54}
55
56impl<I2C, CommE> Mt9v034<I2C>
57where
58 I2C: embedded_hal::blocking::i2c::Write<Error = CommE>
59 + embedded_hal::blocking::i2c::Read<Error = CommE>
60 + embedded_hal::blocking::i2c::WriteRead<Error = CommE>,
61{
62 pub fn new(i2c: I2C, address: u8) -> Self {
64 Self {
65 base_address: address,
66 i2c,
67 win_width_a: 0,
68 win_height_a: 0,
69 win_width_b: 0,
70 win_height_b: 0,
71 col_bin_factor_a: BinningFactor::None,
72 row_bin_factor_a: BinningFactor::None,
73 col_bin_factor_b: BinningFactor::None,
74 row_bin_factor_b: BinningFactor::None,
75 }
76 }
77
78 pub fn set_context(
86 &mut self,
87 context: ParamContext,
88 ) -> Result<(), crate::Error<CommE>> {
89 self.write_general_reg(GeneralRegister::Control, context as u16)
90 }
91
92 pub fn setup(&mut self) -> Result<(), crate::Error<CommE>> {
94 #[cfg(feature = "rttdebug")]
95 rprintln!("mt9v034-i2c setup start 0x{:x}", self.base_address);
96
97 let _version = self.read_reg_u16(GeneralRegister::ChipVersion as u8)?;
99
100 self.set_context_b_default_dimensions()?;
102 self.set_context_b_shutter_defaults()?;
103 self.set_context_a_default_dimensions()?;
104 self.set_context_a_shutter_defaults()?;
105
106 self.set_general_defaults(4096)?;
109
110 self.set_context(ParamContext::ContextA)?;
112
113 self.write_general_reg(GeneralRegister::SoftReset, 0x01)?;
115
116 let _verify_version =
117 self.read_reg_u16(GeneralRegister::ChipVersion as u8)?;
118 #[cfg(feature = "rttdebug")]
119 rprintln!("mt9v034-i2c setup done, vers: 0x{:x}", _verify_version);
120
121 Ok(())
122 }
123
124 pub fn setup_with_dimensions(
134 &mut self,
135 win_width_a: u16,
136 win_height_a: u16,
137 col_bin_a: BinningFactor,
138 row_bin_a: BinningFactor,
139 win_width_b: u16,
140 win_height_b: u16,
141 col_bin_b: BinningFactor,
142 row_bin_b: BinningFactor,
143 default_context: ParamContext,
144 ) -> Result<(), crate::Error<CommE>> {
145 #[cfg(feature = "rttdebug")]
146 rprintln!("mt9v034-i2c setup start 0x{:x}", self.base_address);
147
148 let _version = self.read_reg_u16(GeneralRegister::ChipVersion as u8)?;
152
153 self.set_context_dimensions(
154 ParamContext::ContextB,
155 win_height_b,
156 win_width_b,
157 col_bin_b,
158 row_bin_b,
159 )?;
160
161 self.set_context_b_shutter_defaults()?;
162 self.set_context_dimensions(
163 ParamContext::ContextA,
164 win_height_a,
165 win_width_a,
166 col_bin_a,
167 row_bin_a,
168 )?;
169 self.set_context_a_shutter_defaults()?;
170
171 let max_pixels = match default_context {
172 ParamContext::ContextA => {
173 (self.win_height_a / self.row_bin_factor_a as u16)
174 * (self.win_width_a / self.col_bin_factor_a as u16)
175 }
176 ParamContext::ContextB => {
177 (self.win_height_b / self.row_bin_factor_b as u16)
178 * (self.win_width_b / self.col_bin_factor_b as u16)
179 }
180 };
181 self.set_general_defaults(max_pixels as u32)?;
183
184 self.set_context(default_context)?;
186
187 self.write_general_reg(GeneralRegister::SoftReset, 0x01)?;
189
190 let _verify_version =
191 self.read_reg_u16(GeneralRegister::ChipVersion as u8)?;
192 #[cfg(feature = "rttdebug")]
193 rprintln!("mt9v034-i2c setup done, vers: 0x{:x}", _verify_version);
194
195 Ok(())
196 }
197
198 fn write_general_reg(
199 &mut self,
200 reg: GeneralRegister,
201 data: u16,
202 ) -> Result<(), crate::Error<CommE>> {
203 self.write_reg_u16(reg as u8, data)
204 }
205
206 fn write_context_a_reg(
207 &mut self,
208 reg: ContextARegister,
209 data: u16,
210 ) -> Result<(), crate::Error<CommE>> {
211 self.write_reg_u16(reg as u8, data)
212 }
213
214 fn write_context_b_reg(
215 &mut self,
216 reg: ContextBRegister,
217 data: u16,
218 ) -> Result<(), crate::Error<CommE>> {
219 self.write_reg_u16(reg as u8, data)
220 }
221
222 pub fn set_agc_pixel_count(
225 &mut self,
226 max_pixels: u32,
227 ) -> Result<(), crate::Error<CommE>> {
228 let agc_pixels: u16 = if max_pixels > 65535 {
229 65535
230 } else {
231 max_pixels as u16
232 };
233 self.write_general_reg(GeneralRegister::AgcAecPixelCount, agc_pixels)
234 }
235
236 pub fn set_general_defaults(
239 &mut self,
240 max_pixel_count: u32,
241 ) -> Result<(), crate::Error<CommE>> {
242 self.write_reg_u8(GeneralRegister::RowNoiseConstant as u8, 0x00)?;
243
244 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)?;
254
255 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)?;
259 self.write_general_reg(GeneralRegister::MaxExposure, 0x1F4)?;
260
261 self.write_general_reg(GeneralRegister::AgcMaxGain, 0x0010)?;
262 self.set_agc_pixel_count(max_pixel_count)?;
263 self.write_general_reg(GeneralRegister::AgcAecDesiredBin, 20)?; self.write_general_reg(GeneralRegister::AdcResCtrl, 0x0303)?; self.write_general_reg(GeneralRegister::AecUpdate, 0x02)?;
267 self.write_general_reg(GeneralRegister::AecLowpass, 0x01)?;
268 self.write_general_reg(GeneralRegister::AgcUpdate, 0x02)?;
269 self.write_general_reg(GeneralRegister::AgcLowpass, 0x02)?;
270
271 Ok(())
272 }
273
274 pub fn set_context_b_default_dimensions(
276 &mut self,
277 ) -> Result<(), crate::Error<CommE>> {
278 const VIDEO_IMG_HEIGHT: u16 = 480 / 2;
280 const VIDEO_IMG_WIDTH: u16 = 752 / 2;
281 const COLUMN_BINNING: BinningFactor = BinningFactor::Two;
282 const ROW_BINNING: BinningFactor = BinningFactor::Two;
283 const WINDOW_W: u16 = VIDEO_IMG_WIDTH * 2;
284 const WINDOW_H: u16 = VIDEO_IMG_HEIGHT * 2;
285
286 self.set_context_dimensions(
287 ParamContext::ContextB,
288 WINDOW_H,
289 WINDOW_W,
290 COLUMN_BINNING,
291 ROW_BINNING,
292 )
293 }
294
295 pub fn set_context_a_default_dimensions(
297 &mut self,
298 ) -> Result<(), crate::Error<CommE>> {
299 const FLOW_IMG_HEIGHT: u16 = 64;
300 const FLOW_IMG_WIDTH: u16 = 64;
301 const COLUMN_BINNING: BinningFactor = BinningFactor::Four;
302 const ROW_BINNING: BinningFactor = BinningFactor::Four;
303 const WINDOW_W: u16 = FLOW_IMG_WIDTH * 4;
304 const WINDOW_H: u16 = FLOW_IMG_HEIGHT * 4;
305
306 self.set_context_dimensions(
307 ParamContext::ContextA,
308 WINDOW_H,
309 WINDOW_W,
310 COLUMN_BINNING,
311 ROW_BINNING,
312 )
313 }
314
315 pub fn set_context_dimensions(
321 &mut self,
322 context: ParamContext,
323 window_h: u16,
324 window_w: u16,
325 col_bin_factor: BinningFactor,
326 row_bin_factor: BinningFactor,
327 ) -> Result<(), crate::Error<CommE>> {
328 let min_h_blank: u16 = match col_bin_factor {
338 BinningFactor::None => 61,
339 BinningFactor::Two => 71,
340 BinningFactor::Four => 91,
341 };
342
343 let col_binning = binning_factor_to_selector(col_bin_factor);
344 let row_binning = binning_factor_to_selector(row_bin_factor);
345
346 let h_blank: u16 = 425 + min_h_blank;
350 const V_BLANK: u16 = 10;
351
352 const MIN_COL_START: u16 = 1;
353 const MIN_ROW_START: u16 = 4; let col_start: u16 = (MAX_FRAME_WIDTH - window_w) / 2 + MIN_COL_START;
356 let row_start: u16 = (MAX_FRAME_HEIGHT - window_h) / 2 + MIN_ROW_START;
358
359 let read_mode: u16 =
362 0x300 | ((col_binning as u16) << 2) | (row_binning as u16);
363
364 match context {
365 ParamContext::ContextA => {
366 self.win_width_a = window_w;
367 self.win_height_a = window_h;
368 self.col_bin_factor_a = col_bin_factor;
369 self.row_bin_factor_a = row_bin_factor;
370 self.write_context_a_reg(
371 ContextARegister::WindowWidth,
372 window_w,
373 )?;
374 self.write_context_a_reg(
375 ContextARegister::WindowHeight,
376 window_h,
377 )?;
378 self.write_context_a_reg(ContextARegister::HBlanking, h_blank)?;
379 self.write_context_a_reg(ContextARegister::VBlanking, V_BLANK)?;
380 self.write_context_a_reg(
381 ContextARegister::ReadMode,
382 read_mode,
383 )?;
384 self.write_context_a_reg(
385 ContextARegister::ColumnStart,
386 col_start,
387 )?;
388 self.write_context_a_reg(
389 ContextARegister::RowStart,
390 row_start,
391 )?;
392 }
393 ParamContext::ContextB => {
394 self.win_width_b = window_w;
395 self.win_height_b = window_h;
396 self.col_bin_factor_b = col_bin_factor;
397 self.row_bin_factor_b = row_bin_factor;
398 self.write_context_b_reg(
399 ContextBRegister::WindowWidth,
400 window_w,
401 )?;
402 self.write_context_b_reg(
403 ContextBRegister::WindowHeight,
404 window_h,
405 )?;
406 self.write_context_b_reg(ContextBRegister::HBlanking, h_blank)?;
407 self.write_context_b_reg(ContextBRegister::VBlanking, V_BLANK)?;
408 self.write_context_b_reg(
409 ContextBRegister::ReadMode,
410 read_mode,
411 )?;
412 self.write_context_b_reg(
413 ContextBRegister::ColumnStart,
414 col_start,
415 )?;
416 self.write_context_b_reg(
417 ContextBRegister::RowStart,
418 row_start,
419 )?;
420 }
421 }
422
423 Ok(())
424 }
425
426 pub fn set_context_a_shutter_defaults(
428 &mut self,
429 ) -> Result<(), crate::Error<CommE>> {
430 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(())
437 }
438
439 pub fn set_context_b_shutter_defaults(
441 &mut self,
442 ) -> Result<(), crate::Error<CommE>> {
443 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(())
450 }
451
452 #[cfg(feature = "rttdebug")]
453 pub fn dump_context_a_settings(
454 &mut self,
455 ) -> Result<(), crate::Error<CommE>> {
456 rprintln!("-- Context A settings:");
457 self.dump_register_setting(ContextARegister::WindowWidth as u8)?;
458 self.dump_register_setting(ContextARegister::WindowHeight as u8)?;
459 self.dump_register_setting(ContextARegister::HBlanking as u8)?;
460 self.dump_register_setting(ContextARegister::VBlanking as u8)?;
461 self.dump_register_setting(ContextARegister::ReadMode as u8)?;
462 self.dump_register_setting(ContextARegister::ColumnStart as u8)?;
463 self.dump_register_setting(ContextARegister::RowStart as u8)?;
464 self.dump_register_setting(ContextARegister::CoarseShutter1 as u8)?;
465 self.dump_register_setting(ContextARegister::CoarseShutter2 as u8)?;
466 self.dump_register_setting(ContextARegister::CoarseShutterCtrl as u8)?;
467 self.dump_register_setting(ContextARegister::CoarseShutterTotal as u8)?;
468 Ok(())
469 }
470
471 #[cfg(feature = "rttdebug")]
472 pub fn dump_context_b_settings(
473 &mut self,
474 ) -> Result<(), crate::Error<CommE>> {
475 rprintln!("-- Context B settings:");
476 self.dump_register_setting(ContextBRegister::WindowWidth as u8)?;
477 self.dump_register_setting(ContextBRegister::WindowHeight as u8)?;
478 self.dump_register_setting(ContextBRegister::HBlanking as u8)?;
479 self.dump_register_setting(ContextBRegister::VBlanking as u8)?;
480 self.dump_register_setting(ContextBRegister::ReadMode as u8)?;
481 self.dump_register_setting(ContextBRegister::ColumnStart as u8)?;
482 self.dump_register_setting(ContextBRegister::RowStart as u8)?;
483 self.dump_register_setting(ContextBRegister::CoarseShutter1 as u8)?;
484 self.dump_register_setting(ContextBRegister::CoarseShutter2 as u8)?;
485 self.dump_register_setting(ContextBRegister::CoarseShutterCtrl as u8)?;
486 self.dump_register_setting(ContextBRegister::CoarseShutterTotal as u8)?;
487 Ok(())
488 }
489
490 #[cfg(feature = "rttdebug")]
491 pub fn dump_general_settings(&mut self) -> Result<(), crate::Error<CommE>> {
492 rprintln!("-- General settings:");
493 self.dump_register_setting(GeneralRegister::Control as u8)?;
494 self.dump_register_setting(GeneralRegister::RowNoiseConstant as u8)?;
495
496 self.dump_register_setting(0x13)?;
498 self.dump_register_setting(0x20)?;
499 self.dump_register_setting(0x24)?;
500 self.dump_register_setting(0x2B)?;
501 self.dump_register_setting(0x2F)?;
502
503 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)?;
509 self.dump_register_setting(GeneralRegister::MaxExposure as u8)?;
510
511 self.dump_register_setting(GeneralRegister::AgcMaxGain as u8)?;
512 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)?;
517 self.dump_register_setting(GeneralRegister::AecLowpass as u8)?;
518 self.dump_register_setting(GeneralRegister::AgcUpdate as u8)?;
519 self.dump_register_setting(GeneralRegister::AgcLowpass as u8)?;
520 Ok(())
521 }
522
523 #[cfg(feature = "rttdebug")]
524 pub fn dump_register_setting(
525 &mut self,
526 reg: u8,
527 ) -> Result<(), crate::Error<CommE>> {
528 let val = self.read_reg_u16(reg)?;
529 rprintln!("0x{:X} = 0x{:x} {}", reg, val, val);
530 Ok(())
531 }
532
533 pub fn enable_pixel_test_pattern(
535 &mut self,
536 enable: bool,
537 pattern: PixelTestPattern,
538 ) -> Result<(), crate::Error<CommE>> {
539 if enable {
540 self.write_general_reg(
541 GeneralRegister::TestPattern,
542 (pattern as u16) | 0x2000,
543 )?;
544 self.write_general_reg(GeneralRegister::RowNoiseCorrCtrl, 0x0000)?;
546 } else {
547 self.write_general_reg(GeneralRegister::TestPattern, 0x0000)?;
549 self.write_general_reg(GeneralRegister::RowNoiseCorrCtrl, 0x0101)?;
551 }
552 Ok(())
553 }
554
555 pub fn protect_all_registers(
560 &mut self,
561 protect: bool,
562 ) -> Result<(), crate::Error<CommE>> {
563 self.write_general_reg(
564 GeneralRegister::RegisterLock,
565 if protect { 0xDEAD } else { 0xBEEF },
566 )
567 }
568
569 pub fn read_reg_u8(&mut self, reg: u8) -> Result<u8, crate::Error<CommE>> {
571 let cmd_buf = [reg];
573 let mut recv_buf = [0u8];
574 self.i2c
575 .write_read(self.base_address, &cmd_buf, &mut recv_buf)
576 .map_err(Error::Comm)?;
577
578 Ok(recv_buf[0])
579 }
580
581 pub fn read_reg_u16(
583 &mut self,
584 reg: u8,
585 ) -> Result<u16, crate::Error<CommE>> {
586 let upper = (self.read_reg_u8(reg)? as u16) << 8;
588 let lower = self.read_reg_u8(FOLLOW_UP_ADDRESS)? as u16;
589 Ok(upper | lower)
590 }
591
592 pub fn write_reg_u8(
594 &mut self,
595 reg: u8,
596 val: u8,
597 ) -> Result<(), crate::Error<CommE>> {
598 let write_buf = [reg, val];
599 self.i2c
600 .write(self.base_address, &write_buf)
601 .map_err(Error::Comm)?;
602 Ok(())
603 }
604
605 pub fn write_reg_u16(
607 &mut self,
608 reg: u8,
609 data: u16,
610 ) -> Result<(), crate::Error<CommE>> {
611 self.write_reg_u8(reg, (data >> 8) as u8)?;
613 self.write_reg_u8(FOLLOW_UP_ADDRESS, (data & 0xFF) as u8)?;
615 Ok(())
616 }
617}
618
619const FOLLOW_UP_ADDRESS: u8 = 0xF0;
626
627pub const MAX_FRAME_HEIGHT: u16 = 480;
631pub const MAX_FRAME_WIDTH: u16 = 752;
633
634#[repr(u8)]
635pub enum GeneralRegister {
636 ChipVersion = 0x00,
637 Control = 0x07,
639 SoftReset = 0x0c,
641 HdrEnable = 0x0f,
643 AdcResCtrl = 0x1c,
645 RowNoiseCorrCtrl = 0x70,
647 RowNoiseConstant = 0x72,
649 TestPattern = 0x7f,
651 TiledDigitalGain = 0x80,
653 AgcAecDesiredBin = 0xa5,
655 AecUpdate = 0xa6,
657 AecLowpass = 0xa8,
659 AgcUpdate = 0xa9,
661 AgcLowpass = 0xaa,
663 AgcMaxGain = 0xab,
665 MinExposure = 0xac,
667 MaxExposure = 0xad,
669 AecAgcEnable = 0xaf,
671 AgcAecPixelCount = 0xb0,
673
674 RegisterLock = 0xfe,
676}
677
678#[repr(u16)]
680pub enum ParamContext {
681 ContextA = 0x0188,
682 ContextB = 0x8188,
683}
684
685#[repr(u8)]
686pub enum ContextARegister {
687 ColumnStart = 0x01,
688 RowStart = 0x02,
689 WindowHeight = 0x03,
690 WindowWidth = 0x04,
691 HBlanking = 0x05,
693 VBlanking = 0x06,
695 CoarseShutter1 = 0x08,
697 CoarseShutter2 = 0x09,
699 CoarseShutterCtrl = 0x0A,
701 CoarseShutterTotal = 0x0B,
703 ReadMode = 0x0D,
704 V1Ctrl = 0x31,
705 V2Ctrl = 0x32,
706 V3Ctrl = 0x33,
707 V4Ctrl = 0x34,
708 AnalogGainCtrl = 0x35,
710 FineShutter1 = 0xD3,
712 FineShutter2 = 0xD4,
714 FineShutterTotal = 0xD5,
716}
717
718#[repr(u8)]
719pub enum ContextBRegister {
720 ColumnStart = 0xC9,
721 RowStart = 0xCA,
722 WindowHeight = 0xCB,
723 WindowWidth = 0xCC,
724 HBlanking = 0xCD,
725 VBlanking = 0xCE,
726 CoarseShutter1 = 0xCF,
727 CoarseShutter2 = 0xD0,
728 CoarseShutterCtrl = 0xD1,
729 CoarseShutterTotal = 0xD2,
730 ReadMode = 0x0E,
731 V1Ctrl = 0x39,
732 V2Ctrl = 0x3A,
733 V3Ctrl = 0x3B,
734 V4Ctrl = 0x3C,
735 AnalogGainCtrl = 0x36,
736 FineShutter1 = 0xD6,
737 FineShutter2 = 0xD7,
738 FineShutterTotal = 0xD8,
739}
740
741#[repr(u16)]
743pub enum PixelTestPattern {
744 None = 0x0000,
745 VerticalShade = 0x0800,
746 HorizontalShade = 0x1000,
747 DiagonalShade = 0x1800,
748}
749
750#[repr(u16)]
751#[derive(Copy, Clone, Debug)]
752pub enum BinningFactor {
753 None = 1,
755 Two = 2,
757 Four = 4,
759}
760
761#[repr(u8)]
763#[derive(Copy, Clone, Debug)]
764enum BinningSelector {
765 None = 0b00,
767 Two = 0b01,
769 Four = 0b10,
771}
772
773fn binning_factor_to_selector(factor: BinningFactor) -> BinningSelector {
775 match factor {
776 BinningFactor::None => BinningSelector::None,
777 BinningFactor::Two => BinningSelector::Two,
778 BinningFactor::Four => BinningSelector::Four,
779 }
780}