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