41#ifndef MT9F002_I2C_TRIES
42#define MT9F002_I2C_TRIES 3
55 for (
uint16_t i = 0; i < len_r; i++) {
67 t->
buf[1] = reg & 0xFF;
75 fprintf(stderr,
"[%s] I2C %s of 0x%04x failed (status %d, try %d of %d)\n", tag,
83#include <linux/i2c-dev.h>
84#include <linux/videodev2.h>
86#include "generated/airframe.h"
93#define PRINT(string,...) fprintf(stderr, "[MT9F002->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)
96#define VERBOSE_PRINT PRINT
98#define VERBOSE_PRINT(...)
113#ifndef MT9F002_TARGET_EXPOSURE
114#define MT9F002_TARGET_EXPOSURE 30
118#ifndef MT9F002_GAIN_GREEN1
119#define MT9F002_GAIN_GREEN1 2.0
122#ifndef MT9F002_GAIN_GREEN2
123#define MT9F002_GAIN_GREEN2 2.0
126#ifndef MT9F002_GAIN_RED
127#define MT9F002_GAIN_RED 1.4
130#ifndef MT9F002_GAIN_BLUE
131#define MT9F002_GAIN_BLUE 2.7
150 .dev_name =
"/dev/video1",
151 .subdev_name =
"/dev/v4l-subdev1",
152 .format = V4L2_PIX_FMT_UYVY,
153 .subdev_format = V4L2_MBUS_FMT_SGRBG10_1X10,
158 .camera_intrinsics = {
171 .input_clk_freq = (26 / 2),
229 }
else if (len == 2) {
232 }
else if (len == 4) {
238 PRINT(
"Write_reg with incorrect length %d\r\n", len);
258 for (
uint8_t i = 0; i < len; i++) {
274 serialFormat = (3 << 8) | 2;
276 serialFormat = (2 << 8) | 2;
691 row_speed = (row_speed & 0xFFF8) | (mt->
rowSpeed_2_0 & 0x07);
692 row_speed = (row_speed & 0xF8FF) | ((mt->
row_speed_10_8 & 0x07) << 8);
693 row_speed = (row_speed & (~0x70)) | (0x2 << 4);
741 float subsamplingX_factor = (float)(1 + mt->
x_odd_inc) / 2.0f;
742 float subsamplingY_factor = (float)(1 + mt->
y_odd_inc) / 2.0f;
750 min_line_length = Max(min_line_length,
mt9f002_blanking.min_line_length_pck + 1);
755 min_line_length = Max(min_line_length,
759 min_line_length = Max(min_line_length,
769 clkRatio_num = clkRatio_num / clkRatio_gcd;
770 clkRatio_den = clkRatio_den / clkRatio_gcd;
773 uint32_t min_horizontal_blanking = clkRatio_num;
774 if ((clkRatio_den % 2) != 0) {
775 min_horizontal_blanking *= 2;
779 if ((min_line_length % min_horizontal_blanking) != 0) {
780 min_line_length += min_horizontal_blanking - (min_line_length % min_horizontal_blanking);
789 uint16_t min_frame_length = (y_addr_end - y_addr_start + 1) / subsamplingY_factor + min_frame_blanking_lines;
805 new_fps = mt->
vt_pix_clk * 1000000 / (float)(ll * fl);
815 if (fps_err < min_fps_err) {
816 min_fps_err = fps_err;
824 new_fps = mt->
vt_pix_clk * 1000000 / (float)(ll * min_frame_length);
859 if (fine_integration_min > fine_integration || fine_integration > fine_integration_max) {
860 int32_t upper_coarse_integration = coarse_integration + 1;
861 int32_t upper_fine_integration = fine_integration_min;
863 int32_t lower_coarse_integration = coarse_integration - 1;
864 int32_t lower_fine_integration = fine_integration_max;
867 if (lower_coarse_integration < coarse_integration_min) {
868 coarse_integration = upper_coarse_integration;
869 fine_integration = upper_fine_integration;
872 else if (upper_coarse_integration > coarse_integration_max) {
873 coarse_integration = lower_coarse_integration;
874 fine_integration = lower_fine_integration;
882 if (upper_error < lower_error) {
883 coarse_integration = upper_coarse_integration;
884 fine_integration = upper_fine_integration;
886 coarse_integration = lower_coarse_integration;
887 fine_integration = lower_fine_integration;
893 Bound(fine_integration, fine_integration_min, fine_integration_max);
894 Bound(coarse_integration, coarse_integration_min, coarse_integration_max);
913 uint8_t colamp_gain, analog_gain3, digital_gain;
919 }
else if (gain < 3.0) {
923 }
else if (gain < 6.0) {
927 }
else if (gain < 16.0) {
931 }
else if (gain < 32.0) {
942 uint16_t analog_gain2 = gain / (float)digital_gain / (
float)(1 << colamp_gain) / (
float)(1 << analog_gain3) * 64.0;
943 Bound(analog_gain2, 1, 127);
945 return (analog_gain2 & 0x7F) | ((analog_gain3 & 0x7) << 7) | ((colamp_gain & 0x3) << 10) | ((digital_gain & 0xF) << 12);
967 struct v4l2_rect crop, rect;
968 unsigned int x_odd_inc, y_odd_inc;
969 unsigned int hratio, vratio;
970 unsigned int width, height;
972 unsigned int xMultiple;
973 unsigned int div_res;
974 static const uint8_t xy_odd_inc_tab[] = {1, 1, 3, 3, 7, 7, 7, 7, 15};
982 VERBOSE_PRINT(
"Requested crop - top: %i, left: %i, width: %i, height: %i\n", crop.top, crop.left, crop.width,
996 div_res =
Min(rect.width / width, rect.height / height);
997 div_res = Clip(div_res, 1, 4);
998 x_odd_inc = xy_odd_inc_tab[div_res];
999 y_odd_inc = xy_odd_inc_tab[div_res];
1003 vratio = (rect.height / ((y_odd_inc + 1) / 2) *
MT9F002_SCALER_N) / height;
1004 ratio =
Min(hratio, vratio);
1015 VERBOSE_PRINT(
"Calculated skipping - x: %i, y: %i\n", x_odd_inc, y_odd_inc);
1019 rect.left = crop.left + ((
int32_t)crop.width - (
int32_t)rect.width) / 2;
1020 rect.top = crop.top + ((
int32_t)crop.height - (
int32_t)rect.height) / 2;
1028 xMultiple = 8 * ((x_odd_inc + 1) / 2);
1029 rect.left = Align(rect.left, (
int32_t)xMultiple);
1032 rect.top = Align(rect.top, 4);
1036 VERBOSE_PRINT(
"Granted crop - top: %i, left: %i, width: %i, height: %i\n", crop.top, crop.left, crop.width,
1038 VERBOSE_PRINT(
"Granted output - width: %i, height: %i\n", width, height);
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
enum I2CTransactionStatus status
Transaction status.
I2CTransactionStatus
I2C transaction status.
enum I2CTransactionStatus i2c_blocking_transmit(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len, float timeout)
Submit a write only transaction and wait for it to complete.
enum I2CTransactionStatus i2c_blocking_transceive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len_w, uint16_t len_r, float timeout)
Submit a write/read transaction and wait for it to complete.
@ I2CTransRunning
transaction is currently ongoing
@ I2CTransSuccess
transaction successfully finished by I2C driver
@ I2CTransDone
transaction set to done by user level
@ I2CTransPending
transaction is pending in queue
I2C transaction structure.
uint32_t int32_gcd(uint32_t a, uint32_t b)
int isp_request_statistics_yuv_window(uint16_t x_start, uint16_t x_end, uint16_t y_start, uint16_t y_end, uint16_t x_odd_inc, uint16_t y_odd_inc)
#define VERBOSE_PRINT(...)
static void mt9f002_parallel_stage1(struct mt9f002_t *mt)
Configure stage 1 for parallel connection.
void mt9f002_reset_exposure(struct mt9f002_t *mt)
static uint16_t mt9f002_calc_gain(float gain)
Calculate the gain based on value of 1.0 -> 63.50.
static void mt9f002_mipi_stage1(struct mt9f002_t *mt)
Configure stage 1 for both MiPi and HiSPi connection.
uint16_t min_line_blanking_pck
void mt9f002_set_gains(struct mt9f002_t *mt)
Sets the GreenR, Blue, Red and GreenB gains.
static void mt9f002_mipi_stage3(struct mt9f002_t *mt)
Configure stage 3 for both MiPi and HiSPi connection.
static void mt9f002_parallel_stage2(struct mt9f002_t *mt)
Configure stage 2 for parallel connection.
static void mt9f002_set_blanking(struct mt9f002_t *mt)
uint16_t fine_integration_time_max_margin
uint16_t min_line_length_pck
#define MT9F002_GAIN_BLUE
#define MT9F002_GAIN_GREEN1
uint16_t fine_integration_time_min
static uint32_t read_reg(struct mt9f002_t *mt, uint16_t addr, uint8_t len)
Read multiple bytes from a register.
void mt9f002_init(struct mt9f002_t *mt)
Initialisation of the Aptina MT9F002 CMOS sensor (front camera).
static void mt9f002_set_pll(struct mt9f002_t *mt)
Set the PLL registers based on config.
static void mt9f002_calc_resolution(struct mt9f002_t *mt)
struct blanking_t mt9f002_blanking
float mt9f002_send_resolution
static void mt9f002_mipi_stage2(struct mt9f002_t *mt)
Configure stage 2 for both MiPi and HiSPi connection.
#define MT9F002_TARGET_EXPOSURE
Exposure of the front camera of the bebop.
void mt9f002_setting_update_resolution(float in UNUSED)
static void write_reg(struct mt9f002_t *mt, uint16_t addr, uint32_t val, uint8_t len)
Write multiple bytes to a single register.
static bool i2c_checked(struct i2c_periph *p, struct i2c_transaction *t, uint8_t slave, uint16_t reg, uint8_t len_w, uint16_t len_r, float timeout, const char *tag)
Blocking I2C transaction with result check and retry.
void mt9f002_setting_update_exposure(float in UNUSED)
void mt9f002_set_exposure(struct mt9f002_t *mt)
Set the exposure configuration Depends on the blanking (and therefore the FPS).
float mt9f002_send_exposure
void mt9f002_reset_color(struct mt9f002_t *mt)
#define MT9F002_GAIN_GREEN2
#define MT9F002_I2C_TRIES
static void mt9f002_set_resolution(struct mt9f002_t *mt)
uint16_t min_line_fifo_pck
#define PRINT(string,...)
void mt9f002_setting_update_color(float in UNUSED)
Initialization and configuration of the MT9F002 CMOS Chip.
uint16_t op_sys_clk_div
Fixed PLL config from calculator tool.
float target_exposure
Target exposure time in ms.
enum mt9f002_interface interface
Interface used to connect.
uint16_t op_pix_clk_div
Fixed PLL config from calculator tool.
uint8_t x_odd_inc
X increment for subsampling (1,3,7,15,31 accepted).
float input_clk_freq
Input clock frequency.
#define CFG_MT9F002_X_ADDR_MIN
#define CFG_MT9F002_Y_ADDR_MIN
float gain_red
Gain for the Red pixels [1., 63.50].
uint16_t offset_x
Offset from left in pixels.
#define CFG_MT9F002_WINDOW_HEIGHT_MIN
float gain_green2
Gain for the GreenB pixels [1., 63.50].
float target_fps
FPS wanted.
uint16_t output_width
Output width.
#define CFG_MT9F002_PIXEL_ARRAY_WIDTH
float vt_pix_clk
Calculated based on PLL.
float gain_green1
Gain for the GreenR pixels [1., 63.50].
uint8_t shift_vt_pix_clk_div
Fixed PLL config from calculator tool.
uint16_t frame_length
Calculated frame length of blanking.
@ MT9F002_PARALLEL
Parallel type connection.
@ MT9F002_HiSPi
HiSPi type connection.
@ MT9F002_MIPI
MIPI type connection.
uint16_t pre_pll_clk_div
Fixed PLL config from calculator tool.
uint16_t output_height
Output height.
uint8_t row_speed_10_8
Fixed PLL config from calculator tool.
float op_pix_clk
Calculated based on PLL.
uint8_t rowSpeed_2_0
Fixed PLL config from calculator tool.
uint16_t offset_y
Offset from top in pixels.
uint8_t y_odd_inc
Y increment for subsampling (1,3,7,15,31 accepted).
uint16_t vt_sys_clk_div
Fixed PLL config from calculator tool.
float gain_blue
Gain for the Blue pixels [1., 63.50].
#define MT9F002_OUTPUT_WIDTH
#define CFG_MT9F002_WINDOW_WIDTH_MIN
#define CFG_MT9F002_PIXEL_ARRAY_HEIGHT
uint16_t output_scaler
Output scaler.
uint16_t vt_pix_clk_div
Fixed PLL config from calculator tool.
struct i2c_periph * i2c_periph
I2C peripheral used to communicate over.
#define CFG_MT9F002_Y_ADDR_MAX
#define CFG_MT9F002_X_ADDR_MAX
uint16_t pll_multiplier
Fixed PLL config from calculator tool.
float real_exposure
Real exposure time in ms.
uint16_t line_length
Calculated line length of blanking.
struct i2c_transaction i2c_trans
I2C transaction for communication with CMOS chip.
#define MT9F002_TARGET_FPS
#define MT9F002_OUTPUT_HEIGHT
float real_fps
Real calculated FPS.
#define MT9F002_GREEN1_GAIN
#define MT9F002_COLUMN_CORRECTION
#define MT9F002_OP_PIX_CLK_DIV
#define MT9F002_DATA_PEDESTAL_
#define MT9F002_X_OUTPUT_SIZE
#define MT9F002_DATAPATH_SELECT
#define MT9F002_COARSE_INTEGRATION_TIME_MAX_MARGIN
#define MT9F002_EXTRA_DELAY
#define MT9F002_PRE_PLL_CLK_DIV
#define MT9F002_ROW_SPEED
#define MT9F002_CPP_DATA_FORMAT
#define MT9F002_OP_SYS_CLK_DIV
#define MT9F002_SCALING_MODE
#define MT9F002_GLOBAL_GAIN
#define MT9F002_GREEN2_GAIN
#define MT9F002_RESET_REGISTER
#define MT9F002_DAC_LD_14_15
#define MT9F002_DAC_LD_24_25
#define MT9F002_X_ADDR_END
#define MT9F002_Y_ODD_INC
#define MT9F002_FRAME_LENGTH_MAX
#define MT9F002_DIGITAL_TEST
#define MT9F002_CTX_WR_DATA_REG
#define MT9F002_X_ADDR_START
#define MT9F002_READ_MODE
#define MT9F002_DAC_LD_28_29
#define MT9F002_ANALOG_CONTROL5
#define MT9F002_FINE_INTEGRATION_TIME_
#define MT9F002_SMIA_TEST
#define MT9F002_LINE_LENGTH_PCK
#define MT9F002_FRAME_LENGTH_LINES
#define MT9F002_SERIAL_FORMAT
#define MT9F002_ANALOG_CONTROL7
#define MT9F002_VT_SYS_CLK_DIV
#define MT9F002_BLUE_GAIN
#define MT9F002_ANALOG_CONTROL4
#define MT9F002_MIN_FRAME_BLANKING_LINES
#define MT9F002_VT_PIX_CLK_DIV
#define MT9F002_MODE_SELECT
#define MT9F002_Y_ADDR_END
#define MT9F002_PLL_MULTIPLIER
#define MT9F002_DAC_LD_30_31
#define MT9F002_DARK_CONTROL3
#define MT9F002_MASK_CORRUPTED_FRAMES
#define MT9F002_COARSE_INTEGRATION_TIME_MIN
#define MT9F002_Y_ADDR_START
#define MT9F002_SOFTWARE_RESET
#define MT9F002_Y_OUTPUT_SIZE
#define MT9F002_CTX_CONTROL_REG
#define MT9F002_X_ODD_INC
#define MT9F002_COARSE_INTEGRATION_TIME
#define MT9F002_LINE_LENGTH_MAX
Paparazzi fixed point algebra.
#define VIDEO_FILTER_ISP
Enable ISP.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.