Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
mt9f002.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 Freek van Tienen <freek.v.tienen@gmail.com>
3  *
4  * This file is part of Paparazzi.
5  *
6  * Paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * Paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  *
20  */
21 
27 #include "std.h"
28 #include "mt9f002.h"
29 #include "mt9f002_regs.h"
30 #include "isp/libisp.h"
31 #include "math/pprz_algebra_int.h"
33 
34 #include <stdio.h>
35 #include <unistd.h>
36 #include <fcntl.h>
37 #include <sys/ioctl.h>
38 #include <linux/i2c-dev.h>
39 #include <linux/videodev2.h>
40 
41 #include "generated/airframe.h"
42 #ifdef BOARD_DISCO
43 #include "boards/disco.h"
44 #else
45 #include "boards/bebop.h"
46 #endif
47 
48 #define PRINT(string,...) fprintf(stderr, "[MT9F002->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)
49 
50 #if MT9F002_VERBOSE
51 #define VERBOSE_PRINT PRINT
52 #else
53 #define VERBOSE_PRINT(...)
54 #endif
55 
56 // The sequencing of the pixel array is controlled by the x_addr_start, y_addr_start,
57 // x_addr_end, and y_addr_end registers. For both parallel and serial HiSPi interfaces, the
58 // output image size is controlled by the x_output_size and y_output_size registers.
59 
60 // Horizontal Mirror
61 // Vertical Flip
62 
68 #ifndef MT9F002_TARGET_EXPOSURE
69 #define MT9F002_TARGET_EXPOSURE 30
70 #endif
71 
72 /* Set the colour balance gains */
73 #ifndef MT9F002_GAIN_GREEN1
74 #define MT9F002_GAIN_GREEN1 2.0
75 #endif
76 
77 #ifndef MT9F002_GAIN_GREEN2
78 #define MT9F002_GAIN_GREEN2 2.0
79 #endif
80 
81 #ifndef MT9F002_GAIN_RED
82 #define MT9F002_GAIN_RED 1.4
83 #endif
84 
85 #ifndef MT9F002_GAIN_BLUE
86 #define MT9F002_GAIN_BLUE 2.7
87 #endif
88 
89 /* Camera structure */
91  .output_size = {
94  },
95  .sensor_size = {
98  },
99  .crop = {
100  .x = 0,
101  .y = 0,
104  },
105  .dev_name = "/dev/video1",
106  .subdev_name = "/dev/v4l-subdev1",
107  .format = V4L2_PIX_FMT_UYVY,
108  .subdev_format = V4L2_MBUS_FMT_SGRBG10_1X10,
109  .buf_cnt = 5,
110  .filters = VIDEO_FILTER_ISP,
111  .cv_listener = NULL,
112  .fps = MT9F002_TARGET_FPS,
113  .camera_intrinsics = {
114  .focal_x = MT9F002_FOCAL_X,
115  .focal_y = MT9F002_FOCAL_Y,
116  .center_x = MT9F002_CENTER_X,
117  .center_y = MT9F002_CENTER_Y,
118  .Dhane_k = MT9F002_DHANE_K,
119  }
120 };
121 
122 /* Initialize MT9F002 chipset (Front camera) */
123 struct mt9f002_t mt9f002 = {
124  // Precomputed values to go from InputCLK of (26/2)MHz to 96MH
126  .input_clk_freq = (26 / 2),
127  .vt_pix_clk_div = 7,
128  .vt_sys_clk_div = 1,
129  .pre_pll_clk_div = 1,
130  .pll_multiplier = 59,
131  .op_pix_clk_div = 8,
132  .op_sys_clk_div = 1,
134  .rowSpeed_2_0 = 1,
135  .row_speed_10_8 = 1,
136 
137  // Initial values
146  .output_scaler = 1,
153  .x_odd_inc = 1,
154  .y_odd_inc = 1,
155 
156  // I2C connection port
157  .i2c_periph = &i2c0,
158 
159  // settings
163 };
164 
165 struct blanking_t {
172 
176 static void write_reg(struct mt9f002_t *mt, uint16_t addr, uint32_t val, uint8_t len)
177 {
178  mt->i2c_trans.buf[0] = addr >> 8;
179  mt->i2c_trans.buf[1] = addr & 0xFF;
180 
181  // Fix signdness based on length
182  if (len == 1) {
183  mt->i2c_trans.buf[2] = val & 0xFF;
184  } else if (len == 2) {
185  mt->i2c_trans.buf[2] = (val >> 8) & 0xFF;
186  mt->i2c_trans.buf[3] = val & 0xFF;
187  } else if (len == 4) {
188  mt->i2c_trans.buf[2] = (val >> 24) & 0xFF;
189  mt->i2c_trans.buf[3] = (val >> 16) & 0xFF;
190  mt->i2c_trans.buf[4] = (val >> 8) & 0xFF;
191  mt->i2c_trans.buf[5] = val & 0xFF;
192  } else {
193  PRINT("Write_reg with incorrect length %d\r\n", len);
194  }
195 
196  // Transmit the buffer
198 }
199 
203 static uint32_t read_reg(struct mt9f002_t *mt, uint16_t addr, uint8_t len)
204 {
205  uint32_t ret = 0;
206  mt->i2c_trans.buf[0] = addr >> 8;
207  mt->i2c_trans.buf[1] = addr & 0xFF;
208 
209  // Transmit the buffer and receive back
211 
212  /* Fix signdness */
213  for (uint8_t i = 0; i < len; i++) {
214  ret |= mt->i2c_trans.buf[len - i - 1] << (8 * i);
215  }
216  return ret;
217 }
218 
222 static inline void mt9f002_mipi_stage1(struct mt9f002_t *mt)
223 {
224  write_reg(mt, MT9F002_RESET_REGISTER, 0x0118, 2);
225  write_reg(mt, MT9F002_MODE_SELECT, 0x00, 1);
226 
227  uint32_t serialFormat;
228  if (mt->interface == MT9F002_HiSPi) {
229  serialFormat = (3 << 8) | 2; // 2 Serial lanes
230  } else {
231  serialFormat = (2 << 8) | 2; // 2 Serial lanes
232  }
233  write_reg(mt, MT9F002_SERIAL_FORMAT, serialFormat, 2);
234  uint32_t dataFormat = (8 << 8) | 8; // 8 Bits pixel depth
235  write_reg(mt, MT9F002_CPP_DATA_FORMAT, dataFormat, 2);
236 
237  write_reg(mt, MT9F002_MFR_3D00, 0x0435, 2);
238  write_reg(mt, MT9F002_MFR_3D02, 0x435D, 2);
239  write_reg(mt, MT9F002_MFR_3D04, 0x6698, 2);
240  write_reg(mt, MT9F002_MFR_3D06, 0xFFFF, 2);
241  write_reg(mt, MT9F002_MFR_3D08, 0x7783, 2);
242  write_reg(mt, MT9F002_MFR_3D0A, 0x101B, 2);
243  write_reg(mt, MT9F002_MFR_3D0C, 0x732C, 2);
244  write_reg(mt, MT9F002_MFR_3D0E, 0x4230, 2);
245  write_reg(mt, MT9F002_MFR_3D10, 0x5881, 2);
246  write_reg(mt, MT9F002_MFR_3D12, 0x5C3A, 2);
247  write_reg(mt, MT9F002_MFR_3D14, 0x0140, 2);
248  write_reg(mt, MT9F002_MFR_3D16, 0x2300, 2);
249  write_reg(mt, MT9F002_MFR_3D18, 0x815F, 2);
250  write_reg(mt, MT9F002_MFR_3D1A, 0x6789, 2);
251  write_reg(mt, MT9F002_MFR_3D1C, 0x5920, 2);
252  write_reg(mt, MT9F002_MFR_3D1E, 0x0C20, 2);
253  write_reg(mt, MT9F002_MFR_3D20, 0x21C0, 2);
254  write_reg(mt, MT9F002_MFR_3D22, 0x4684, 2);
255  write_reg(mt, MT9F002_MFR_3D24, 0x4892, 2);
256  write_reg(mt, MT9F002_MFR_3D26, 0x1A00, 2);
257  write_reg(mt, MT9F002_MFR_3D28, 0xBA4C, 2);
258  write_reg(mt, MT9F002_MFR_3D2A, 0x8D48, 2);
259  write_reg(mt, MT9F002_MFR_3D2C, 0x4641, 2);
260  write_reg(mt, MT9F002_MFR_3D2E, 0x408C, 2);
261  write_reg(mt, MT9F002_MFR_3D30, 0x4784, 2);
262  write_reg(mt, MT9F002_MFR_3D32, 0x4A87, 2);
263  write_reg(mt, MT9F002_MFR_3D34, 0x561A, 2);
264  write_reg(mt, MT9F002_MFR_3D36, 0x00A5, 2);
265  write_reg(mt, MT9F002_MFR_3D38, 0x1A00, 2);
266  write_reg(mt, MT9F002_MFR_3D3A, 0x5693, 2);
267  write_reg(mt, MT9F002_MFR_3D3C, 0x4D8D, 2);
268  write_reg(mt, MT9F002_MFR_3D3E, 0x4A47, 2);
269  write_reg(mt, MT9F002_MFR_3D40, 0x4041, 2);
270  write_reg(mt, MT9F002_MFR_3D42, 0x8200, 2);
271  write_reg(mt, MT9F002_MFR_3D44, 0x24B7, 2);
272  write_reg(mt, MT9F002_MFR_3D46, 0x0024, 2);
273  write_reg(mt, MT9F002_MFR_3D48, 0x8D4F, 2);
274  write_reg(mt, MT9F002_MFR_3D4A, 0x831A, 2);
275  write_reg(mt, MT9F002_MFR_3D4C, 0x00B4, 2);
276  write_reg(mt, MT9F002_MFR_3D4E, 0x4684, 2);
277  write_reg(mt, MT9F002_MFR_3D50, 0x49CE, 2);
278  write_reg(mt, MT9F002_MFR_3D52, 0x4946, 2);
279  write_reg(mt, MT9F002_MFR_3D54, 0x4140, 2);
280  write_reg(mt, MT9F002_MFR_3D56, 0x9247, 2);
281  write_reg(mt, MT9F002_MFR_3D58, 0x844B, 2);
282  write_reg(mt, MT9F002_MFR_3D5A, 0xCE4B, 2);
283  write_reg(mt, MT9F002_MFR_3D5C, 0x4741, 2);
284  write_reg(mt, MT9F002_MFR_3D5E, 0x502F, 2);
285  write_reg(mt, MT9F002_MFR_3D60, 0xBD3A, 2);
286  write_reg(mt, MT9F002_MFR_3D62, 0x5181, 2);
287  write_reg(mt, MT9F002_MFR_3D64, 0x5E73, 2);
288  write_reg(mt, MT9F002_MFR_3D66, 0x7C0A, 2);
289  write_reg(mt, MT9F002_MFR_3D68, 0x7770, 2);
290  write_reg(mt, MT9F002_MFR_3D6A, 0x8085, 2);
291  write_reg(mt, MT9F002_MFR_3D6C, 0x6A82, 2);
292  write_reg(mt, MT9F002_MFR_3D6E, 0x6742, 2);
293  write_reg(mt, MT9F002_MFR_3D70, 0x8244, 2);
294  write_reg(mt, MT9F002_MFR_3D72, 0x831A, 2);
295  write_reg(mt, MT9F002_MFR_3D74, 0x0099, 2);
296  write_reg(mt, MT9F002_MFR_3D76, 0x44DF, 2);
297  write_reg(mt, MT9F002_MFR_3D78, 0x1A00, 2);
298  write_reg(mt, MT9F002_MFR_3D7A, 0x8542, 2);
299  write_reg(mt, MT9F002_MFR_3D7C, 0x8567, 2);
300  write_reg(mt, MT9F002_MFR_3D7E, 0x826A, 2);
301  write_reg(mt, MT9F002_MFR_3D80, 0x857C, 2);
302  write_reg(mt, MT9F002_MFR_3D82, 0x6B80, 2);
303  write_reg(mt, MT9F002_MFR_3D84, 0x7000, 2);
304  write_reg(mt, MT9F002_MFR_3D86, 0xB831, 2);
305  write_reg(mt, MT9F002_MFR_3D88, 0x40BE, 2);
306  write_reg(mt, MT9F002_MFR_3D8A, 0x6700, 2);
307  write_reg(mt, MT9F002_MFR_3D8C, 0x0CBD, 2);
308  write_reg(mt, MT9F002_MFR_3D8E, 0x4482, 2);
309  write_reg(mt, MT9F002_MFR_3D90, 0x7898, 2);
310  write_reg(mt, MT9F002_MFR_3D92, 0x7480, 2);
311  write_reg(mt, MT9F002_MFR_3D94, 0x5680, 2);
312  write_reg(mt, MT9F002_MFR_3D96, 0x9755, 2);
313  write_reg(mt, MT9F002_MFR_3D98, 0x8057, 2);
314  write_reg(mt, MT9F002_MFR_3D9A, 0x8056, 2);
315  write_reg(mt, MT9F002_MFR_3D9C, 0x9256, 2);
316  write_reg(mt, MT9F002_MFR_3D9E, 0x8057, 2);
317  write_reg(mt, MT9F002_MFR_3DA0, 0x8055, 2);
318  write_reg(mt, MT9F002_MFR_3DA2, 0x817C, 2);
319  write_reg(mt, MT9F002_MFR_3DA4, 0x969B, 2);
320  write_reg(mt, MT9F002_MFR_3DA6, 0x56A6, 2);
321  write_reg(mt, MT9F002_MFR_3DA8, 0x44BE, 2);
322  write_reg(mt, MT9F002_MFR_3DAA, 0x000C, 2);
323  write_reg(mt, MT9F002_MFR_3DAC, 0x867A, 2);
324  write_reg(mt, MT9F002_MFR_3DAE, 0x9474, 2);
325  write_reg(mt, MT9F002_MFR_3DB0, 0x8A79, 2);
326  write_reg(mt, MT9F002_MFR_3DB2, 0x9367, 2);
327  write_reg(mt, MT9F002_MFR_3DB4, 0xBF6A, 2);
328  write_reg(mt, MT9F002_MFR_3DB6, 0x816C, 2);
329  write_reg(mt, MT9F002_MFR_3DB8, 0x8570, 2);
330  write_reg(mt, MT9F002_MFR_3DBA, 0x836C, 2);
331  write_reg(mt, MT9F002_MFR_3DBC, 0x826A, 2);
332  write_reg(mt, MT9F002_MFR_3DBE, 0x8245, 2);
333  write_reg(mt, MT9F002_MFR_3DC0, 0xFFFF, 2);
334  write_reg(mt, MT9F002_MFR_3DC2, 0xFFD6, 2);
335  write_reg(mt, MT9F002_MFR_3DC4, 0x4582, 2);
336  write_reg(mt, MT9F002_MFR_3DC6, 0x6A82, 2);
337  write_reg(mt, MT9F002_MFR_3DC8, 0x6C83, 2);
338  write_reg(mt, MT9F002_MFR_3DCA, 0x7000, 2);
339  write_reg(mt, MT9F002_MFR_3DCC, 0x8024, 2);
340  write_reg(mt, MT9F002_MFR_3DCE, 0xB181, 2);
341  write_reg(mt, MT9F002_MFR_3DD0, 0x6859, 2);
342  write_reg(mt, MT9F002_MFR_3DD2, 0x732B, 2);
343  write_reg(mt, MT9F002_MFR_3DD4, 0x4030, 2);
344  write_reg(mt, MT9F002_MFR_3DD6, 0x4982, 2);
345  write_reg(mt, MT9F002_MFR_3DD8, 0x101B, 2);
346  write_reg(mt, MT9F002_MFR_3DDA, 0x4083, 2);
347  write_reg(mt, MT9F002_MFR_3DDC, 0x6785, 2);
348  write_reg(mt, MT9F002_MFR_3DDE, 0x3A00, 2);
349  write_reg(mt, MT9F002_MFR_3DE0, 0x8820, 2);
350  write_reg(mt, MT9F002_MFR_3DE2, 0x0C59, 2);
351  write_reg(mt, MT9F002_MFR_3DE4, 0x8546, 2);
352  write_reg(mt, MT9F002_MFR_3DE6, 0x8348, 2);
353  write_reg(mt, MT9F002_MFR_3DE8, 0xD04C, 2);
354  write_reg(mt, MT9F002_MFR_3DEA, 0x8B48, 2);
355  write_reg(mt, MT9F002_MFR_3DEC, 0x4641, 2);
356  write_reg(mt, MT9F002_MFR_3DEE, 0x4083, 2);
357  write_reg(mt, MT9F002_MFR_3DF0, 0x1A00, 2);
358  write_reg(mt, MT9F002_MFR_3DF2, 0x8347, 2);
359  write_reg(mt, MT9F002_MFR_3DF4, 0x824A, 2);
360  write_reg(mt, MT9F002_MFR_3DF6, 0x9A56, 2);
361  write_reg(mt, MT9F002_MFR_3DF8, 0x1A00, 2);
362  write_reg(mt, MT9F002_MFR_3DFA, 0x951A, 2);
363  write_reg(mt, MT9F002_MFR_3DFC, 0x0056, 2);
364  write_reg(mt, MT9F002_MFR_3DFE, 0x914D, 2);
365  write_reg(mt, MT9F002_MFR_3E00, 0x8B4A, 2);
366  write_reg(mt, MT9F002_MFR_3E02, 0x4700, 2);
367  write_reg(mt, MT9F002_MFR_3E04, 0x0300, 2);
368  write_reg(mt, MT9F002_MFR_3E06, 0x2492, 2);
369  write_reg(mt, MT9F002_MFR_3E08, 0x0024, 2);
370  write_reg(mt, MT9F002_MFR_3E0A, 0x8A1A, 2);
371  write_reg(mt, MT9F002_MFR_3E0C, 0x004F, 2);
372  write_reg(mt, MT9F002_MFR_3E0E, 0xB446, 2);
373  write_reg(mt, MT9F002_MFR_3E10, 0x8349, 2);
374  write_reg(mt, MT9F002_MFR_3E12, 0xB249, 2);
375  write_reg(mt, MT9F002_MFR_3E14, 0x4641, 2);
376  write_reg(mt, MT9F002_MFR_3E16, 0x408B, 2);
377  write_reg(mt, MT9F002_MFR_3E18, 0x4783, 2);
378  write_reg(mt, MT9F002_MFR_3E1A, 0x4BDB, 2);
379  write_reg(mt, MT9F002_MFR_3E1C, 0x4B47, 2);
380  write_reg(mt, MT9F002_MFR_3E1E, 0x4180, 2);
381  write_reg(mt, MT9F002_MFR_3E20, 0x502B, 2);
382  write_reg(mt, MT9F002_MFR_3E22, 0x4C3A, 2);
383  write_reg(mt, MT9F002_MFR_3E24, 0x4180, 2);
384  write_reg(mt, MT9F002_MFR_3E26, 0x737C, 2);
385  write_reg(mt, MT9F002_MFR_3E28, 0xD124, 2);
386  write_reg(mt, MT9F002_MFR_3E2A, 0x9068, 2);
387  write_reg(mt, MT9F002_MFR_3E2C, 0x8A20, 2);
388  write_reg(mt, MT9F002_MFR_3E2E, 0x2170, 2);
389  write_reg(mt, MT9F002_MFR_3E30, 0x8081, 2);
390  write_reg(mt, MT9F002_MFR_3E32, 0x6A67, 2);
391  write_reg(mt, MT9F002_MFR_3E34, 0x4257, 2);
392  write_reg(mt, MT9F002_MFR_3E36, 0x5544, 2);
393  write_reg(mt, MT9F002_MFR_3E38, 0x8644, 2);
394  write_reg(mt, MT9F002_MFR_3E3A, 0x9755, 2);
395  write_reg(mt, MT9F002_MFR_3E3C, 0x5742, 2);
396  write_reg(mt, MT9F002_MFR_3E3E, 0x676A, 2);
397  write_reg(mt, MT9F002_MFR_3E40, 0x807D, 2);
398  write_reg(mt, MT9F002_MFR_3E42, 0x3180, 2);
399  write_reg(mt, MT9F002_MFR_3E44, 0x7000, 2);
400  write_reg(mt, MT9F002_MFR_3E46, 0x0000, 2);
401  write_reg(mt, MT9F002_MFR_3E48, 0x0000, 2);
402  write_reg(mt, MT9F002_MFR_3E4A, 0x0000, 2);
403  write_reg(mt, MT9F002_MFR_3E4C, 0x0000, 2);
404  write_reg(mt, MT9F002_MFR_3E4E, 0x0000, 2);
405  write_reg(mt, MT9F002_MFR_3E50, 0x0000, 2);
406  write_reg(mt, MT9F002_MFR_3E52, 0x0000, 2);
407  write_reg(mt, MT9F002_MFR_3E54, 0x0000, 2);
408  write_reg(mt, MT9F002_MFR_3E56, 0x0000, 2);
409  write_reg(mt, MT9F002_MFR_3E58, 0x0000, 2);
410  write_reg(mt, MT9F002_MFR_3E5A, 0x0000, 2);
411  write_reg(mt, MT9F002_MFR_3E5C, 0x0000, 2);
412  write_reg(mt, MT9F002_MFR_3E5E, 0x0000, 2);
413  write_reg(mt, MT9F002_MFR_3E60, 0x0000, 2);
414  write_reg(mt, MT9F002_MFR_3E62, 0x0000, 2);
415  write_reg(mt, MT9F002_MFR_3E64, 0x0000, 2);
416  write_reg(mt, MT9F002_MFR_3E66, 0x0000, 2);
417  write_reg(mt, MT9F002_MFR_3E68, 0x0000, 2);
418  write_reg(mt, MT9F002_MFR_3E6A, 0x0000, 2);
419  write_reg(mt, MT9F002_MFR_3E6C, 0x0000, 2);
420  write_reg(mt, MT9F002_MFR_3E6E, 0x0000, 2);
421  write_reg(mt, MT9F002_MFR_3E70, 0x0000, 2);
422  write_reg(mt, MT9F002_MFR_3E72, 0x0000, 2);
423  write_reg(mt, MT9F002_MFR_3E74, 0x0000, 2);
424  write_reg(mt, MT9F002_MFR_3E76, 0x0000, 2);
425  write_reg(mt, MT9F002_MFR_3E78, 0x0000, 2);
426  write_reg(mt, MT9F002_MFR_3E7A, 0x0000, 2);
427  write_reg(mt, MT9F002_MFR_3E7C, 0x0000, 2);
428  write_reg(mt, MT9F002_MFR_3E7E, 0x0000, 2);
429  write_reg(mt, MT9F002_MFR_3E80, 0x0000, 2);
430  write_reg(mt, MT9F002_MFR_3E82, 0x0000, 2);
431  write_reg(mt, MT9F002_MFR_3E84, 0x0000, 2);
432  write_reg(mt, MT9F002_MFR_3E86, 0x0000, 2);
433  write_reg(mt, MT9F002_MFR_3E88, 0x0000, 2);
434  write_reg(mt, MT9F002_MFR_3E8A, 0x0000, 2);
435  write_reg(mt, MT9F002_MFR_3E8C, 0x0000, 2);
436  write_reg(mt, MT9F002_MFR_3E8E, 0x0000, 2);
437  write_reg(mt, MT9F002_MFR_3E90, 0x0000, 2);
438  write_reg(mt, MT9F002_MFR_3E92, 0x0000, 2);
439  write_reg(mt, MT9F002_MFR_3E94, 0x0000, 2);
440  write_reg(mt, MT9F002_MFR_3E96, 0x0000, 2);
441  write_reg(mt, MT9F002_MFR_3E98, 0x0000, 2);
442  write_reg(mt, MT9F002_MFR_3E9A, 0x0000, 2);
443  write_reg(mt, MT9F002_MFR_3E9C, 0x0000, 2);
444  write_reg(mt, MT9F002_MFR_3E9E, 0x0000, 2);
445  write_reg(mt, MT9F002_MFR_3EA0, 0x0000, 2);
446  write_reg(mt, MT9F002_MFR_3EA2, 0x0000, 2);
447  write_reg(mt, MT9F002_MFR_3EA4, 0x0000, 2);
448  write_reg(mt, MT9F002_MFR_3EA6, 0x0000, 2);
449  write_reg(mt, MT9F002_MFR_3EA8, 0x0000, 2);
450  write_reg(mt, MT9F002_MFR_3EAA, 0x0000, 2);
451  write_reg(mt, MT9F002_MFR_3EAC, 0x0000, 2);
452  write_reg(mt, MT9F002_MFR_3EAE, 0x0000, 2);
453  write_reg(mt, MT9F002_MFR_3EB0, 0x0000, 2);
454  write_reg(mt, MT9F002_MFR_3EB2, 0x0000, 2);
455  write_reg(mt, MT9F002_MFR_3EB4, 0x0000, 2);
456  write_reg(mt, MT9F002_MFR_3EB6, 0x0000, 2);
457  write_reg(mt, MT9F002_MFR_3EB8, 0x0000, 2);
458  write_reg(mt, MT9F002_MFR_3EBA, 0x0000, 2);
459  write_reg(mt, MT9F002_MFR_3EBC, 0x0000, 2);
460  write_reg(mt, MT9F002_MFR_3EBE, 0x0000, 2);
461  write_reg(mt, MT9F002_MFR_3EC0, 0x0000, 2);
462  write_reg(mt, MT9F002_MFR_3EC2, 0x0000, 2);
463  write_reg(mt, MT9F002_MFR_3EC4, 0x0000, 2);
464  write_reg(mt, MT9F002_MFR_3EC6, 0x0000, 2);
465  write_reg(mt, MT9F002_MFR_3EC8, 0x0000, 2);
466  write_reg(mt, MT9F002_MFR_3ECA, 0x0000, 2);
467  write_reg(mt, MT9F002_MFR_3176, 0x4000, 2);
468  write_reg(mt, MT9F002_MFR_317C, 0xA00A, 2);
469  write_reg(mt, MT9F002_MFR_3EE6, 0x0000, 2);
470  write_reg(mt, MT9F002_MFR_3ED8, 0xE0E0, 2);
471  write_reg(mt, MT9F002_MFR_3EE8, 0x0001, 2);
472  write_reg(mt, MT9F002_SMIA_TEST, 0x0005, 2);
473 }
474 
478 static inline void mt9f002_mipi_stage2(struct mt9f002_t *mt)
479 {
480  write_reg(mt, MT9F002_SMIA_TEST, 0x0045, 2);
481 }
482 
486 static inline void mt9f002_mipi_stage3(struct mt9f002_t *mt)
487 {
488  write_reg(mt, MT9F002_EXTRA_DELAY, 0x0000, 2);
489  write_reg(mt, MT9F002_RESET_REGISTER, 0x0118, 2);
490  write_reg(mt, MT9F002_MFR_3EDC, 0x68CF, 2);
491  write_reg(mt, MT9F002_MFR_3EE2, 0xE363, 2);
492 }
493 
497 static inline void mt9f002_parallel_stage1(struct mt9f002_t *mt)
498 {
499  write_reg(mt, MT9F002_RESET_REGISTER , 0x0010, 2);
500  write_reg(mt, MT9F002_GLOBAL_GAIN , 0x1430, 2);
501  write_reg(mt, MT9F002_RESET_REGISTER , 0x0010, 2);
502  write_reg(mt, MT9F002_RESET_REGISTER , 0x0010, 2);
503  write_reg(mt, MT9F002_RESET_REGISTER , 0x0010, 2);
504  write_reg(mt, MT9F002_DAC_LD_14_15 , 0xE525, 2);
505  write_reg(mt, MT9F002_CTX_CONTROL_REG, 0x0000, 2);
506  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xF873, 2);
507  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x08AA, 2);
508  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x3219, 2);
509  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x3219, 2);
510  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x3219, 2);
511  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x3200, 2);
512  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x3200, 2);
513  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x3200, 2);
514  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x3200, 2);
515  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x3200, 2);
516  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x1769, 2);
517  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xA6F3, 2);
518  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xA6F3, 2);
519  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xA6F3, 2);
520  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xA6F3, 2);
521  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xA6F3, 2);
522  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xA6F3, 2);
523  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xA6F3, 2);
524  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xAFF3, 2);
525  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xFA64, 2);
526  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xFA64, 2);
527  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xFA64, 2);
528  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xF164, 2);
529  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xFA64, 2);
530  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xFA64, 2);
531  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xFA64, 2);
532  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xF164, 2);
533  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x276E, 2);
534  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x18CF, 2);
535  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x18CF, 2);
536  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x18CF, 2);
537  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x28CF, 2);
538  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x18CF, 2);
539  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x18CF, 2);
540  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x18CF, 2);
541  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x18CF, 2);
542  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x2363, 2);
543  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x2363, 2);
544  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x2352, 2);
545  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x2363, 2);
546  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x2363, 2);
547  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x2363, 2);
548  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x2352, 2);
549  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x2352, 2);
550  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xA394, 2);
551  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xA394, 2);
552  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x8F8F, 2);
553  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xA3D4, 2);
554  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xA394, 2);
555  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xA394, 2);
556  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x8F8F, 2);
557  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x8FCF, 2);
558  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xDC23, 2);
559  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xDC63, 2);
560  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xDC63, 2);
561  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xDC23, 2);
562  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xDC23, 2);
563  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xDC63, 2);
564  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xDC63, 2);
565  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0xDC23, 2);
566  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x0F73, 2);
567  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x85C0, 2);
568  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x85C0, 2);
569  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x85C0, 2);
570  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x85C0, 2);
571  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x85C0, 2);
572  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x85C0, 2);
573  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x85C0, 2);
574  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x85C4, 2);
575  write_reg(mt, MT9F002_CTX_WR_DATA_REG, 0x0000, 2);
576  write_reg(mt, MT9F002_ANALOG_CONTROL4, 0x8000, 2);
577  write_reg(mt, MT9F002_DAC_LD_14_15 , 0xE525, 2);
578  write_reg(mt, MT9F002_DATA_PEDESTAL_ , 0x00A8, 2);
579  write_reg(mt, MT9F002_RESET_REGISTER , 0x0090, 2);
580  write_reg(mt, MT9F002_SERIAL_FORMAT , 0x0301, 2);
581  write_reg(mt, MT9F002_RESET_REGISTER , 0x1090, 2);
582  write_reg(mt, MT9F002_SMIA_TEST , 0x0845, 2);
583  write_reg(mt, MT9F002_RESET_REGISTER , 0x1080, 2);
584  write_reg(mt, MT9F002_DATAPATH_SELECT, 0xD880, 2);
585  write_reg(mt, MT9F002_RESET_REGISTER , 0x9080, 2);
586  write_reg(mt, MT9F002_DATAPATH_SELECT, 0xD880, 2);
587  write_reg(mt, MT9F002_RESET_REGISTER , 0x10C8, 2);
588  write_reg(mt, MT9F002_DATAPATH_SELECT, 0xD880, 2);
589 }
590 
594 static inline void mt9f002_parallel_stage2(struct mt9f002_t *mt)
595 {
596  write_reg(mt, MT9F002_ANALOG_CONTROL4, 0x8000, 2);
597  write_reg(mt, MT9F002_READ_MODE, 0x0041, 2);
598 
599  write_reg(mt, MT9F002_READ_MODE , 0x04C3, 2);
600  write_reg(mt, MT9F002_READ_MODE , 0x04C3, 2);
601  write_reg(mt, MT9F002_ANALOG_CONTROL5 , 0x0000, 2);
602  write_reg(mt, MT9F002_ANALOG_CONTROL5 , 0x0000, 2);
603  write_reg(mt, MT9F002_ANALOG_CONTROL5 , 0x0000, 2);
604  write_reg(mt, MT9F002_ANALOG_CONTROL5 , 0x0000, 2);
605  write_reg(mt, MT9F002_DAC_LD_28_29 , 0x0047, 2);
606  write_reg(mt, MT9F002_COLUMN_CORRECTION , 0xB080, 2);
607  write_reg(mt, MT9F002_COLUMN_CORRECTION , 0xB100, 2);
608  write_reg(mt, MT9F002_DARK_CONTROL3 , 0x0020, 2);
609  write_reg(mt, MT9F002_DAC_LD_24_25 , 0x6349, 2);
610  write_reg(mt, MT9F002_ANALOG_CONTROL7 , 0x800A, 2);
611  write_reg(mt, MT9F002_RESET_REGISTER , 0x90C8, 2);
612  write_reg(mt, MT9F002_CTX_CONTROL_REG , 0x8005, 2);
613  write_reg(mt, MT9F002_ANALOG_CONTROL7 , 0x800A, 2);
614  write_reg(mt, MT9F002_DAC_LD_28_29 , 0x0047, 2);
615  write_reg(mt, MT9F002_DAC_LD_30_31 , 0x15F0, 2);
616  write_reg(mt, MT9F002_DAC_LD_30_31 , 0x15F0, 2);
617  write_reg(mt, MT9F002_DAC_LD_30_31 , 0x15F0, 2);
618  write_reg(mt, MT9F002_DAC_LD_28_29 , 0x0047, 2);
619  write_reg(mt, MT9F002_DAC_LD_28_29 , 0x0047, 2);
620  write_reg(mt, MT9F002_RESET_REGISTER , 0x10C8, 2);
621  //write_reg(mt, MT9F002_RESET_REGISTER , 0x14C8, 2); // reset bad frame
623  write_reg(mt, MT9F002_DIGITAL_TEST, 0x0000 , 2);
624  //write_reg(mt, MT9F002_DATAPATH_SELECT , 0xd881, 2); // permanent line valid
625  write_reg(mt, MT9F002_DATAPATH_SELECT, 0xd880, 2);
626  write_reg(mt, MT9F002_MASK_CORRUPTED_FRAMES , 0x0001, 1); // 0 output corrupted frame, 1 mask them
627 }
628 
632 static inline void mt9f002_set_pll(struct mt9f002_t *mt)
633 {
634  // Update registers
641 
642  uint16_t smia = read_reg(mt, MT9F002_SMIA_TEST, 2);
643  write_reg(mt, MT9F002_SMIA_TEST, (smia & 0xFFBF) | (mt->shift_vt_pix_clk_div << 6), 2); // shift_vt_pix_clk_div
644 
645  uint16_t row_speed = read_reg(mt, MT9F002_ROW_SPEED, 2);
646  row_speed = (row_speed & 0xFFF8) | (mt->rowSpeed_2_0 & 0x07); // rowSpeed_2_0
647  row_speed = (row_speed & 0xF8FF) | ((mt->row_speed_10_8 & 0x07) << 8); // row_speed_10_8
648  row_speed = (row_speed & (~0x70)) | (0x2 << 4); // Change opclk_delay
649  write_reg(mt, MT9F002_ROW_SPEED, row_speed, 2);
650 
651  // Compute clocks
652  mt->vt_pix_clk = mt->input_clk_freq * (float)mt->pll_multiplier * (float)(1 + mt->shift_vt_pix_clk_div)
653  / ((float)mt->pre_pll_clk_div * (float)mt->vt_sys_clk_div * (float)mt->vt_pix_clk_div);
654  mt->op_pix_clk = mt->input_clk_freq * (float)mt->pll_multiplier
655  / ((float)mt->pre_pll_clk_div * (float)mt->op_sys_clk_div * (float)mt->op_pix_clk_div);
656 }
657 
658 // set the frame rate of the camera
659 static void mt9f002_set_blanking(struct mt9f002_t *mt)
660 {
661  //Set the blanking configuration
662  if (mt->x_odd_inc > 1) {
663  if (mt->y_odd_inc > 1) {
664  /* Binning XY */
670  } else {
671  /* Binning X */
677  }
678  } else {
682  if (mt->output_scaler > 1) {
683  /* Scaler mode */
686  } else {
687  /* Normal mode */
690  }
691  }
692 
693  uint16_t x_addr_start = read_reg(mt, MT9F002_X_ADDR_START, 2);
694  uint16_t x_addr_end = read_reg(mt, MT9F002_X_ADDR_END, 2);
695 
696  float subsamplingX_factor = (float)(1 + mt->x_odd_inc) / 2.0f; // See page 52
697  float subsamplingY_factor = (float)(1 + mt->y_odd_inc) / 2.0f; // See page 52
698 
699  /* Calculate minimum line length based on p. 53 */
700  // line length based on window width
701  uint16_t min_line_length = (uint16_t)((x_addr_end - x_addr_start + mt->x_odd_inc) / subsamplingX_factor / 2) +
703 
704  // must be strictly longer than min length set in the min_line_length_pck register
705  min_line_length = Max(min_line_length, mt9f002_blanking.min_line_length_pck + 1);
706 
707  // row time must be strictly larger than the time needed for the FIFO to output the data
708  if (mt->interface == MT9F002_MIPI || mt->interface == MT9F002_HiSPi) {
709  // 2 lanes, pll clocks
710  min_line_length = Max(min_line_length,
712  } else {
713  // pll clocks
714  min_line_length = Max(min_line_length,
716  }
717 
718  /* Do some magic to get it to work with P7 ISP (with horizontal blanking) */
719  uint32_t clkRatio_num = mt->op_sys_clk_div * mt->op_pix_clk_div * mt->row_speed_10_8 * (1 + mt->shift_vt_pix_clk_div);
720  uint32_t clkRatio_den = mt->vt_sys_clk_div * mt->vt_pix_clk_div;
721 
722  /* Divide by the GCD to find smallest ratio */
723  uint32_t clkRatio_gcd = int32_gcd(clkRatio_num, clkRatio_den);
724  clkRatio_num = clkRatio_num / clkRatio_gcd;
725  clkRatio_den = clkRatio_den / clkRatio_gcd;
726 
727  /* Calculate minimum horizontal blanking, since fpga line_length must be divisible by 2 */
728  uint32_t min_horizontal_blanking = clkRatio_num;
729  if ((clkRatio_den % 2) != 0) {
730  min_horizontal_blanking *= 2;
731  }
732 
733  /* Fix fpga correction based on min horizontal blanking */
734  if ((min_line_length % min_horizontal_blanking) != 0) {
735  min_line_length += min_horizontal_blanking - (min_line_length % min_horizontal_blanking);
736  }
737  mt->line_length = min_line_length;
738 
739  /* Calculate minimum frame length lines */
740  uint16_t y_addr_start = read_reg(mt, MT9F002_Y_ADDR_START, 2);
741  uint16_t y_addr_end = read_reg(mt, MT9F002_Y_ADDR_END, 2);
742  uint16_t min_frame_blanking_lines = read_reg(mt, MT9F002_MIN_FRAME_BLANKING_LINES, 2);
743  // frame time is limited by total number of rows (EQ 10)
744  uint16_t min_frame_length = (y_addr_end - y_addr_start + 1) / subsamplingY_factor + min_frame_blanking_lines;
745  mt->frame_length = min_frame_length;
746 
747  /* Calculate FPS we get using these minimums (Maximum FPS) */
748  mt->real_fps = mt->vt_pix_clk * 1e6 / (float)(mt->line_length * mt->frame_length);
749  VERBOSE_PRINT("Maximum FPS: %0.3f\n", mt->real_fps);
750 
751  /* Check if we need to downscale the FPS and bruteforce better solution */
752  if (mt->target_fps > 0 && mt->target_fps < mt->real_fps) {
753  float min_fps_err = mt->real_fps - mt->target_fps;
754  float new_fps = mt->real_fps;
755 
756  // Go through all possible line lengths
757  for (uint32_t ll = min_line_length; ll <= MT9F002_LINE_LENGTH_MAX; ll += min_horizontal_blanking) {
758  // Go through all possible frame lengths
759  for (uint32_t fl = min_frame_length; fl < MT9F002_FRAME_LENGTH_MAX; fl++) {
760  new_fps = mt->vt_pix_clk * 1000000 / (float)(ll * fl);
761 
762  // Calculate FPS error and save if it is better
763  float fps_err = new_fps - mt->target_fps;
764 
765  // Stop searching if FPS is lower or equal
766  if (fps_err < 0) {
767  break;
768  }
769 
770  if (fps_err < min_fps_err) {
771  min_fps_err = fps_err;
772  mt->line_length = ll;
773  mt->frame_length = fl;
774  mt->real_fps = new_fps;
775  }
776  }
777 
778  // Calculate if next step is still needed (since we only need to go one step below target_fps)
779  new_fps = mt->vt_pix_clk * 1000000 / (float)(ll * min_frame_length);
780 
781  // Stop searching if FPS is lower or equal
782  if (new_fps - mt->target_fps <= 0.f) {
783  break;
784  }
785  }
786  }
787 
788  VERBOSE_PRINT("Set FPS: %0.3f\n", mt->vt_pix_clk * 1000000 / (float)(mt->line_length * mt->frame_length));
789 
790  /* Actually set the calculated values */
793 }
794 
800 {
801  /* Fetch minimum and maximum integration times */
802  uint16_t coarse_integration_min = read_reg(mt, MT9F002_COARSE_INTEGRATION_TIME_MIN, 2);
803  uint16_t coarse_integration_max = mt->frame_length - read_reg(mt, MT9F002_COARSE_INTEGRATION_TIME_MAX_MARGIN, 2);
804  uint16_t fine_integration_min = mt9f002_blanking.fine_integration_time_min;
805  uint16_t fine_integration_max_margin = mt9f002_blanking.fine_integration_time_max_margin;
806  uint16_t fine_integration_max = mt->line_length - fine_integration_max_margin;
807 
808  /* Compute fine and coarse integration time */
809  uint32_t integration = mt->target_exposure * mt->vt_pix_clk * 1000;
810  uint16_t coarse_integration = integration / mt->line_length;
811  uint16_t fine_integration = integration % mt->line_length;
812 
813  /* Make sure fine integration is inside bounds */
814  if (fine_integration_min > fine_integration || fine_integration > fine_integration_max) {
815  int32_t upper_coarse_integration = coarse_integration + 1;
816  int32_t upper_fine_integration = fine_integration_min;
817 
818  int32_t lower_coarse_integration = coarse_integration - 1;
819  int32_t lower_fine_integration = fine_integration_max;
820 
821  // Check if lower case is invalid (take upper coarse)
822  if (lower_coarse_integration < coarse_integration_min) {
823  coarse_integration = upper_coarse_integration;
824  fine_integration = upper_fine_integration;
825  }
826  // Check if upper case is invalid (take lower coarse)
827  else if (upper_coarse_integration > coarse_integration_max) {
828  coarse_integration = lower_coarse_integration;
829  fine_integration = lower_fine_integration;
830  }
831  // Both are good
832  else {
833  // Calculate error to decide which is better
834  int32_t upper_error = abs((mt->line_length * upper_coarse_integration + upper_fine_integration) - integration);
835  int32_t lower_error = abs((mt->line_length * lower_coarse_integration + lower_fine_integration) - integration);
836 
837  if (upper_error < lower_error) {
838  coarse_integration = upper_coarse_integration;
839  fine_integration = upper_fine_integration;
840  } else {
841  coarse_integration = lower_coarse_integration;
842  fine_integration = lower_fine_integration;
843  }
844  }
845  }
846 
847  /* Fix saturations */
848  Bound(fine_integration, fine_integration_min, fine_integration_max);
849  Bound(coarse_integration, coarse_integration_min, coarse_integration_max);
850 
851  /* Set the registers */
852  mt->real_exposure = (float)(coarse_integration * mt->line_length + fine_integration) / (mt->vt_pix_clk * 1000);
853  write_reg(mt, MT9F002_COARSE_INTEGRATION_TIME, coarse_integration, 2);
854  write_reg(mt, MT9F002_FINE_INTEGRATION_TIME_, fine_integration, 2);
855 }
856 
860 static uint16_t mt9f002_calc_gain(float gain)
861 {
862  // Check if gain is valid
863  if (gain < 1.0) {
864  gain = 1.0;
865  }
866 
867  // Calculation of colamp, analg3 and digital gain based on table 19 p56
868  uint8_t colamp_gain, analog_gain3, digital_gain;
869  if (gain < 1.50) {
870  // This is not recommended
871  colamp_gain = 0;
872  analog_gain3 = 0;
873  digital_gain = 1;
874  } else if (gain < 3.0) {
875  colamp_gain = 1;
876  analog_gain3 = 0;
877  digital_gain = 1;
878  } else if (gain < 6.0) {
879  colamp_gain = 2;
880  analog_gain3 = 0;
881  digital_gain = 1;
882  } else if (gain < 16.0) {
883  colamp_gain = 3;
884  analog_gain3 = 0;
885  digital_gain = 1;
886  } else if (gain < 32.0) {
887  colamp_gain = 3;
888  analog_gain3 = 0;
889  digital_gain = 2;
890  } else {
891  colamp_gain = 3;
892  analog_gain3 = 0;
893  digital_gain = 4;
894  }
895 
896  // Calculate gain 2 (fine gain)
897  uint16_t analog_gain2 = gain / (float)digital_gain / (float)(1 << colamp_gain) / (float)(1 << analog_gain3) * 64.0;
898  Bound(analog_gain2, 1, 127);
899 
900  return (analog_gain2 & 0x7F) | ((analog_gain3 & 0x7) << 7) | ((colamp_gain & 0x3) << 10) | ((digital_gain & 0xF) << 12);
901 }
902 
906 void mt9f002_set_gains(struct mt9f002_t *mt)
907 {
912 }
913 
914 static void mt9f002_calc_resolution(struct mt9f002_t *mt)
915 {
916  struct v4l2_rect crop, rect;
917  unsigned int x_odd_inc, y_odd_inc;
918  unsigned int hratio, vratio;
919  unsigned int width, height;
920  unsigned int ratio;
921  unsigned int xMultiple;
922  unsigned int div_res;
923  static const uint8_t xy_odd_inc_tab[] = {1, 1, 3, 3, 7, 7, 7, 7, 15};
924 
925  crop.left = mt->offset_x;
926  crop.top = mt->offset_y;
927  crop.width = mt->sensor_width;
928  crop.height = mt->sensor_height;
929 
930  VERBOSE_PRINT("Requested output - width: %i, height: %i\n", mt->output_width, mt->output_height);
931  VERBOSE_PRINT("Requested crop - top: %i, left: %i, width: %i, height: %i\n", crop.top, crop.left, crop.width,
932  crop.height);
933 
934  /* Clamp the crop rectangle boundaries and Align them to a multiple of 2
935  * pixels to ensure a GRBG Bayer pattern.
936  */
937  rect.width = Clip(Align(crop.width, 4), 1, CFG_MT9F002_PIXEL_ARRAY_WIDTH);
938  rect.height = Clip(Align(crop.height, 4), 1, CFG_MT9F002_PIXEL_ARRAY_HEIGHT);
939 
940  /* Clamp the width and height to avoid dividing by zero. */
941  width = Clip(Align(mt->output_width, 2), Max(rect.width / 8, CFG_MT9F002_WINDOW_WIDTH_MIN), rect.width);
942  height = Clip(Align(mt->output_height, 2), Max((rect.height / 8), CFG_MT9F002_WINDOW_HEIGHT_MIN), rect.height);
943 
944  /* Calculate binning / skipping, we enforce that binning in X and Y are the same */
945  div_res = Min(rect.width / width, rect.height / height);
946  div_res = Clip(div_res, 1, 4);
947  x_odd_inc = xy_odd_inc_tab[div_res];
948  y_odd_inc = xy_odd_inc_tab[div_res];
949 
950  /* Calculate remaining scaling not handled by binning / skipping */
951  hratio = (rect.width / ((x_odd_inc + 1) / 2) * MT9F002_SCALER_N) / width;
952  vratio = (rect.height / ((y_odd_inc + 1) / 2) * MT9F002_SCALER_N) / height;
953  ratio = Min(hratio, vratio);
954 
955  /* Check ratio */
956  if (ratio > CFG_SCALER_M_MAX) {
957  /* Fix ratio to maximum and adjust the crop window */
958  ratio = CFG_SCALER_M_MAX;
959  }
960 
961  rect.width = mt->output_width * ratio * ((x_odd_inc + 1) / 2) / MT9F002_SCALER_N;
962  rect.height = mt->output_height * ratio * ((y_odd_inc + 1) / 2) / MT9F002_SCALER_N;
963 
964  VERBOSE_PRINT("Calculated skipping - x: %i, y: %i\n", x_odd_inc, y_odd_inc);
965  VERBOSE_PRINT("Calculated scaler - %i/%i = %0.3f\n", MT9F002_SCALER_N, ratio, MT9F002_SCALER_N / ((float)ratio));
966 
967  // center crop to same as requested
968  rect.left = crop.left + ((int32_t)crop.width - (int32_t)rect.width) / 2;
969  rect.top = crop.top + ((int32_t)crop.height - (int32_t)rect.height) / 2;
970 
971  rect.left = Min(rect.left, CFG_MT9F002_PIXEL_ARRAY_WIDTH - rect.width);
972  rect.top = Min(rect.top, CFG_MT9F002_PIXEL_ARRAY_HEIGHT - rect.height);
973  rect.left = Clip(Align(rect.left, 2), CFG_MT9F002_X_ADDR_MIN, CFG_MT9F002_X_ADDR_MAX);
974  rect.top = Clip(Align(rect.top, 2), CFG_MT9F002_Y_ADDR_MIN, CFG_MT9F002_Y_ADDR_MAX);
975 
976  /* Align left offset to 8 */
977  xMultiple = 8 * ((x_odd_inc + 1) / 2);
978  rect.left = Align(rect.left, xMultiple);
979 
980  /* Align top offset to 2 */
981  rect.top = Align(rect.top, 4);
982 
983  /* Update crop */
984  crop = rect;
985  VERBOSE_PRINT("Granted crop - top: %i, left: %i, width: %i, height: %i\n", crop.top, crop.left, crop.width,
986  crop.height);
987  VERBOSE_PRINT("Granted output - width: %i, height: %i\n", width, height);
988 
989  /* Update values */
990  mt->output_scaler = ratio;
991  mt->x_odd_inc = x_odd_inc;
992  mt->y_odd_inc = y_odd_inc;
993 
994  mt->offset_x = crop.left;
995  mt->offset_y = crop.top;
996  mt->sensor_width = crop.width;
997  mt->sensor_height = crop.height;
998 }
999 
1000 static void mt9f002_set_resolution(struct mt9f002_t *mt)
1001 {
1002  /* Set window pos */
1007 
1008  /* Set output resolution */
1011 
1012  /* scaler */
1013  if (mt->output_scaler > 1) {
1014  /* enable scaling mode */
1015  write_reg(mt, MT9F002_SCALING_MODE, 2, 2);
1016  write_reg(mt, MT9F002_DATAPATH_SELECT, 0xd8b0, 2); // bayer resampling
1018  }
1019 
1020  /* Binning / Skipping */
1021  if (mt->x_odd_inc > 1 || mt->x_odd_inc > 1) {
1022  write_reg(mt, MT9F002_READ_MODE, 0x0441, 2);
1023  } else {
1024  write_reg(mt, MT9F002_READ_MODE, 0x0041, 2);
1025  }
1026  write_reg(mt, MT9F002_X_ODD_INC, mt->x_odd_inc, 2);
1027  write_reg(mt, MT9F002_Y_ODD_INC, mt->y_odd_inc, 2);
1028 
1029  return;
1030 }
1031 
1036 void mt9f002_init(struct mt9f002_t *mt)
1037 {
1038  /* Reset the device */
1039  //TODO???
1040 
1041  /* Setup i2c transaction */
1043 
1044  /* Software reset */
1045  write_reg(mt, MT9F002_SOFTWARE_RESET, 0x1, 1);
1046  usleep(1000000); // Wait for one second
1047 
1048  /* Based on the interface configure stage 1 */
1049  if (mt->interface == MT9F002_MIPI || mt->interface == MT9F002_HiSPi) {
1050  mt9f002_mipi_stage1(mt);
1051  } else {
1053  }
1054 
1055  /* Set the PLL based on Input clock and wanted clock frequency */
1056  mt9f002_set_pll(mt);
1057 
1058  /* Calculate binning/skipping/scaling for requested sensor domain and output resolution */
1060 
1061  /* Based on the interface configure stage 2 */
1062  if (mt->interface == MT9F002_MIPI || mt->interface == MT9F002_HiSPi) {
1063  mt9f002_mipi_stage2(mt);
1064  } else {
1066  }
1067 
1069 
1070  /* Update blanking (based on FPS) */
1072 
1073  /* Update statistics window (inside of cropped sensor area)*/
1075 
1076  /* Update exposure (based on target_exposure) */
1078 
1079  /* Update gains for the different pixel colors */
1080  mt9f002_set_gains(mt);
1081 
1082  /* Based on the interface configure stage 3 */
1083  if (mt->interface == MT9F002_MIPI || mt->interface == MT9F002_HiSPi) {
1084  mt9f002_mipi_stage3(mt);
1085  }
1086 
1087  /* Turn the stream on */
1088  write_reg(mt, MT9F002_MODE_SELECT, 0x01, 1);
1089 
1090  VERBOSE_PRINT("MT9F002 initialized\n");
1091 }
1092 
1094 {
1095  mt->gain_red = MT9F002_GAIN_RED;
1099  mt9f002_set_gains(mt);
1100 }
1101 
1103 {
1106 }
1107 
1108 /* Handler for propagating user resolution change so the camera
1109  *
1110  */
1112 {
1115 
1120 
1121  /* Calculate binning/skipping/scaling for requested sensor domain and output resolution */
1122  mt9f002_calc_resolution(&mt9f002);
1123 
1124  mt9f002_set_resolution(&mt9f002);
1125 
1126  /* Update blanking (based on FPS) */
1127  mt9f002_set_blanking(&mt9f002);
1128 
1129  // Update the isp_config
1130  isp_request_statistics_yuv_window(0, mt9f002.sensor_width, 0, mt9f002.sensor_height, 1, 1);
1131 }
1132 
1134 {
1135  mt9f002_set_gains(&mt9f002);
1136 }
1137 
1139 {
1140  mt9f002_set_exposure(&mt9f002);
1141 }
1142 
#define MT9F002_DAC_LD_30_31
Definition: mt9f002_regs.h:622
#define MT9F002_MFR_3E16
Definition: mt9f002_regs.h:521
uint16_t min_line_blanking_pck
Definition: mt9f002.c:166
#define MT9F002_MFR_3E04
Definition: mt9f002_regs.h:512
#define MT9F002_MFR_3E3E
Definition: mt9f002_regs.h:541
static void mt9f002_parallel_stage2(struct mt9f002_t *mt)
Configure stage 2 for parallel connection.
Definition: mt9f002.c:594
#define MT9F002_SOFTWARE_RESET
Definition: mt9f002_regs.h:54
unsigned short uint16_t
Definition: types.h:16
#define MT9F002_MFR_3D88
Definition: mt9f002_regs.h:450
#define CFG_MT9F002_X_ADDR_MAX
Definition: mt9f002.h:41
#define MT9F002_MFR_3E4A
Definition: mt9f002_regs.h:547
#define MT9F002_MFR_3E26
Definition: mt9f002_regs.h:529
#define MT9F002_MFR_3E8A
Definition: mt9f002_regs.h:579
#define MT9F002_MFR_317C
Definition: mt9f002_regs.h:613
void mt9f002_setting_update_exposure(float in)
Definition: mt9f002.c:1138
#define MT9F002_MFR_3E32
Definition: mt9f002_regs.h:535
#define MT9F002_MFR_3D3A
Definition: mt9f002_regs.h:411
struct blanking_t mt9f002_blanking
#define MT9F002_MFR_3E8C
Definition: mt9f002_regs.h:580
uint16_t fine_integration_time_min
Definition: mt9f002.c:169
#define MT9F002_MFR_3D32
Definition: mt9f002_regs.h:407
#define MT9F002_X_ADDR_END
Definition: mt9f002_regs.h:82
uint16_t sensor_width
Definition: mt9f002.h:168
#define MT9F002_CENTER_X
Definition: mt9f002.h:116
float set_zoom
Image zoom set point.
Definition: mt9f002.h:177
#define MT9F002_MFR_3D14
Definition: mt9f002_regs.h:392
#define MT9F002_MFR_3E66
Definition: mt9f002_regs.h:561
#define MT9F002_MFR_3DC8
Definition: mt9f002_regs.h:482
#define MT9F002_MFR_3E0E
Definition: mt9f002_regs.h:517
#define Min(x, y)
Definition: esc_dshot.c:85
#define MT9F002_MFR_3EC4
Definition: mt9f002_regs.h:608
#define MT9F002_MFR_3E94
Definition: mt9f002_regs.h:584
#define MT9F002_MFR_3D7C
Definition: mt9f002_regs.h:444
#define MT9F002_FINE_INTEGRATION_TIME_
Definition: mt9f002_regs.h:177
void mt9f002_set_exposure(struct mt9f002_t *mt)
Set the exposure configuration Depends on the blanking (and therefore the FPS)
Definition: mt9f002.c:799
#define MT9F002_MFR_3E7C
Definition: mt9f002_regs.h:572
#define MT9F002_MFR_3E8E
Definition: mt9f002_regs.h:581
#define MT9F002_MFR_3E1C
Definition: mt9f002_regs.h:524
#define CFG_MT9F002_Y_ADDR_MIN
Definition: mt9f002.h:42
#define MT9F002_MFR_3DC4
Definition: mt9f002_regs.h:480
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
#define MT9F002_MFR_3EB4
Definition: mt9f002_regs.h:600
#define MT9F002_LINE_LENGTH_PCK
Definition: mt9f002_regs.h:79
#define MT9F002_MFR_3EBC
Definition: mt9f002_regs.h:604
uint8_t x_odd_inc
X increment for subsampling (1,3,7,15,31 accepted)
Definition: mt9f002.h:171
#define MT9F002_MASK_CORRUPTED_FRAMES
Definition: mt9f002_regs.h:56
#define MT9F002_MFR_3E46
Definition: mt9f002_regs.h:545
#define MT9F002_CTX_WR_DATA_REG
Definition: mt9f002_regs.h:235
#define MT9F002_MFR_3EAE
Definition: mt9f002_regs.h:597
#define MT9F002_MFR_3E6A
Definition: mt9f002_regs.h:563
#define MT9F002_MIN_FRAME_BLANKING_LINES
Definition: mt9f002_regs.h:136
#define MT9F002_MFR_3E4C
Definition: mt9f002_regs.h:548
uint16_t output_height
Output height.
Definition: mt9f002.h:161
#define MT9F002_LINE_LENGTH_MAX
Definition: mt9f002_regs.h:7
uint16_t op_sys_clk_div
Fixed PLL config from calculator tool.
Definition: mt9f002.h:141
#define MT9F002_MFR_3D86
Definition: mt9f002_regs.h:449
struct img_size_t output_size
Output image size.
Definition: video_device.h:56
uint8_t rowSpeed_2_0
Fixed PLL config from calculator tool.
Definition: mt9f002.h:143
#define MT9F002_MFR_3DF8
Definition: mt9f002_regs.h:506
#define MT9F002_ZOOM
Definition: mt9f002.h:101
#define MT9F002_MFR_3EB8
Definition: mt9f002_regs.h:602
#define MT9F002_MFR_3D20
Definition: mt9f002_regs.h:398
#define MT9F002_MFR_3DB8
Definition: mt9f002_regs.h:474
#define MT9F002_OUTPUT_HEIGHT
Definition: mt9f002.h:85
uint16_t vt_pix_clk_div
Fixed PLL config from calculator tool.
Definition: mt9f002.h:136
#define MT9F002_MFR_3D46
Definition: mt9f002_regs.h:417
#define MT9F002_MFR_3DDA
Definition: mt9f002_regs.h:491
#define MT9F002_MFR_3DCC
Definition: mt9f002_regs.h:484
#define MT9F002_MFR_3D18
Definition: mt9f002_regs.h:394
#define MT9F002_MFR_3D2C
Definition: mt9f002_regs.h:404
#define MT9F002_DAC_LD_14_15
Definition: mt9f002_regs.h:619
#define MT9F002_MFR_3E7A
Definition: mt9f002_regs.h:571
#define MT9F002_MFR_3E72
Definition: mt9f002_regs.h:567
#define MT9F002_MFR_3DB0
Definition: mt9f002_regs.h:470
#define MT9F002_MFR_3D7A
Definition: mt9f002_regs.h:443
#define MT9F002_MFR_3D6A
Definition: mt9f002_regs.h:435
uint16_t pll_multiplier
Fixed PLL config from calculator tool.
Definition: mt9f002.h:139
#define MT9F002_GAIN_RED
Definition: mt9f002.c:82
#define MT9F002_SCALING_MODE
Definition: mt9f002_regs.h:90
#define MT9F002_MFR_3D9A
Definition: mt9f002_regs.h:459
#define MT9F002_MFR_3D38
Definition: mt9f002_regs.h:410
#define MT9F002_MFR_3EE6
Definition: mt9f002_regs.h:614
static void mt9f002_mipi_stage1(struct mt9f002_t *mt)
Configure stage 1 for both MiPi and HiSPi connection.
Definition: mt9f002.c:222
#define MT9F002_MFR_3DDE
Definition: mt9f002_regs.h:493
#define MT9F002_MFR_3E3C
Definition: mt9f002_regs.h:540
#define MT9F002_MFR_3DD6
Definition: mt9f002_regs.h:489
#define MT9F002_BLUE_GAIN
Definition: mt9f002_regs.h:205
#define MT9F002_CENTER_Y
Definition: mt9f002.h:119
#define MT9F002_MFR_3DA0
Definition: mt9f002_regs.h:462
#define MT9F002_X_ODD_INC
Definition: mt9f002_regs.h:87
#define MT9F002_MFR_3D64
Definition: mt9f002_regs.h:432
float set_offset_x
Signed fractional offset from centre of image of original sensor [-0.5,0.5].
Definition: mt9f002.h:178
#define MT9F002_MFR_3D60
Definition: mt9f002_regs.h:430
#define MT9F002_MFR_3D4E
Definition: mt9f002_regs.h:421
#define MT9F002_MFR_3DAE
Definition: mt9f002_regs.h:469
#define MT9F002_MFR_3E28
Definition: mt9f002_regs.h:530
#define MT9F002_MFR_3E48
Definition: mt9f002_regs.h:546
#define MT9F002_MFR_3E38
Definition: mt9f002_regs.h:538
#define MT9F002_ADDRESS
Definition: mt9f002_regs.h:4
#define MT9F002_MFR_3D96
Definition: mt9f002_regs.h:457
#define MT9F002_MFR_3E6C
Definition: mt9f002_regs.h:564
#define MT9F002_TARGET_EXPOSURE
Exposure of the front camera of the bebop.
Definition: mt9f002.c:69
float set_offset_y
Signed fractional offset from centre of image of original sensor [-0.5,0.5].
Definition: mt9f002.h:179
#define MT9F002_COARSE_INTEGRATION_TIME_MIN
Definition: mt9f002_regs.h:105
#define MT9F002_DIGITAL_TEST
Definition: mt9f002_regs.h:226
#define MT9F002_MFR_3D24
Definition: mt9f002_regs.h:400
#define MT9F002_MFR_3DC0
Definition: mt9f002_regs.h:478
#define MT9F002_ANALOG_CONTROL4
Definition: mt9f002_regs.h:248
#define MT9F002_MFR_3E9C
Definition: mt9f002_regs.h:588
#define MT9F002_DAC_LD_28_29
Definition: mt9f002_regs.h:621
#define MT9F002_MFR_3E86
Definition: mt9f002_regs.h:577
static uint32_t read_reg(struct mt9f002_t *mt, uint16_t addr, uint8_t len)
Read multiple bytes from a register.
Definition: mt9f002.c:203
float gain_blue
Gain for the Blue pixels [1., 63.50].
Definition: mt9f002.h:156
#define MT9F002_READ_MODE
Definition: mt9f002_regs.h:201
#define MT9F002_CTX_CONTROL_REG
Definition: mt9f002_regs.h:234
#define MT9F002_MFR_3D7E
Definition: mt9f002_regs.h:445
#define MT9F002_MFR_3EE2
Definition: mt9f002_regs.h:617
#define MT9F002_MFR_3D1A
Definition: mt9f002_regs.h:395
#define MT9F002_MFR_3E5A
Definition: mt9f002_regs.h:555
#define MT9F002_MFR_3E92
Definition: mt9f002_regs.h:583
#define MT9F002_MFR_3DF4
Definition: mt9f002_regs.h:504
#define MT9F002_VT_PIX_CLK_DIV
Definition: mt9f002_regs.h:72
uint16_t offset_x
Offset from left in pixels.
Definition: mt9f002.h:165
#define MT9F002_MFR_3D1C
Definition: mt9f002_regs.h:396
#define MT9F002_MFR_3DAC
Definition: mt9f002_regs.h:468
#define MT9F002_MFR_3DD4
Definition: mt9f002_regs.h:488
#define MT9F002_MFR_3E5E
Definition: mt9f002_regs.h:557
#define MT9F002_MFR_3EBA
Definition: mt9f002_regs.h:603
HiSPi type connection.
Definition: mt9f002.h:128
#define MT9F002_MFR_3DA6
Definition: mt9f002_regs.h:465
static void mt9f002_set_resolution(struct mt9f002_t *mt)
Definition: mt9f002.c:1000
#define MT9F002_MFR_3D44
Definition: mt9f002_regs.h:416
#define MT9F002_MFR_3E2C
Definition: mt9f002_regs.h:532
#define MT9F002_MFR_3E40
Definition: mt9f002_regs.h:542
uint8_t y_odd_inc
Y increment for subsampling (1,3,7,15,31 accepted)
Definition: mt9f002.h:172
uint16_t vt_sys_clk_div
Fixed PLL config from calculator tool.
Definition: mt9f002.h:137
#define MT9F002_MFR_3E62
Definition: mt9f002_regs.h:559
#define MT9F002_MFR_3E34
Definition: mt9f002_regs.h:536
#define MT9F002_MFR_3E54
Definition: mt9f002_regs.h:552
#define MT9F002_MFR_3E0A
Definition: mt9f002_regs.h:515
#define MT9F002_MFR_3D9E
Definition: mt9f002_regs.h:461
#define MT9F002_MFR_3D28
Definition: mt9f002_regs.h:402
#define MT9F002_OP_SYS_CLK_DIV
Definition: mt9f002_regs.h:77
#define MT9F002_SCALER_N
Definition: mt9f002_regs.h:6
#define MT9F002_MFR_3D0E
Definition: mt9f002_regs.h:389
#define MT9F002_DAC_LD_24_25
Definition: mt9f002_regs.h:620
#define MT9F002_MFR_3E64
Definition: mt9f002_regs.h:560
#define MT9F002_SERIAL_FORMAT
Definition: mt9f002_regs.h:258
bool i2c_blocking_transceive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len_w, uint16_t len_r)
Submit a write/read transaction and wait for it to complete.
Definition: i2c.c:403
uint16_t output_scaler
Output scaler.
Definition: mt9f002.h:162
#define MT9F002_MFR_3EDC
Definition: mt9f002_regs.h:616
#define MT9F002_DATA_PEDESTAL_
Definition: mt9f002_regs.h:183
#define MT9F002_ANALOG_CONTROL5
Definition: mt9f002_regs.h:249
#define MT9F002_MFR_3E56
Definition: mt9f002_regs.h:553
void mt9f002_init(struct mt9f002_t *mt)
Initialisation of the Aptina MT9F002 CMOS sensor (front camera)
Definition: mt9f002.c:1036
#define MT9F002_MFR_3D48
Definition: mt9f002_regs.h:418
#define MT9F002_MFR_3DEC
Definition: mt9f002_regs.h:500
#define MT9F002_MFR_3D78
Definition: mt9f002_regs.h:442
#define MT9F002_MFR_3DFA
Definition: mt9f002_regs.h:507
#define MT9F002_MFR_3D8E
Definition: mt9f002_regs.h:453
uint16_t fine_integration_time_max_margin
Definition: mt9f002.c:170
#define MT9F002_Y_OUTPUT_SIZE
Definition: mt9f002_regs.h:85
#define MT9F002_MFR_3DC2
Definition: mt9f002_regs.h:479
#define MT9F002_MFR_3EC8
Definition: mt9f002_regs.h:610
#define MT9F002_MFR_3D40
Definition: mt9f002_regs.h:414
#define MT9F002_MFR_3DB6
Definition: mt9f002_regs.h:473
#define MT9F002_MFR_3D66
Definition: mt9f002_regs.h:433
#define MT9F002_MFR_3E22
Definition: mt9f002_regs.h:527
uint8_t shift_vt_pix_clk_div
Fixed PLL config from calculator tool.
Definition: mt9f002.h:142
#define MT9F002_OFFSET_X
Definition: mt9f002.h:91
#define MT9F002_MFR_3E60
Definition: mt9f002_regs.h:558
#define MT9F002_FOCAL_Y
Definition: mt9f002.h:113
#define MT9F002_MFR_3D50
Definition: mt9f002_regs.h:422
#define MT9F002_MFR_3EB0
Definition: mt9f002_regs.h:598
#define MT9F002_COARSE_INTEGRATION_TIME
Definition: mt9f002_regs.h:62
#define MT9F002_MFR_3DA2
Definition: mt9f002_regs.h:463
uint8_t row_speed_10_8
Fixed PLL config from calculator tool.
Definition: mt9f002.h:144
#define MT9F002_MFR_3D94
Definition: mt9f002_regs.h:456
#define MT9F002_MFR_3E98
Definition: mt9f002_regs.h:586
void mt9f002_setting_update_color(float in)
Definition: mt9f002.c:1133
#define MT9F002_MFR_3DFC
Definition: mt9f002_regs.h:508
#define MT9F002_MFR_3E2A
Definition: mt9f002_regs.h:531
#define MT9F002_MFR_3DA4
Definition: mt9f002_regs.h:464
transaction set to done by user level
Definition: i2c.h:59
#define VERBOSE_PRINT(...)
Definition: mt9f002.c:53
#define MT9F002_MFR_3D4C
Definition: mt9f002_regs.h:420
enum mt9f002_interface interface
Interface used to connect.
Definition: mt9f002.h:134
#define MT9F002_MFR_3D52
Definition: mt9f002_regs.h:423
#define MT9F002_GREEN1_GAIN
Definition: mt9f002_regs.h:204
static void mt9f002_mipi_stage3(struct mt9f002_t *mt)
Configure stage 3 for both MiPi and HiSPi connection.
Definition: mt9f002.c:486
#define MT9F002_MFR_3DB4
Definition: mt9f002_regs.h:472
struct mt9f002_t mt9f002
Definition: mt9f002.c:123
#define MT9F002_MFR_3D90
Definition: mt9f002_regs.h:454
#define MT9F002_MFR_3D98
Definition: mt9f002_regs.h:458
#define MT9F002_MFR_3DDC
Definition: mt9f002_regs.h:492
static uint16_t mt9f002_calc_gain(float gain)
Calculate the gain based on value of 1.0 -> 63.50.
Definition: mt9f002.c:860
uint16_t line_length
Calculated line length of blanking.
Definition: mt9f002.h:147
uint16_t min_line_length_pck
Definition: mt9f002.c:167
#define MT9F002_MFR_3D68
Definition: mt9f002_regs.h:434
#define MT9F002_DHANE_K
Definition: mt9f002.h:122
float real_fps
Real calculated FPS.
Definition: mt9f002.h:151
#define MT9F002_MFR_3E6E
Definition: mt9f002_regs.h:565
#define MT9F002_GAIN_BLUE
Definition: mt9f002.c:86
#define MT9F002_DATAPATH_SELECT
Definition: mt9f002_regs.h:211
#define MT9F002_RESET_REGISTER
Definition: mt9f002_regs.h:180
#define MT9F002_MFR_3DE6
Definition: mt9f002_regs.h:497
static void mt9f002_parallel_stage1(struct mt9f002_t *mt)
Configure stage 1 for parallel connection.
Definition: mt9f002.c:497
#define MT9F002_MFR_3E96
Definition: mt9f002_regs.h:585
#define MT9F002_MFR_3D3E
Definition: mt9f002_regs.h:413
#define MT9F002_MFR_3DBA
Definition: mt9f002_regs.h:475
#define MT9F002_MFR_3DB2
Definition: mt9f002_regs.h:471
uint16_t val[TCOUPLE_NB]
#define MT9F002_MFR_3E12
Definition: mt9f002_regs.h:519
#define MT9F002_MFR_3D62
Definition: mt9f002_regs.h:431
unsigned long uint32_t
Definition: types.h:18
static void mt9f002_set_pll(struct mt9f002_t *mt)
Set the PLL registers based on config.
Definition: mt9f002.c:632
#define MT9F002_MFR_3EA2
Definition: mt9f002_regs.h:591
#define CFG_MT9F002_X_ADDR_MIN
Definition: mt9f002.h:40
#define MT9F002_MFR_3D8A
Definition: mt9f002_regs.h:451
float gain_green1
Gain for the GreenR pixels [1., 63.50].
Definition: mt9f002.h:155
#define MT9F002_ANALOG_CONTROL7
Definition: mt9f002_regs.h:250
uint16_t output_width
Output width.
Definition: mt9f002.h:160
#define MT9F002_MFR_3DFE
Definition: mt9f002_regs.h:509
#define MT9F002_MFR_3E24
Definition: mt9f002_regs.h:528
#define MT9F002_MFR_3DAA
Definition: mt9f002_regs.h:467
#define MT9F002_MFR_3E80
Definition: mt9f002_regs.h:574
#define MT9F002_MFR_3DE4
Definition: mt9f002_regs.h:496
#define MT9F002_MFR_3EB2
Definition: mt9f002_regs.h:599
#define MT9F002_MFR_3D12
Definition: mt9f002_regs.h:391
#define MT9F002_FRAME_LENGTH_LINES
Definition: mt9f002_regs.h:78
static void write_reg(struct mt9f002_t *mt, uint16_t addr, uint32_t val, uint8_t len)
Write multiple bytes to a single register.
Definition: mt9f002.c:176
#define MT9F002_MFR_3DE8
Definition: mt9f002_regs.h:498
#define MT9F002_SCALE_M
Definition: mt9f002_regs.h:92
#define MT9F002_MFR_3EB6
Definition: mt9f002_regs.h:601
#define MT9F002_MFR_3DEA
Definition: mt9f002_regs.h:499
#define MT9F002_MFR_3E90
Definition: mt9f002_regs.h:582
void mt9f002_set_gains(struct mt9f002_t *mt)
Sets the GreenR, Blue, Red and GreenB gains.
Definition: mt9f002.c:906
#define MT9F002_MFR_3E20
Definition: mt9f002_regs.h:526
float vt_pix_clk
Calculated based on PLL.
Definition: mt9f002.h:145
#define MT9F002_MFR_3E9A
Definition: mt9f002_regs.h:587
#define MT9F002_MFR_3E58
Definition: mt9f002_regs.h:554
#define MT9F002_MFR_3D26
Definition: mt9f002_regs.h:401
float target_fps
FPS wanted.
Definition: mt9f002.h:150
#define MT9F002_X_OUTPUT_SIZE
Definition: mt9f002_regs.h:84
#define MT9F002_COLUMN_CORRECTION
Definition: mt9f002_regs.h:233
#define MT9F002_MFR_3E76
Definition: mt9f002_regs.h:569
#define MT9F002_DARK_CONTROL3
Definition: mt9f002_regs.h:237
#define MT9F002_MFR_3EAA
Definition: mt9f002_regs.h:595
#define MT9F002_MFR_3D74
Definition: mt9f002_regs.h:440
#define MT9F002_Y_ADDR_END
Definition: mt9f002_regs.h:83
#define MT9F002_MFR_3D10
Definition: mt9f002_regs.h:390
#define MT9F002_MFR_3DBC
Definition: mt9f002_regs.h:476
#define MT9F002_EXTRA_DELAY
Definition: mt9f002_regs.h:179
#define MT9F002_MFR_3E08
Definition: mt9f002_regs.h:514
#define MT9F002_MFR_3E5C
Definition: mt9f002_regs.h:556
float gain_red
Gain for the Red pixels [1., 63.50].
Definition: mt9f002.h:157
#define MT9F002_MFR_3D84
Definition: mt9f002_regs.h:448
#define MT9F002_MFR_3D0C
Definition: mt9f002_regs.h:388
#define Max(x, y)
Definition: main_fbw.c:53
#define MT9F002_MFR_3D06
Definition: mt9f002_regs.h:385
#define MT9F002_MFR_3E0C
Definition: mt9f002_regs.h:516
#define MT9F002_OUTPUT_WIDTH
Definition: mt9f002.h:82
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
#define MT9F002_GAIN_GREEN1
Definition: mt9f002.c:74
#define MT9F002_MFR_3DD2
Definition: mt9f002_regs.h:487
#define MT9F002_MFR_3E52
Definition: mt9f002_regs.h:551
#define MT9F002_MFR_3D1E
Definition: mt9f002_regs.h:397
#define MT9F002_MFR_3D58
Definition: mt9f002_regs.h:426
#define MT9F002_COARSE_INTEGRATION_TIME_MAX_MARGIN
Definition: mt9f002_regs.h:106
signed long int32_t
Definition: types.h:19
#define MT9F002_MFR_3EC6
Definition: mt9f002_regs.h:609
#define MT9F002_MFR_3D92
Definition: mt9f002_regs.h:455
#define MT9F002_MFR_3E3A
Definition: mt9f002_regs.h:539
#define MT9F002_MFR_3D4A
Definition: mt9f002_regs.h:419
struct i2c_periph * i2c_periph
I2C peripheral used to communicate over.
Definition: mt9f002.h:174
float gain_green2
Gain for the GreenB pixels [1., 63.50].
Definition: mt9f002.h:158
#define MT9F002_MFR_3D5C
Definition: mt9f002_regs.h:428
#define MT9F002_VT_SYS_CLK_DIV
Definition: mt9f002_regs.h:73
#define MT9F002_MFR_3DBE
Definition: mt9f002_regs.h:477
Initialization and configuration of the MT9F002 CMOS Chip.
#define MT9F002_MFR_3E50
Definition: mt9f002_regs.h:550
#define MT9F002_MODE_SELECT
Definition: mt9f002_regs.h:52
uint16_t offset_y
Offset from top in pixels.
Definition: mt9f002.h:166
#define MT9F002_OP_PIX_CLK_DIV
Definition: mt9f002_regs.h:76
#define CFG_SCALER_M_MAX
Definition: mt9f002.h:35
float real_exposure
Real exposure time in ms.
Definition: mt9f002.h:153
#define MT9F002_MFR_3D54
Definition: mt9f002_regs.h:424
#define MT9F002_MFR_3DE2
Definition: mt9f002_regs.h:495
#define MT9F002_MFR_3E18
Definition: mt9f002_regs.h:522
#define MT9F002_MFR_3E42
Definition: mt9f002_regs.h:543
#define MT9F002_GREEN2_GAIN
Definition: mt9f002_regs.h:207
#define MT9F002_MFR_3ECA
Definition: mt9f002_regs.h:611
#define MT9F002_SMIA_TEST
Definition: mt9f002_regs.h:209
#define MT9F002_MFR_3D8C
Definition: mt9f002_regs.h:452
#define MT9F002_MFR_3EC2
Definition: mt9f002_regs.h:607
struct i2c_transaction i2c_trans
I2C transaction for communication with CMOS chip.
Definition: mt9f002.h:175
#define PRINT(string,...)
Definition: mt9f002.c:48
#define MT9F002_MFR_3E1A
Definition: mt9f002_regs.h:523
#define MT9F002_GLOBAL_GAIN
Definition: mt9f002_regs.h:208
uint16_t min_line_fifo_pck
Definition: mt9f002.c:168
#define MT9F002_MFR_3D04
Definition: mt9f002_regs.h:384
#define MT9F002_MFR_3E82
Definition: mt9f002_regs.h:575
#define MT9F002_MFR_3E44
Definition: mt9f002_regs.h:544
#define MT9F002_MFR_3E02
Definition: mt9f002_regs.h:511
#define VIDEO_FILTER_ISP
Enable ISP.
Definition: video_device.h:35
#define MT9F002_MFR_3ED8
Definition: mt9f002_regs.h:615
#define MT9F002_MFR_3DF0
Definition: mt9f002_regs.h:502
unsigned char uint8_t
Definition: types.h:14
#define MT9F002_Y_ADDR_START
Definition: mt9f002_regs.h:81
#define MT9F002_MFR_3D80
Definition: mt9f002_regs.h:446
#define MT9F002_MFR_3DC6
Definition: mt9f002_regs.h:481
#define MT9F002_MFR_3E74
Definition: mt9f002_regs.h:568
#define MT9F002_MFR_3D6C
Definition: mt9f002_regs.h:436
#define MT9F002_MFR_3D56
Definition: mt9f002_regs.h:425
#define MT9F002_MFR_3D2E
Definition: mt9f002_regs.h:405
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)
Definition: libisp.c:214
#define CFG_MT9F002_WINDOW_HEIGHT_MIN
Definition: mt9f002.h:37
#define MT9F002_CPP_DATA_FORMAT
Definition: mt9f002_regs.h:59
#define MT9F002_MFR_3E10
Definition: mt9f002_regs.h:518
#define MT9F002_MFR_3E00
Definition: mt9f002_regs.h:510
#define MT9F002_MFR_3DD8
Definition: mt9f002_regs.h:490
#define MT9F002_MFR_3EAC
Definition: mt9f002_regs.h:596
#define MT9F002_MFR_3E36
Definition: mt9f002_regs.h:537
uint16_t op_pix_clk_div
Fixed PLL config from calculator tool.
Definition: mt9f002.h:140
#define MT9F002_MFR_3E70
Definition: mt9f002_regs.h:566
#define MT9F002_MFR_3EA4
Definition: mt9f002_regs.h:592
#define MT9F002_MFR_3EBE
Definition: mt9f002_regs.h:605
#define CFG_MT9F002_PIXEL_ARRAY_WIDTH
Definition: mt9f002.h:39
#define MT9F002_MFR_3D76
Definition: mt9f002_regs.h:441
#define MT9F002_MFR_3D16
Definition: mt9f002_regs.h:393
#define CFG_MT9F002_WINDOW_WIDTH_MIN
Definition: mt9f002.h:36
#define MT9F002_MFR_3EA0
Definition: mt9f002_regs.h:590
#define MT9F002_MFR_3E2E
Definition: mt9f002_regs.h:533
#define MT9F002_MFR_3D08
Definition: mt9f002_regs.h:386
#define CFG_MT9F002_Y_ADDR_MAX
Definition: mt9f002.h:43
#define MT9F002_MFR_3D5E
Definition: mt9f002_regs.h:429
#define MT9F002_PLL_MULTIPLIER
Definition: mt9f002_regs.h:75
void mt9f002_reset_exposure(struct mt9f002_t *mt)
Definition: mt9f002.c:1102
#define MT9F002_MFR_3DF6
Definition: mt9f002_regs.h:505
void mt9f002_setting_update_resolution(float in)
Definition: mt9f002.c:1111
#define MT9F002_MFR_3D42
Definition: mt9f002_regs.h:415
float op_pix_clk
Calculated based on PLL.
Definition: mt9f002.h:146
#define MT9F002_MFR_3E4E
Definition: mt9f002_regs.h:549
#define MT9F002_Y_ODD_INC
Definition: mt9f002_regs.h:89
#define MT9F002_MFR_3176
Definition: mt9f002_regs.h:612
uint16_t pre_pll_clk_div
Fixed PLL config from calculator tool.
Definition: mt9f002.h:138
Parallel type connection.
Definition: mt9f002.h:129
#define MT9F002_MFR_3DEE
Definition: mt9f002_regs.h:501
#define MT9F002_MFR_3D30
Definition: mt9f002_regs.h:406
#define MT9F002_MFR_3EC0
Definition: mt9f002_regs.h:606
#define MT9F002_MFR_3E68
Definition: mt9f002_regs.h:562
#define MT9F002_MFR_3DCA
Definition: mt9f002_regs.h:483
#define MT9F002_MFR_3D70
Definition: mt9f002_regs.h:438
#define MT9F002_RED_GAIN
Definition: mt9f002_regs.h:206
#define MT9F002_MFR_3D00
Definition: mt9f002_regs.h:382
static void mt9f002_set_blanking(struct mt9f002_t *mt)
Definition: mt9f002.c:659
#define MT9F002_MFR_3EE8
Definition: mt9f002_regs.h:618
#define MT9F002_MFR_3D3C
Definition: mt9f002_regs.h:412
#define MT9F002_MFR_3EA6
Definition: mt9f002_regs.h:593
#define MT9F002_MFR_3D0A
Definition: mt9f002_regs.h:387
#define MT9F002_MFR_3DF2
Definition: mt9f002_regs.h:503
#define MT9F002_FRAME_LENGTH_MAX
Definition: mt9f002_regs.h:8
#define MT9F002_MFR_3E78
Definition: mt9f002_regs.h:570
void mt9f002_reset_color(struct mt9f002_t *mt)
Definition: mt9f002.c:1093
#define MT9F002_MFR_3E1E
Definition: mt9f002_regs.h:525
#define MT9F002_MFR_3DE0
Definition: mt9f002_regs.h:494
#define MT9F002_MFR_3E7E
Definition: mt9f002_regs.h:573
#define MT9F002_MFR_3E88
Definition: mt9f002_regs.h:578
#define MT9F002_MFR_3D5A
Definition: mt9f002_regs.h:427
#define MT9F002_OFFSET_Y
Definition: mt9f002.h:96
uint32_t int32_gcd(uint32_t a, uint32_t b)
#define MT9F002_MFR_3D82
Definition: mt9f002_regs.h:447
uint16_t w
The width.
Definition: image.h:75
#define MT9F002_MFR_3E84
Definition: mt9f002_regs.h:576
#define MT9F002_TARGET_FPS
Definition: mt9f002.h:105
#define CFG_MT9F002_PIXEL_ARRAY_HEIGHT
Definition: mt9f002.h:38
static void mt9f002_mipi_stage2(struct mt9f002_t *mt)
Configure stage 2 for both MiPi and HiSPi connection.
Definition: mt9f002.c:478
#define MT9F002_MFR_3D72
Definition: mt9f002_regs.h:439
uint16_t sensor_height
Definition: mt9f002.h:169
#define MT9F002_MFR_3D22
Definition: mt9f002_regs.h:399
#define MT9F002_MFR_3DCE
Definition: mt9f002_regs.h:485
#define MT9F002_PRE_PLL_CLK_DIV
Definition: mt9f002_regs.h:74
#define MT9F002_MFR_3D36
Definition: mt9f002_regs.h:409
#define MT9F002_MFR_3E06
Definition: mt9f002_regs.h:513
static void mt9f002_calc_resolution(struct mt9f002_t *mt)
Definition: mt9f002.c:914
#define MT9F002_MFR_3D2A
Definition: mt9f002_regs.h:403
#define MT9F002_FOCAL_X
Definition: mt9f002.h:110
#define MT9F002_X_ADDR_START
Definition: mt9f002_regs.h:80
#define MT9F002_ROW_SPEED
Definition: mt9f002_regs.h:178
#define MT9F002_MFR_3E9E
Definition: mt9f002_regs.h:589
V4L2 device settings.
Definition: video_device.h:55
uint16_t frame_length
Calculated frame length of blanking.
Definition: mt9f002.h:148
float input_clk_freq
Input clock frequency.
Definition: mt9f002.h:135
#define MT9F002_MFR_3D34
Definition: mt9f002_regs.h:408
MIPI type connection.
Definition: mt9f002.h:127
#define MT9F002_MFR_3DA8
Definition: mt9f002_regs.h:466
#define MT9F002_MFR_3D02
Definition: mt9f002_regs.h:383
#define MT9F002_GAIN_GREEN2
Definition: mt9f002.c:78
float target_exposure
Target exposure time in ms.
Definition: mt9f002.h:152
#define MT9F002_MFR_3E14
Definition: mt9f002_regs.h:520
bool i2c_blocking_transmit(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len)
Submit a write only transaction and wait for it to complete.
Definition: i2c.c:359
Paparazzi fixed point algebra.
#define MT9F002_MFR_3EA8
Definition: mt9f002_regs.h:594
#define MT9F002_MFR_3D6E
Definition: mt9f002_regs.h:437
#define MT9F002_MFR_3E30
Definition: mt9f002_regs.h:534
#define MT9F002_MFR_3D9C
Definition: mt9f002_regs.h:460
struct video_config_t front_camera
Definition: mt9f002.c:90
#define MT9F002_MFR_3DD0
Definition: mt9f002_regs.h:486