APM:Libraries
CameraSensor_Mt9v117.cpp
Go to the documentation of this file.
1 /*
2  This program is free software: you can redistribute it and/or modify
3  it under the terms of the GNU General Public License as published by
4  the Free Software Foundation, either version 3 of the License, or
5  (at your option) any later version.
6 
7  This program is distributed in the hope that it will be useful,
8  but WITHOUT ANY WARRANTY; without even the implied warranty of
9  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10  GNU General Public License for more details.
11 
12  You should have received a copy of the GNU General Public License
13  along with this program. If not, see <http://www.gnu.org/licenses/>.
14  */
15 #include <AP_HAL/AP_HAL.h>
16 
17 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
18 #include "CameraSensor_Mt9v117.h"
19 
20 #include <utility>
21 
22 #include "GPIO.h"
23 
24 /* Cam sensor register definitions */
25 #define CHIP_ID 0x0
26 #define MT9V117_CHIP_ID 0x2282
27 #define COMMAND_REGISTER 0x0040
28 #define HOST_COMMAND_OK (1 << 15)
29 #define HOST_COMMAND_2 (1 << 2)
30 #define HOST_COMMAND_1 (1 << 1)
31 #define HOST_COMMAND_0 (1 << 0)
32 #define PAD_SLEW 0x0030
33 #define RESET_AND_MISC_CONTROL 0x001a
34 #define RESET_SOC_I2C (1 << 0)
35 #define ACCESS_CTL_STAT 0x0982
36 #define PHYSICAL_ADDRESS_ACCESS 0x098a
37 #define LOGICAL_ADDRESS_ACCESS 0x098e
38 #define AE_TRACK_JUMP_DIVISOR 0xa812
39 #define CAM_AET_SKIP_FRAMES 0xc868
40 
41 #define AE_RULE_VAR 9
42 #define AE_RULE_ALGO_OFFSET 4
43 #define AE_RULE_ALGO_AVERAGE 0
44 #define AE_RULE_ALGO_WEIGHTED 1
45 #define AE_TRACK_VAR 10
46 #define AWB_VAR 11
47 #define AWB_PIXEL_THRESHOLD_COUNT_OFFSET 64
48 #define LOW_LIGHT_VAR 15
49 #define CAM_CTRL_VAR 18
50 #define CAM_SENSOR_CFG_Y_ADDR_START_OFFSET 0
51 #define CAM_SENSOR_CFG_X_ADDR_START_OFFSET 2
52 #define CAM_SENSOR_CFG_Y_ADDR_END_OFFSET 4
53 #define CAM_SENSOR_CFG_X_ADDR_END_OFFSET 6
54 #define CAM_SENSOR_CFG_FRAME_LENGTH_LINES_OFFSET 14
55 #define CAM_SENSOR_CFG_CPIPE_LAST_ROW_OFFSET 20
56 #define CAM_SENSOR_CFG_FDPERIOD_60HZ 22
57 #define CAM_SENSOR_CFG_FDPERIOD_50HZ 24
58 #define CAM_SENSOR_CFG_MAX_FDZONE_60_OFFSET 26
59 #define CAM_SENSOR_CFG_MAX_FDZONE_50_OFFSET 28
60 #define CAM_SENSOR_CFG_TARGET_FDZONE_60_OFFSET 30
61 #define CAM_SENSOR_CFG_TARGET_FDZONE_50_OFFSET 32
62 #define CAM_SENSOR_CONTROL_READ_MODE_OFFSET 40
63 #define CAM_SENSOR_CONTROL_Y_SKIP_EN (1 << 2)
64 #define CAM_SENSOR_CONTROL_VERT_FLIP_EN (1 << 1)
65 #define CAM_SENSOR_CONTROL_HORZ_MIRROR_EN (1 << 0)
66 #define CAM_FLICKER_PERIOD_OFFSET 62
67 #define CAM_FLICKER_PERIOD_60HZ 0
68 #define CAM_FLICKER_PERIOD_50HZ 1
69 #define CAM_CROP_WINDOW_XOFFSET_OFFSET 72
70 #define CAM_CROP_WINDOW_YOFFSET_OFFSET 74
71 #define CAM_CROP_WINDOW_WIDTH_OFFSET 76
72 #define CAM_CROP_WINDOW_HEIGHT_OFFSET 78
73 #define CAM_CROP_MODE_OFFSET 80
74 #define CAM_OUTPUT_WIDTH_OFFSET 84
75 #define CAM_OUTPUT_HEIGHT_OFFSET 86
76 #define CAM_OUTPUT_FORMAT_OFFSET 88
77 #define CAM_OUTPUT_FORMAT_RGB_565 (0 << 12)
78 #define CAM_OUTPUT_FORMAT_RGB_555 (1 << 12)
79 #define CAM_OUTPUT_FORMAT_RGB_444X (2 << 12)
80 #define CAM_OUTPUT_FORMAT_RGB_X444 (3 << 12)
81 #define CAM_OUTPUT_FORMAT_BAYER_10 (0 << 10)
82 #define CAM_OUTPUT_FORMAT_YUV (0 << 8)
83 #define CAM_OUTPUT_FORMAT_RGB (1 << 8)
84 #define CAM_OUTPUT_FORMAT_BAYER (2 << 8)
85 #define CAM_OUTPUT_FORMAT_BT656_ENABLE (1 << 3)
86 #define CAM_OUTPUT_FORMAT_MONO_ENABLE (1 << 2)
87 #define CAM_OUTPUT_FORMAT_SWAP_BYTES (1 << 1)
88 #define CAM_OUTPUT_FORMAT_SWAP_RED_BLUE (1 << 0)
89 #define CAM_STAT_AWB_HG_WINDOW_XSTART_OFFSET 236
90 #define CAM_STAT_AWB_HG_WINDOW_YSTART_OFFSET 238
91 #define CAM_STAT_AWB_HG_WINDOW_XEND_OFFSET 240
92 #define CAM_STAT_AWB_HG_WINDOW_YEND_OFFSET 242
93 #define CAM_STAT_AE_INITIAL_WINDOW_XSTART_OFFSET 244
94 #define CAM_STAT_AE_INITIAL_WINDOW_YSTART_OFFSET 246
95 #define CAM_STAT_AE_INITIAL_WINDOW_XEND_OFFSET 248
96 #define CAM_STAT_AE_INITIAL_WINDOW_YEND_OFFSET 250
97 #define CAM_LL_START_GAIN_METRIC_OFFSET 278
98 #define CAM_LL_STOP_GAIN_METRIC_OFFSET 280
99 #define SYSMGR_VAR 23
100 #define SYSMGR_NEXT_STATE_OFFSET 0
101 #define PATCHLDR_VAR 24
102 #define PATCHLDR_LOADER_ADDRESS_OFFSET 0
103 #define PATCHLDR_PATCH_ID_OFFSET 2
104 #define PATCHLDR_FIRMWARE_ID_OFFSET 4
105 
106 extern const AP_HAL::HAL& hal;
107 
108 using namespace Linux;
109 
112  enum mt9v117_res res,
113  uint16_t nrst_gpio, uint32_t clock_freq)
114  : CameraSensor(device_path)
115  , _dev(std::move(dev))
116  , _nrst_gpio(nrst_gpio)
117  , _clock_freq(clock_freq)
118 {
119  if (!_dev) {
120  AP_HAL::panic("Could not find I2C bus for CameraSensor_Mt9v117");
121  }
122 
123  switch (res) {
124  case MT9V117_QVGA:
125  _init_sensor();
127  break;
128  default:
129  AP_HAL::panic("mt9v117: unsupported resolution\n");
130  break;
131  }
132 
133  _itu656_enable();
134  _config_change();
135 }
136 
137 uint8_t CameraSensor_Mt9v117::_read_reg8(uint16_t reg)
138 {
139  uint8_t buf[2];
140  buf[0] = (uint8_t) (reg >> 8);
141  buf[1] = (uint8_t) (reg & 0xFF);
142 
143  if (!_dev->transfer(buf, 2, buf, 1)) {
144  hal.console->printf("mt9v117: error reading 0x%2x\n", reg);
145  return 0;
146  }
147 
148  return buf[0];
149 }
150 
151 void CameraSensor_Mt9v117::_write_reg8(uint16_t reg, uint8_t val)
152 {
153  uint8_t buf[3];
154  buf[0] = (uint8_t) (reg >> 8);
155  buf[1] = (uint8_t) (reg & 0xFF);
156  buf[2] = val;
157 
158  if (!_dev->transfer(buf, 3, nullptr, 0)) {
159  hal.console->printf("mt9v117: error writing 0x%2x\n", reg);
160  }
161 }
162 
163 uint16_t CameraSensor_Mt9v117::_read_reg16(uint16_t reg)
164 {
165  uint8_t buf[2];
166  buf[0] = (uint8_t) (reg >> 8);
167  buf[1] = (uint8_t) (reg & 0xFF);
168 
169  if (!_dev->transfer(buf, 2, buf, 2)) {
170  hal.console->printf("mt9v117: error reading 0x%4x\n", reg);
171  return 0;
172  }
173 
174  return (buf[0] << 8 | buf[1]);
175 }
176 
177 void CameraSensor_Mt9v117::_write_reg16(uint16_t reg, uint16_t val)
178 {
179  uint8_t buf[4];
180  buf[0] = (uint8_t) (reg >> 8);
181  buf[1] = (uint8_t) (reg & 0xFF);
182  buf[2] = (uint8_t) (val >> 8);
183  buf[3] = (uint8_t) (val & 0xFF);
184 
185  if (!_dev->transfer(buf, 4, nullptr, 0)) {
186  hal.console->printf("mt9v117: error writing 0x%4x\n", reg);
187  }
188 }
189 
190 void CameraSensor_Mt9v117::_write_reg32(uint16_t reg, uint32_t val)
191 {
192  uint8_t buf[6];
193  buf[0] = (uint8_t) (reg >> 8);
194  buf[1] = (uint8_t) (reg & 0xFF);
195  buf[2] = (uint8_t) (val >> 24);
196  buf[3] = (uint8_t) ((val >> 16) & 0xFF);
197  buf[4] = (uint8_t) ((val >> 8) & 0xFF);
198  buf[5] = (uint8_t) (val & 0xFF);
199 
200  if (!_dev->transfer(buf, 6, nullptr, 0)) {
201  hal.console->printf("mt9v117: error writing 0x%8x\n", reg);
202  }
203 }
204 
205 inline uint16_t CameraSensor_Mt9v117::_var2reg(uint16_t var,
206  uint16_t offset)
207 {
208  return (0x8000 | (var << 10) | offset);
209 }
210 
211 uint16_t CameraSensor_Mt9v117::_read_var16(uint16_t var, uint16_t offset)
212 {
213  uint16_t reg = _var2reg(var, offset);
214  return _read_reg16(reg);
215 }
216 
218  uint16_t offset,
219  uint16_t value)
220 {
221  uint16_t reg = _var2reg(var, offset);
222  _write_reg16(reg, value);
223 }
224 
225 uint8_t CameraSensor_Mt9v117::_read_var8(uint16_t var, uint16_t offset)
226 {
227  uint16_t reg = _var2reg(var, offset);
228  return _read_reg8(reg);
229 }
230 
232  uint16_t offset,
233  uint8_t value)
234 {
235  uint16_t reg = _var2reg(var, offset);
236  return _write_reg8(reg, value);
237 }
238 
240  uint16_t offset,
241  uint32_t value)
242 {
243  uint16_t reg = _var2reg(var, offset);
244  return _write_reg32(reg, value);
245 }
246 
248 {
249  uint16_t cmd_status;
250  /* timeout 100ms delay 10ms */
251  int timeout = 10;
252 
254 
256 
257  do {
258  hal.scheduler->delay(10);
259  cmd_status = _read_reg16(COMMAND_REGISTER);
260  timeout--;
261  } while (((cmd_status & HOST_COMMAND_1) != 0) &&
262  (timeout > 0));
263 
264  if (timeout == 0) {
265  hal.console->printf("mt9v117:"
266  "timeout waiting or command to complete\n");
267  }
268 
269  if ((cmd_status & HOST_COMMAND_OK) == 0) {
270  hal.console->printf("mt9v117:config change failed\n");
271  }
272 }
273 
275 {
279 }
280 
282 {
285  /* sleep 50ms after soft reset */
286  hal.scheduler->delay(50);
287 }
288 
290 {
291  uint16_t cmd_status;
292  /* timeout 100ms delay 10ms */
293  int timeout = 10;
294 
295  /* Errata item 2 */
296  _write_reg16(0x301a, 0x10d0);
297  _write_reg16(0x31c0, 0x1404);
298  _write_reg16(0x3ed8, 0x879c);
299  _write_reg16(0x3042, 0x20e1);
300  _write_reg16(0x30d4, 0x8020);
301  _write_reg16(0x30c0, 0x0026);
302  _write_reg16(0x301a, 0x10d4);
303 
304  /* Errata item 6 */
305  _write_var16(AE_TRACK_VAR, 0x0002, 0x00d3);
306  _write_var16(CAM_CTRL_VAR, 0x0078, 0x00a0);
307  _write_var16(CAM_CTRL_VAR, 0x0076, 0x0140);
308 
309  /* Errata item 8 */
310  _write_var16(LOW_LIGHT_VAR, 0x0004, 0x00fc);
311  _write_var16(LOW_LIGHT_VAR, 0x0038, 0x007f);
312  _write_var16(LOW_LIGHT_VAR, 0x003a, 0x007f);
313  _write_var16(LOW_LIGHT_VAR, 0x003c, 0x007f);
314  _write_var16(LOW_LIGHT_VAR, 0x0004, 0x00f4);
315 
316  /* Patch 0403; Critical; Sensor optimization */
317  _write_reg16(ACCESS_CTL_STAT, 0x0001);
319 
320  /* write patch */
321  for (unsigned int i = 0; i < MT9V117_PATCH_LINE_NUM; i++) {
322  _dev->transfer(_patch_lines[i].data, _patch_lines[i].size, nullptr, 0);
323  }
324 
326 
330 
332 
333  do {
334  hal.scheduler->delay(10);
335  cmd_status = _read_reg16(COMMAND_REGISTER);
336  timeout--;
337  } while (((cmd_status & HOST_COMMAND_0) != 0) &&
338  (timeout > 0));
339 
340  if ((cmd_status & HOST_COMMAND_OK) == 0) {
341  hal.console->printf("mt9v117:patch apply failed\n");
342  }
343 }
344 
346 {
349 
350  /* Set pixclk pad slew to 6 and data out pad slew to 1 */
351  _write_reg16(PAD_SLEW, _read_reg16(PAD_SLEW) | 0x0600 | 0x0001);
352 }
353 
355 {
366 
369 
372 
373  /* Set gain metric for 111.2 fps
374  * The final fps depends on the input clock
375  * (89.2fps on bebop) so a modification may be needed here */
378 
379  /* set crop window */
384 
385  /* Enable auto-stats mode */
393 }
394 
396 {
397  AP_HAL::DigitalSource *gpio_source;
398  uint16_t id;
399 
400  if (_nrst_gpio != 0xFFFF) {
401  gpio_source = hal.gpio->channel(_nrst_gpio);
402  gpio_source->mode(HAL_GPIO_OUTPUT);
403  gpio_source->write(1);
404  uint32_t delay = 3.5f + (35.0f - 3.5f) *
405  (54000000.0f - (float)_clock_freq) /
406  (54000000.0f - 6000000.0f);
407  hal.scheduler->delay(delay);
408  }
409 
410  id = _read_reg16(CHIP_ID);
411  if (id != MT9V117_CHIP_ID) {
412  AP_HAL::panic("Mt9v117: bad chip id 0x%04x\n", id);
413  }
414  _soft_reset();
415  _apply_patch();
417 }
418 
419 #endif
CameraSensor_Mt9v117(const char *device_path, AP_HAL::OwnPtr< AP_HAL::I2CDevice > dev, enum mt9v117_res res, uint16_t nrst_gpio, uint32_t clock_freq)
#define AE_RULE_ALGO_OFFSET
#define LOGICAL_ADDRESS_ACCESS
#define ACCESS_CTL_STAT
#define HOST_COMMAND_1
#define CHIP_ID
#define AWB_VAR
#define CAM_STAT_AWB_HG_WINDOW_YEND_OFFSET
#define HOST_COMMAND_OK
AP_HAL::UARTDriver * console
Definition: HAL.h:110
void _write_var16(uint16_t var, uint16_t offset, uint16_t value)
#define CAM_CROP_WINDOW_WIDTH_OFFSET
#define CAM_SENSOR_CFG_TARGET_FDZONE_60_OFFSET
#define PATCHLDR_PATCH_ID_OFFSET
#define PHYSICAL_ADDRESS_ACCESS
void _write_var32(uint16_t var, uint16_t offset, uint32_t value)
#define LOW_LIGHT_VAR
#define CAM_STAT_AE_INITIAL_WINDOW_YEND_OFFSET
uint16_t _var2reg(uint16_t var, uint16_t offset)
void delay(uint32_t ms)
Definition: system.cpp:91
#define PATCHLDR_LOADER_ADDRESS_OFFSET
#define CAM_SENSOR_CFG_X_ADDR_START_OFFSET
virtual void mode(uint8_t output)=0
#define CAM_SENSOR_CFG_X_ADDR_END_OFFSET
#define CAM_LL_START_GAIN_METRIC_OFFSET
#define CAM_CROP_WINDOW_HEIGHT_OFFSET
#define CAM_OUTPUT_HEIGHT_OFFSET
virtual void delay(uint16_t ms)=0
#define CAM_STAT_AE_INITIAL_WINDOW_YSTART_OFFSET
void _write_reg16(uint16_t reg, uint16_t val)
#define RESET_SOC_I2C
void _write_reg8(uint16_t reg, uint8_t val)
virtual void printf(const char *,...) FMT_PRINTF(2
Definition: BetterStream.cpp:5
#define CAM_OUTPUT_FORMAT_OFFSET
static AP_HAL::OwnPtr< AP_HAL::Device > dev
Definition: ICM20789.cpp:16
void _write_reg32(uint16_t reg, uint32_t val)
#define f(i)
#define PATCHLDR_VAR
#define CAM_SENSOR_CFG_FRAME_LENGTH_LINES_OFFSET
#define CAM_OUTPUT_WIDTH_OFFSET
virtual bool transfer(const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len) override=0
#define AWB_PIXEL_THRESHOLD_COUNT_OFFSET
virtual AP_HAL::DigitalSource * channel(uint16_t n)=0
#define CAM_STAT_AE_INITIAL_WINDOW_XEND_OFFSET
#define CAM_STAT_AWB_HG_WINDOW_XEND_OFFSET
#define AE_RULE_VAR
#define SYSMGR_VAR
uint16_t _read_var16(uint16_t var, uint16_t offset)
AP_HAL::OwnPtr< AP_HAL::I2CDevice > _dev
#define AE_RULE_ALGO_AVERAGE
#define MT9V117_CHIP_ID
#define PATCHLDR_FIRMWARE_ID_OFFSET
#define HAL_GPIO_OUTPUT
Definition: GPIO.h:8
#define CAM_SENSOR_CFG_Y_ADDR_START_OFFSET
#define CAM_SENSOR_CFG_MAX_FDZONE_60_OFFSET
#define COMMAND_REGISTER
#define CAM_CTRL_VAR
#define CAM_SENSOR_CFG_CPIPE_LAST_ROW_OFFSET
#define MT9V117_PATCH_LINE_NUM
#define CAM_LL_STOP_GAIN_METRIC_OFFSET
#define PAD_SLEW
AP_HAL::GPIO * gpio
Definition: HAL.h:111
#define CAM_CROP_WINDOW_XOFFSET_OFFSET
static const struct mt9v117_patch _patch_lines[MT9V117_PATCH_LINE_NUM]
float value
#define CAM_SENSOR_CONTROL_Y_SKIP_EN
const AP_HAL::HAL & hal
-*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
Definition: AC_PID_test.cpp:14
void panic(const char *errormsg,...) FMT_PRINTF(1
Definition: system.cpp:140
#define AE_TRACK_VAR
#define AE_TRACK_JUMP_DIVISOR
#define CAM_STAT_AE_INITIAL_WINDOW_XSTART_OFFSET
#define CAM_SENSOR_CONTROL_READ_MODE_OFFSET
void _write_var8(uint16_t var, uint16_t offset, uint8_t value)
#define SYSMGR_NEXT_STATE_OFFSET
uint16_t _read_reg16(uint16_t reg)
#define CAM_CROP_WINDOW_YOFFSET_OFFSET
AP_HAL::Scheduler * scheduler
Definition: HAL.h:114
#define HOST_COMMAND_0
#define CAM_CROP_MODE_OFFSET
#define CAM_OUTPUT_FORMAT_BT656_ENABLE
#define RESET_AND_MISC_CONTROL
virtual void write(uint8_t value)=0
uint8_t _read_var8(uint16_t var, uint16_t offset)
#define CAM_AET_SKIP_FRAMES
#define CAM_SENSOR_CFG_Y_ADDR_END_OFFSET