source: flair-src/trunk/lib/FlairSensorActuator/src/ParrotCamH.cpp @ 4

Last change on this file since 4 was 3, checked in by Sanahuja Guillaume, 6 years ago

sensoractuator

File size: 34.8 KB
Line 
1//  created:    2014/07/16
2//  filename:   ParrotCamH.cpp
3//
4//  author:     Guillaume Sanahuja
5//
6//  version:    $Id: $
7//
8//  purpose:    class for ardrone2 horizontal camera
9//
10//
11/*********************************************************************/
12
13/*
14* A V4L2 driver for Aptina MT9M114 cameras.
15*
16* Copyright 2011 Aldebaran Robotics Written
17* by Joseph Pinkasfeld with substantial inspiration from ov7670 code.
18*
19* Authors:
20* joseph pinkasfeld <joseph.pinkasfeld@gmail.com>
21* Ludovic SMAL <lsmal@aldebaran-robotics.com>
22* Corentin Le Molgat <clemolgat@aldebaran-robotics.com>
23* Arne Böckmann <arneboe@tzi.de>
24*
25* This file may be distributed under the terms of the GNU General
26* Public License, version 2.
27*/
28
29#include "ParrotCamH.h"
30#include <Unix_I2cPort.h>
31#include <fcntl.h>
32#include <unistd.h>
33
34#define I2C_8BIT 1
35#define I2C_16BIT 2
36#define I2C_32BIT 4
37
38/* Registers */
39#define REG_CHIP_ID 0x0000
40#define REG_MON_MAJOR_VERSION 0x8000
41#define REG_MON_MINOR_VERION 0x8002
42#define REG_MON_RELEASE_VERSION 0x8004
43#define REG_RESET_AND_MISC_CONTROL 0x001A
44#define REG_PAD_SLEW_CONTROL 0x001E
45#define REG_COMMAND_REGISTER 0x0080
46#define HOST_COMMAND_APPLY_PATCH 0x0001
47#define HOST_COMMAND_SET_STATE 0x0002
48#define HOST_COMMAND_REFRESH 0x0004
49#define HOST_COMMAND_WAIT_FOR_EVENT 0x0008
50#define HOST_COMMAND_OK 0x8000
51#define REG_ACCESS_CTL_STAT 0x0982
52#define REG_PHYSICAL_ADDRESS_ACCESS 0x098A
53#define REG_LOGICAL_ADDRESS_ACCESS 0x098E
54#define MCU_VARIABLE_DATA0 0x0990
55#define MCU_VARIABLE_DATA1 0x0992
56#define REG_RESET_REGISTER 0x301A
57#define REG_DAC_TXLO_ROW 0x316A
58#define REG_DAC_TXLO 0x316C
59#define REG_DAC_LD_4_5 0x3ED0
60#define REG_DAC_LD_6_7 0x3ED2
61#define REG_DAC_ECL 0x316E
62#define REG_DELTA_DK_CONTROL 0x3180
63#define REG_SAMP_COL_PUP2 0x3E14
64#define REG_COLUMN_CORRECTION 0x30D4
65#define REG_LL_ALGO 0xBC04
66#define LL_EXEC_DELTA_DK_CORRECTION 0x0200
67#define REG_CAM_DGAIN_RED 0xC840
68#define REG_CAM_DGAIN_GREEN_1 0xC842
69#define REG_CAM_DGAIN_GREEN_2 0xC844
70#define REG_CAM_DGAIN_BLUE 0xC846
71#define REG_CAM_SYSCTL_PLL_ENABLE 0xC97E
72#define REG_CAM_SYSCTL_PLL_DIVIDER_M_N 0xC980
73#define REG_CAM_SYSCTL_PLL_DIVIDER_P 0xC982
74#define REG_CAM_SENSOR_CFG_Y_ADDR_START 0xC800
75#define REG_CAM_SENSOR_CFG_X_ADDR_START 0xC802
76#define REG_CAM_SENSOR_CFG_Y_ADDR_END 0xC804
77#define REG_CAM_SENSOR_CFG_X_ADDR_END 0xC806
78#define REG_CAM_SENSOR_CFG_PIXCLK 0xC808
79#define REG_CAM_SENSOR_CFG_ROW_SPEED 0xC80C
80#define REG_CAM_SENSOR_CFG_FINE_INTEG_TIME_MIN 0xC80E
81#define REG_CAM_SENSOR_CFG_FINE_INTEG_TIME_MAX 0xC810
82#define REG_CAM_SENSOR_CFG_FRAME_LENGTH_LINES 0xC812
83#define REG_CAM_SENSOR_CFG_LINE_LENGTH_PCK 0xC814
84#define REG_CAM_SENSOR_CFG_FINE_CORRECTION 0xC816
85#define REG_CAM_SENSOR_CFG_CPIPE_LAST_ROW 0xC818
86#define REG_CAM_SENSOR_CFG_REG_0_DATA 0xC826
87#define REG_CAM_SENSOR_CONTROL_READ_MODE 0xC834
88#define CAM_SENSOR_CONTROL_VERT_FLIP_EN 0x0002
89#define CAM_SENSOR_CONTROL_HORZ_FLIP_EN 0x0001
90#define CAM_SENSOR_CONTROL_BINNING_EN 0x0330
91#define CAM_SENSOR_CONTROL_SKIPPING_EN 0x0110
92#define CAM_MON_HEARTBEAT 0x8006 //the frame counter. updates on vertical blanking.
93#define REG_CAM_CROP_WINDOW_XOFFSET 0xC854
94#define REG_CAM_CROP_WINDOW_YOFFSET 0xC856
95#define REG_CAM_CROP_WINDOW_WIDTH 0xC858
96#define REG_CAM_CROP_WINDOW_HEIGHT 0xC85A
97#define REG_CAM_CROP_CROPMODE 0xC85C
98#define REG_CAM_OUTPUT_WIDTH 0xC868
99#define REG_CAM_OUTPUT_HEIGHT 0xC86A
100#define REG_CAM_OUTPUT_FORMAT 0xC86C
101#define REG_CAM_OUTPUT_OFFSET 0xC870
102#define REG_CAM_PORT_OUTPUT_CONTROL 0xC984
103#define REG_CAM_OUPUT_FORMAT_YUV 0xC86E
104#define REG_CAM_STAT_AWB_CLIP_WINDOW_XSTART 0xC914
105#define REG_CAM_STAT_AWB_CLIP_WINDOW_YSTART 0xC916
106#define REG_CAM_STAT_AWB_CLIP_WINDOW_XEND 0xC918
107#define REG_CAM_STAT_AWB_CLIP_WINDOW_YEND 0xC91A
108#define REG_CAM_STAT_AE_INITIAL_WINDOW_XSTART 0xC91C
109#define REG_CAM_STAT_AE_INITIAL_WINDOW_YSTART 0xC91E
110#define REG_CAM_STAT_AE_INITIAL_WINDOW_XEND 0xC920
111#define REG_CAM_STAT_AE_INITIAL_WINDOW_YEND 0xC922
112#define REG_CAM_PGA_PGA_CONTROL 0xC95E
113#define REG_SYSMGR_NEXT_STATE 0xDC00
114#define REG_SYSMGR_CURRENT_STATE 0xDC01
115#define REG_PATCHLDR_LOADER_ADDRESS 0xE000
116#define REG_PATCHLDR_PATCH_ID 0xE002
117#define REG_PATCHLDR_FIRMWARE_ID 0xE004
118#define REG_PATCHLDR_APPLY_STATUS 0xE008
119#define REG_AUTO_BINNING_MODE 0xE801
120#define REG_CAM_SENSOR_CFG_MAX_ANALOG_GAIN 0xC81C
121#define REG_CROP_CROPMODE 0xC85C
122#define REG_CAM_AET_AEMODE 0xC878
123#define REG_CAM_AET_TARGET_AVG_LUMA 0xC87A
124#define REG_CAM_AET_TARGET_AVERAGE_LUMA_DARK 0xC87B
125#define REG_CAM_AET_BLACK_CLIPPING_TARGET 0xC87C
126#define REG_CAM_AET_AE_MAX_VIRT_AGAIN 0xC886
127#define REG_CAM_AET_MAX_FRAME_RATE 0xC88C
128#define REG_CAM_AET_MIN_FRAME_RATE 0xC88E
129#define REG_CAM_AET_TARGET_GAIN 0xC890
130#define REG_AE_ALGORITHM 0xA404
131#define REG_AE_TRACK_MODE 0xA802
132#define REG_AE_TRACK_AE_TRACKING_DAMPENING_SPEED 0xA80A
133#define REG_CAM_LL_START_BRIGHTNESS 0xC926
134#define REG_CAM_LL_STOP_BRIGHTNESS 0xC928
135#define REG_CAM_LL_START_GAIN_METRIC 0xC946
136#define REG_CAM_LL_STOP_GAIN_METRIC 0xC948
137#define REG_CAM_LL_START_TARGET_LUMA_BM 0xC952
138#define REG_CAM_LL_STOP_TARGET_LUMA_BM 0xC954
139#define REG_CAM_LL_START_SATURATION 0xC92A
140#define REG_CAM_LL_END_SATURATION 0xC92B
141#define REG_CAM_LL_START_DESATURATION 0xC92C
142#define REG_CAM_LL_END_DESATURATION 0xC92D
143#define REG_CAM_LL_START_DEMOSAIC 0xC92E
144#define REG_CAM_LL_START_AP_GAIN 0xC92F
145#define REG_CAM_LL_START_AP_THRESH 0xC930
146#define REG_CAM_LL_STOP_DEMOSAIC 0xC931
147#define REG_CAM_LL_STOP_AP_GAIN 0xC932
148#define REG_CAM_LL_STOP_AP_THRESH 0xC933
149#define REG_CAM_LL_START_NR_RED 0xC934
150#define REG_CAM_LL_START_NR_GREEN 0xC935
151#define REG_CAM_LL_START_NR_BLUE 0xC936
152#define REG_CAM_LL_START_NR_THRESH 0xC937
153#define REG_CAM_LL_STOP_NR_RED 0xC938
154#define REG_CAM_LL_STOP_NR_GREEN 0xC939
155#define REG_CAM_LL_STOP_NR_BLUE 0xC93A
156#define REG_CAM_LL_STOP_NR_THRESH 0xC93B
157#define REG_CAM_LL_START_CONTRAST_BM 0xC93C
158#define REG_CAM_LL_STOP_CONTRAST_BM 0xC93E
159#define REG_CAM_LL_GAMMA 0xC940
160#define REG_CAM_LL_START_CONTRAST_GRADIENT 0xC942
161#define REG_CAM_LL_STOP_CONTRAST_GRADIENT 0xC943
162#define REG_CAM_LL_START_CONTRAST_LUMA_PERCENTAGE 0xC944
163#define REG_CAM_LL_STOP_CONTRAST_LUMA_PERCENTAGE 0xC945
164#define REG_CAM_LL_START_FADE_TO_BLACK_LUMA 0xC94A
165#define REG_CAM_LL_STOP_FADE_TO_BLACK_LUMA 0xC94C
166#define REG_CAM_LL_CLUSTER_DC_TH_BM 0xC94E
167#define REG_CAM_LL_CLUSTER_DC_GATE_PERCENTAGE 0xC950
168#define REG_CAM_LL_SUMMING_SENSITIVITY_FACTOR 0xC951
169#define REG_CAM_LL_MODE 0xBC02 //might be BC07.
170#define REG_CCM_DELTA_GAIN 0xB42A
171#define REG_CAM_HUE_ANGLE 0xC873
172// AWB
173#define REG_AWB_AWB_MODE 0xC909
174#define REG_AWB_COL_TEMP 0xC8F0//color temp, only writeable if awb mode is manual. in kelvin
175#define REG_AWB_COL_TEMP_MAX 0xC8EE//maximum color temp in kelvin
176#define REG_AWB_COL_TEMP_MIN 0xC8EC//minimum color temp in kelvin
177// UVC
178#define REG_UVC_AE_MODE 0xCC00
179#define REG_UVC_AUTO_WHITE_BALANCE_TEMPERATURE 0xCC01
180#define REG_UVC_AE_PRIORITY 0xCC02
181#define REG_UVC_POWER_LINE_FREQUENCY 0xCC03
182#define REG_UVC_EXPOSURE_TIME 0xCC04
183#define REG_UVC_BACKLIGHT_COMPENSATION 0xCC08
184#define REG_UVC_BRIGHTNESS 0xCC0A //set brightness in auto exposure mode.
185#define REG_UVC_CONTRAST 0xCC0C //not exactly what the name suggests. See chip documentation
186#define REG_UVC_GAIN 0xCC0E
187#define REG_UVC_HUE 0xCC10
188#define REG_UVC_SATURATION 0xCC12
189#define REG_UVC_SHARPNESS 0xCC14
190#define REG_UVC_GAMMA 0xCC16
191#define REG_UVC_WHITE_BALANCE_TEMPERATURE 0xCC18
192#define REG_UVC_FRAME_INTERVAL 0xCC1C
193#define REG_UVC_MANUAL_EXPOSURE 0xCC20
194#define REG_UVC_FLICKER_AVOIDANCE 0xCC21
195#define REG_UVC_ALGO 0xCC22
196#define REG_UVC_RESULT_STATUS 0xCC24
197/**This variable selects the system event that the host wishes to wait for.
198* 1: end of frame
199* 2: start of frame */
200#define REG_CMD_HANDLER_WAIT_FOR_EVENT 0xFC00
201/** This variable determines the number of system event occurrences for which the
202* Command Handler component will wait */
203#define REG_CMD_HANDLER_NUM_WAIT_EVENTS 0xFC02
204/**Result status code for last refresh command. Updates after refresh command.
205* Possible values:
2060x00: ENOERR - refresh successful
2070x13: EINVCROPX - invalid horizontal crop configuration
2080x14: EINVCROPY - invalid vertical crop configuration
2090x15: EINVTC - invalid Tilt Correction percentage
210*/
211#define REG_SEQ_ERROR_CODE 0x8406
212/* SYS_STATE values (for SYSMGR_NEXT_STATE and SYSMGR_CURRENT_STATE) */
213#define MT9M114_SYS_STATE_ENTER_CONFIG_CHANGE 0x28
214#define MT9M114_SYS_STATE_STREAMING 0x31
215#define MT9M114_SYS_STATE_START_STREAMING 0x34
216#define MT9M114_SYS_STATE_ENTER_SUSPEND 0x40
217#define MT9M114_SYS_STATE_SUSPENDED 0x41
218#define MT9M114_SYS_STATE_ENTER_STANDBY 0x50
219#define MT9M114_SYS_STATE_STANDBY 0x52
220#define MT9M114_SYS_STATE_LEAVE_STANDBY 0x54
221
222static struct regval_list mt9m114_720p36_regs[] = {
223    {REG_LOGICAL_ADDRESS_ACCESS, 2, 0x0000},
224    {REG_CAM_SENSOR_CFG_Y_ADDR_START, 2, 124},
225    {REG_CAM_SENSOR_CFG_X_ADDR_START, 2, 4},
226    {REG_CAM_SENSOR_CFG_Y_ADDR_END, 2, 851},
227    {REG_CAM_SENSOR_CFG_X_ADDR_END, 2, 1291},
228    {REG_CAM_SENSOR_CFG_PIXCLK, 4, 48000000},
229    {REG_CAM_SENSOR_CFG_ROW_SPEED, 2, 0x0001},
230    {REG_CAM_SENSOR_CFG_FINE_INTEG_TIME_MIN, 2, 219},
231    {REG_CAM_SENSOR_CFG_FINE_INTEG_TIME_MAX, 2, 1558},
232    {REG_CAM_SENSOR_CFG_FRAME_LENGTH_LINES, 2, 778},
233    {REG_CAM_SENSOR_CFG_LINE_LENGTH_PCK, 2, 1689},
234    {REG_CAM_SENSOR_CFG_FINE_CORRECTION, 2, 96},
235    {REG_CAM_SENSOR_CFG_CPIPE_LAST_ROW, 2, 723},
236    {REG_CAM_SENSOR_CFG_REG_0_DATA, 2, 0x0020},
237// {REG_CAM_SENSOR_CONTROL_READ_MODE, 1, 0x0000},
238    {REG_CAM_CROP_WINDOW_XOFFSET, 2, 0x0000},
239    {REG_CAM_CROP_WINDOW_YOFFSET, 2, 0x0000},
240    {REG_CAM_CROP_WINDOW_WIDTH, 2, 1280},
241    {REG_CAM_CROP_WINDOW_HEIGHT, 2, 720},
242    {REG_CROP_CROPMODE, 1, 0x03},
243    {REG_CAM_OUTPUT_WIDTH, 2, 1280},
244    {REG_CAM_OUTPUT_HEIGHT, 2, 720},
245    {REG_CAM_AET_AEMODE, 1, 0x00},
246    {REG_CAM_AET_MAX_FRAME_RATE, 2, 0x24AB},
247    {REG_CAM_AET_MIN_FRAME_RATE, 2, 0x24AB},
248    {REG_CAM_STAT_AWB_CLIP_WINDOW_XSTART, 2, 0x0000},
249    {REG_CAM_STAT_AWB_CLIP_WINDOW_YSTART, 2, 0x0000},
250    {REG_CAM_STAT_AWB_CLIP_WINDOW_XEND, 2, 1279},
251    {REG_CAM_STAT_AWB_CLIP_WINDOW_YEND, 2, 719},
252    {REG_CAM_STAT_AE_INITIAL_WINDOW_XSTART, 2, 0x0000},
253    {REG_CAM_STAT_AE_INITIAL_WINDOW_YSTART, 2, 0x0000},
254    {REG_CAM_STAT_AE_INITIAL_WINDOW_XEND, 2, 255},
255    {REG_CAM_STAT_AE_INITIAL_WINDOW_YEND, 2, 143},
256    { 0xffff, 0xffff ,0xffff }  /* END MARKER */
257};
258
259static struct regval_list mt9m114_vga_30_scaling_regs[] = {
260    {REG_LOGICAL_ADDRESS_ACCESS, 2, 0x0000},
261    {REG_CAM_SENSOR_CFG_Y_ADDR_START, 2, 4},
262    {REG_CAM_SENSOR_CFG_X_ADDR_START, 2, 4},
263    {REG_CAM_SENSOR_CFG_Y_ADDR_END, 2, 971},
264    {REG_CAM_SENSOR_CFG_X_ADDR_END, 2, 1291},
265    {REG_CAM_SENSOR_CFG_PIXCLK, 4, 48000000},
266    {REG_CAM_SENSOR_CFG_ROW_SPEED, 2, 1},//FIXME according to the documentation this value is unused, however we still set the default. No idea why
267    {REG_CAM_SENSOR_CFG_FINE_INTEG_TIME_MIN, 2, 219},
268    {REG_CAM_SENSOR_CFG_FINE_INTEG_TIME_MAX, 2, 1460},
269    {REG_CAM_SENSOR_CFG_FRAME_LENGTH_LINES, 2, 1006},//FIXME might be a typo. default value is 1007
270    {REG_CAM_SENSOR_CFG_LINE_LENGTH_PCK, 2, 1591},//FIXME might be a typo? default is 1589
271    {REG_CAM_SENSOR_CFG_FINE_CORRECTION, 2, 96},
272    {REG_CAM_SENSOR_CFG_CPIPE_LAST_ROW, 2, 963},
273    {REG_CAM_SENSOR_CFG_REG_0_DATA, 2, 0x0020},
274// {REG_CAM_SENSOR_CONTROL_READ_MODE, 1, 0x0000},
275    {REG_CAM_CROP_WINDOW_XOFFSET, 2, 0x0000},
276    {REG_CAM_CROP_WINDOW_YOFFSET, 2, 0x0000},
277    {REG_CAM_CROP_WINDOW_WIDTH, 2, 1280},
278    {REG_CAM_CROP_WINDOW_HEIGHT, 2, 960},
279    {REG_CROP_CROPMODE, 1, 3},
280    {REG_CAM_OUTPUT_WIDTH, 2, 640},
281    {REG_CAM_OUTPUT_HEIGHT, 2, 480},
282    {REG_CAM_AET_AEMODE, 1, 0x00},
283    {REG_CAM_AET_MAX_FRAME_RATE, 2, 0x1DFD},
284    {REG_CAM_AET_MIN_FRAME_RATE, 2, 0x1DFD},
285    {REG_CAM_STAT_AWB_CLIP_WINDOW_XSTART, 2, 0x0000},
286    {REG_CAM_STAT_AWB_CLIP_WINDOW_YSTART, 2, 0x0000},
287    {REG_CAM_STAT_AWB_CLIP_WINDOW_XEND, 2, 639},
288    {REG_CAM_STAT_AWB_CLIP_WINDOW_YEND, 2, 479},
289    {REG_CAM_STAT_AE_INITIAL_WINDOW_XSTART, 2, 0x0000},
290    {REG_CAM_STAT_AE_INITIAL_WINDOW_YSTART, 2, 0x0000},
291    {REG_CAM_STAT_AE_INITIAL_WINDOW_XEND, 2, 127},
292    {REG_CAM_STAT_AE_INITIAL_WINDOW_YEND, 2, 95},
293// {REG_AUTO_BINNING_MODE,1, 0x00},
294    { 0xffff, 0xffff ,0xffff }//array end,
295};
296
297static struct regval_list mt9m114_qvga_30_scaling_regs[] = {
298    {REG_LOGICAL_ADDRESS_ACCESS, 2, 0x0000},
299    {REG_CAM_SENSOR_CFG_Y_ADDR_START, 2, 4},
300    {REG_CAM_SENSOR_CFG_X_ADDR_START, 2, 4},
301    {REG_CAM_SENSOR_CFG_Y_ADDR_END, 2, 971},
302    {REG_CAM_SENSOR_CFG_X_ADDR_END, 2, 1291},
303    {REG_CAM_SENSOR_CFG_PIXCLK, 4, 48000000},
304    {REG_CAM_SENSOR_CFG_ROW_SPEED, 2, 1},
305    {REG_CAM_SENSOR_CFG_FINE_INTEG_TIME_MIN, 2, 219},
306    {REG_CAM_SENSOR_CFG_FINE_INTEG_TIME_MAX, 2, 1460},
307    {REG_CAM_SENSOR_CFG_FRAME_LENGTH_LINES, 2, 1006},
308    {REG_CAM_SENSOR_CFG_LINE_LENGTH_PCK, 2, 1591},
309    {REG_CAM_SENSOR_CFG_FINE_CORRECTION, 2, 96},
310    {REG_CAM_SENSOR_CFG_CPIPE_LAST_ROW, 2, 963},
311    {REG_CAM_SENSOR_CFG_REG_0_DATA, 2, 0x0020},
312    //{REG_CAM_SENSOR_CONTROL_READ_MODE, 1, 0x0000},
313    {REG_CAM_CROP_WINDOW_XOFFSET, 2, 0x0000},
314    {REG_CAM_CROP_WINDOW_YOFFSET, 2, 0x0000},
315    {REG_CAM_CROP_WINDOW_WIDTH, 2, 1280},
316    {REG_CAM_CROP_WINDOW_HEIGHT, 2, 960},
317    {REG_CROP_CROPMODE, 1, 3},
318    {REG_CAM_OUTPUT_WIDTH, 2, 320},
319    {REG_CAM_OUTPUT_HEIGHT, 2, 240},
320    {REG_CAM_AET_AEMODE, 1, 0x00},
321    {REG_CAM_AET_MAX_FRAME_RATE, 2, 0x1DFD},
322    {REG_CAM_AET_MIN_FRAME_RATE, 2, 0x1DFD},
323    {REG_CAM_STAT_AWB_CLIP_WINDOW_XSTART, 2, 0x0000},
324    {REG_CAM_STAT_AWB_CLIP_WINDOW_YSTART, 2, 0x0000},
325    {REG_CAM_STAT_AWB_CLIP_WINDOW_XEND, 2, 319},
326    {REG_CAM_STAT_AWB_CLIP_WINDOW_YEND, 2, 239},
327    {REG_CAM_STAT_AE_INITIAL_WINDOW_XSTART, 2, 0x0000},
328    {REG_CAM_STAT_AE_INITIAL_WINDOW_YSTART, 2, 0x0000},
329    {REG_CAM_STAT_AE_INITIAL_WINDOW_XEND, 2, 63},
330    {REG_CAM_STAT_AE_INITIAL_WINDOW_YEND, 2, 47},
331//  {REG_AUTO_BINNING_MODE,1, 0x00},
332
333    { 0xffff, 0xffff ,0xffff }
334};
335/*
336static struct regval_list mt9m114_qvga_30_scaling_regs[] = {
337  {REG_CAM_OUTPUT_WIDTH, 2, 320},
338  {REG_CAM_OUTPUT_HEIGHT, 2, 240},
339  { 0xffff, 0xffff ,0xffff }
340};*/
341static struct regval_list pga_regs[] = {
342    { 0x098E, 2, 0},
343    { 0xC95E, 2, 3},
344    { 0xC95E, 2, 2},
345    { 0x3640, 2, 368},
346    { 0x3642, 2, 3787},
347    { 0x3644, 2, 22480},
348    { 0x3646, 2, 33549},
349    { 0x3648, 2, 62062},
350    { 0x364A, 2, 32303},
351    { 0x364C, 2, 18603},
352    { 0x364E, 2, 26192},
353    { 0x3650, 2, 52556},
354    { 0x3652, 2, 44686},
355    { 0x3654, 2, 32431},
356    { 0x3656, 2, 23244},
357    { 0x3658, 2, 7056},
358    { 0x365A, 2, 64140},
359    { 0x365C, 2, 37614},
360    { 0x365E, 2, 32207},
361    { 0x3660, 2, 19178},
362    { 0x3662, 2, 26800},
363    { 0x3664, 2, 45101},
364    { 0x3666, 2, 43151},
365    { 0x3680, 2, 13964},
366    { 0x3682, 2, 1869},
367    { 0x3684, 2, 9871},
368    { 0x3686, 2, 32394},
369    { 0x3688, 2, 38832},
370    { 0x368A, 2, 492},
371    { 0x368C, 2, 2894},
372    { 0x368E, 2, 4687},
373    { 0x3690, 2, 45006},
374    { 0x3692, 2, 34192},
375    { 0x3694, 2, 973},
376    { 0x3696, 2, 2349},
377    { 0x3698, 2, 25323},
378    { 0x369A, 2, 41294},
379    { 0x369C, 2, 46959},
380    { 0x369E, 2, 3405},
381    { 0x36A0, 2, 47531},
382    { 0x36A2, 2, 38860},
383    { 0x36A4, 2, 22506},
384    { 0x36A6, 2, 37359},
385    { 0x36C0, 2, 3569},
386    { 0x36C2, 2, 36620},
387    { 0x36C4, 2, 30224},
388    { 0x36C6, 2, 11116},
389    { 0x36C8, 2, 42739},
390    { 0x36CA, 2, 1681},
391    { 0x36CC, 2, 61514},
392    { 0x36CE, 2, 13265},
393    { 0x36D0, 2, 44462},
394    { 0x36D2, 2, 51635},
395    { 0x36D4, 2, 23184},
396    { 0x36D6, 2, 39789},
397    { 0x36D8, 2, 22480},
398    { 0x36DA, 2, 3885},
399    { 0x36DC, 2, 64882},
400    { 0x36DE, 2, 3505},
401    { 0x36E0, 2, 46314},
402    { 0x36E2, 2, 26864},
403    { 0x36E4, 2, 36813},
404    { 0x36E6, 2, 41555},
405    { 0x3700, 2, 1325},
406    { 0x3702, 2, 60557},
407    { 0x3704, 2, 46961},
408    { 0x3706, 2, 13199},
409    { 0x3708, 2, 25234},
410    { 0x370A, 2, 10253},
411    { 0x370C, 2, 36912},
412    { 0x370E, 2, 46449},
413    { 0x3710, 2, 17713},
414    { 0x3712, 2, 19282},
415    { 0x3714, 2, 10509},
416    { 0x3716, 2, 53295},
417    { 0x3718, 2, 38417},
418    { 0x371A, 2, 8881},
419    { 0x371C, 2, 26834},
420    { 0x371E, 2, 27981},
421    { 0x3720, 2, 39469},
422    { 0x3722, 2, 34321},
423    { 0x3724, 2, 5232},
424    { 0x3726, 2, 20978},
425    { 0x3740, 2, 35307},
426    { 0x3742, 2, 49806},
427    { 0x3744, 2, 62036},
428    { 0x3746, 2, 23250},
429    { 0x3748, 2, 27830},
430    { 0x374A, 2, 8111},
431    { 0x374C, 2, 51085},
432    { 0x374E, 2, 33653},
433    { 0x3750, 2, 24914},
434    { 0x3752, 2, 29270},
435    { 0x3754, 2, 5133},
436    { 0x3756, 2, 5933},
437    { 0x3758, 2, 52436},
438    { 0x375A, 2, 13362},
439    { 0x375C, 2, 18166},
440    { 0x375E, 2, 37550},
441    { 0x3760, 2, 39566},
442    { 0x3762, 2, 61300},
443    { 0x3764, 2, 23602},
444    { 0x3766, 2, 26198},
445    { 0x3782, 2, 480},
446    { 0x3784, 2, 672},
447    { 0xC960, 2, 2800},
448    { 0xC962, 2, 31149},
449    { 0xC964, 2, 22448},
450    { 0xC966, 2, 30936},
451    { 0xC968, 2, 29792},
452    { 0xC96A, 2, 4000},
453    { 0xC96C, 2, 33143},
454    { 0xC96E, 2, 33116},
455    { 0xC970, 2, 33041},
456    { 0xC972, 2, 32855},
457    { 0xC974, 2, 6500},
458    { 0xC976, 2, 31786},
459    { 0xC978, 2, 26268},
460    { 0xC97A, 2, 32319},
461    { 0xC97C, 2, 29650},
462    { 0xC95E, 2, 3},
463    { 0xffff, 0xffff ,0xffff}
464};
465
466static struct regval_list ccm_awb_regs[] = {
467    { 0xC892, 2, 615},
468    { 0xC894, 2, 65306},
469    { 0xC896, 2, 65459},
470    { 0xC898, 2, 65408},
471    { 0xC89A, 2, 358},
472    { 0xC89C, 2, 3},
473    { 0xC89E, 2, 65434},
474    { 0xC8A0, 2, 65204},
475    { 0xC8A2, 2, 589},
476    { 0xC8A4, 2, 447},
477    { 0xC8A6, 2, 65281},
478    { 0xC8A8, 2, 65523},
479    { 0xC8AA, 2, 65397},
480    { 0xC8AC, 2, 408},
481    { 0xC8AE, 2, 65533},
482    { 0xC8B0, 2, 65434},
483    { 0xC8B2, 2, 65255},
484    { 0xC8B4, 2, 680},
485    { 0xC8B6, 2, 473},
486    { 0xC8B8, 2, 65318},
487    { 0xC8BA, 2, 65523},
488    { 0xC8BC, 2, 65459},
489    { 0xC8BE, 2, 306},
490    { 0xC8C0, 2, 65512},
491    { 0xC8C2, 2, 65498},
492    { 0xC8C4, 2, 65229},
493    { 0xC8C6, 2, 706},
494    { 0xC8C8, 2, 117},
495    { 0xC8CA, 2, 284},
496    { 0xC8CC, 2, 154},
497    { 0xC8CE, 2, 261},
498    { 0xC8D0, 2, 164},
499    { 0xC8D2, 2, 172},
500    { 0xC8D4, 2, 2700},
501    { 0xC8D6, 2, 3850},
502    { 0xC8D8, 2, 6500},
503    { 0xC914, 2, 0},
504    { 0xC916, 2, 0},
505    { 0xC918, 2, 1279},
506    { 0xC91A, 2, 719},
507    { 0xC904, 2, 51},
508    { 0xC906, 2, 64},
509    { 0xC8F2, 1, 3},
510    { 0xC8F3, 1, 2},
511    { 0xC906, 2, 60},
512    { 0xC8F4, 2, 0},
513    { 0xC8F6, 2, 0},
514    { 0xC8F8, 2, 0},
515    { 0xC8FA, 2, 59172},
516    { 0xC8FC, 2, 5507},
517    { 0xC8FE, 2, 8261},
518    { 0xC900, 2, 1023},
519    { 0xC902, 2, 124},
520    { 0xC90C, 1, 128},
521    { 0xC90D, 1, 128},
522    { 0xC90E, 1, 128},
523    { 0xC90F, 1, 136},
524    { 0xC910, 1, 128},
525    { 0xC911, 1, 128},
526    { 0xffff, 0xffff ,0xffff}
527};
528
529
530using std::string;
531using namespace flair::core;
532
533namespace flair {
534namespace sensor {
535
536//construct V4LCamera with 1280*720 because
537//CV_CAP_PROP_FRAME_WIDTH and CV_CAP_PROP_FRAME_HEIGHT does not work properly (isp problem?)
538ParrotCamH::ParrotCamH(const FrameworkManager* parent,string name,uint16_t width,uint16_t height,uint8_t priority) : V4LCamera(parent,name,1,1280,720,cvimage::Type::Format::UYVY,priority) {
539    //handle resolution
540    if(width==1280 && height==720) {
541        wh_regval_list=&mt9m114_720p36_regs[0];
542    } else if(width==640 && height==360) {
543        wh_regval_list=&mt9m114_vga_30_scaling_regs[0];
544    } else if(width==320 && height==180) {
545        wh_regval_list=&mt9m114_qvga_30_scaling_regs[0];
546    } else {
547        Thread::Err("Invalid width (%i) and height (%i)\n",width,height);
548        wh_regval_list=&mt9m114_qvga_30_scaling_regs[0];//default value
549    }
550
551    //realocate output, because Camera was created with 1280*720
552    //do not allocate imagedata, allocation is done by the camera
553    delete output;
554    output=new cvimage((IODevice*)this,width,height,cvimage::Type::Format::UYVY,"out",false);
555
556    //i2cport
557    //we assume there is no other device on this bus
558    i2cport=new Unix_I2cPort((Thread*)this,"camv-i2c","/dev/i2c-3");
559    i2cport->SetSlave(0x5d);
560
561    mt9m114_detect();
562}
563
564ParrotCamH::~ParrotCamH() {
565
566}
567
568void ParrotCamH::mt9m114_detect(void) {
569    uint32_t chip_id;
570    uint32_t mon_major_version;
571    uint32_t mon_minor_version;
572    uint32_t mon_release_version;
573
574    soc1040_read_reg(REG_CHIP_ID, &chip_id,2);
575    soc1040_read_reg(REG_MON_MAJOR_VERSION, &mon_major_version,2);
576    soc1040_read_reg(REG_MON_MINOR_VERION, &mon_minor_version,2);
577    soc1040_read_reg(REG_MON_RELEASE_VERSION, &mon_release_version,2);
578
579    if(chip_id!=0)
580        Printf("MT9M114 found : chip_id:%x major:%x minor:%x release:%x\n", chip_id,mon_major_version,mon_minor_version,mon_release_version);
581
582    if (chip_id != 0x2481) { // default chipid
583        Thread::Err("invalid chipid\n");
584        return;
585    }
586
587    mt9m114_init();
588}
589
590void ParrotCamH::mt9m114_init(void) {
591    int ret = 0;
592    ret += mt9m114_reset();
593
594    ret += mt9m114_PLL_settings();
595    //ret += mt9m114_write_array(mt9m114_vga_30_scaling_regs);
596    ret += mt9m114_write_array(wh_regval_list);
597    ret += mt9m114_sensor_optimization();
598    ret += mt9m114_errata_1();
599    ret += mt9m114_errata_2();
600    ret += mt9m114_write_array(pga_regs);
601    ret += mt9m114_write_array(ccm_awb_regs);
602    /*
603    ret += mt9m114_CPIPE_preference();
604    ret += mt9m114_features();
605    ret += mt9m114_write_array(uvc_ctrl_regs);
606    ret += mt9m114_change_config();
607    ret += mt9m114_patch2_black_lvl_correction_fix();
608    ret += mt9m114_patch3_adaptive_sensitivity();*/
609    if (ret != 0) {
610        Thread::Err("MT9M114","MT9M114 : init fail\n");
611    }
612
613}
614
615int ParrotCamH::mt9m114_reset(void) {
616    uint32_t v;
617    Printf("MT9M114 : Resetting chip!\n");
618
619    int fd=open("/dev/v4l-subdev8",O_WRONLY);//part of the reset sequence?
620    close(fd);
621
622    soc1040_read_reg( REG_RESET_AND_MISC_CONTROL, &v,2);
623    soc1040_write_reg(REG_RESET_AND_MISC_CONTROL, v|0x01,2);
624    SleepMS(100); //FIXME This sleep shouldn't be here according to the documentation
625    soc1040_write_reg(REG_RESET_AND_MISC_CONTROL, v & (~1),2);
626    SleepMS(100); //datasheet documentation
627    mt9m114_errata_2();
628
629    system("media-ctl -f \"\\\"soc1040\\\":0[UYVY2X8 1280x720]\"");//part of the reset sequence?
630    return 0;
631}
632
633int ParrotCamH::mt9m114_PLL_settings(void) {
634    soc1040_write_reg(REG_LOGICAL_ADDRESS_ACCESS, 0x0000,2);
635    soc1040_write_reg(REG_CAM_SYSCTL_PLL_ENABLE, 1, 1); //no idea what it does. 1 is default
636    soc1040_write_reg(REG_CAM_SYSCTL_PLL_DIVIDER_M_N, 0x0120,2); //no idea, default is 0x09a0
637    soc1040_write_reg(REG_CAM_SYSCTL_PLL_DIVIDER_P, 0x0700,2);//no idea, default is 0x0700
638    return 0;
639}
640
641int ParrotCamH::mt9m114_errata_1(void) {
642    soc1040_write_reg( REG_SAMP_COL_PUP2,0xFF39,2); //no idea, register is undocumented
643    return 0;
644}
645
646int ParrotCamH::mt9m114_errata_2(void) {
647    //FIXME this doesn't make sense
648    //bit 2 is reserved and the default value is 0 but it is set to 1.
649    //bit 5 is reserved, default is 0 but it is set to 1
650    //MSB is the actual reset register. No idea why the others are changed.
651    soc1040_write_reg(REG_RESET_REGISTER, 564,2); //1000110100
652    return 0;
653}
654
655int ParrotCamH::mt9m114_sensor_optimization(void) {
656    //FIXME all registers used in here are undocumented. No idea about their purpose.
657    //      Maybe some commands for the fpga?
658    soc1040_write_reg( REG_DAC_TXLO_ROW,0x8270,2);
659    soc1040_write_reg(REG_DAC_TXLO, 0x8270,2);
660    soc1040_write_reg(REG_DAC_LD_4_5, 0x3605,2);
661    soc1040_write_reg(REG_DAC_LD_6_7, 0x77FF,2);
662    soc1040_write_reg(REG_DAC_ECL, 0xC233,2);
663    soc1040_write_reg(REG_DELTA_DK_CONTROL, 0x87FF,2);
664    soc1040_write_reg(REG_COLUMN_CORRECTION, 0x6080,2);
665    soc1040_write_reg(REG_AE_TRACK_MODE,0x0008,2);
666    return 0;
667}
668
669void ParrotCamH::SetGain(float value) {
670    int ret = 0;
671    uint16_t shiftedValue = value*255;
672
673    printf("gain %i\n",shiftedValue);
674    ret = soc1040_write_reg(REG_UVC_GAIN,shiftedValue,2);
675
676    if(0 != ret) {
677        Thread::Err("error writing value. errno: 0x%x\n",  ret);
678        return ;
679    }
680
681    ret = check_uvc_status( __func__);
682    if(0 != ret) {
683        //error has already been printed by check_uvc_status.
684        return;
685    }
686    uint32_t v;
687    soc1040_read_reg(REG_UVC_GAIN,&v,2);
688    printf("read %i\n",v);
689    mt9m114_refresh();
690}
691
692void ParrotCamH::SetBrightness(float value) {
693    int ret = 0;
694    uint16_t shiftedValue = value*255;
695
696    ret = soc1040_write_reg(REG_UVC_BRIGHTNESS,shiftedValue,2);
697
698    if(0 != ret) {
699        Thread::Err("error writing value. errno: 0x%x\n", ret);
700        return;
701    }
702    ret = check_uvc_status( __func__);
703
704    if(0 != ret) {
705        //error has already been printed by check_uvc_status.
706        return;
707    }
708
709    mt9m114_refresh();
710}
711
712void ParrotCamH::SetSaturation(float value) {
713    int ret = 0;
714    uint16_t shiftedValue = value*255;
715
716    ret = soc1040_write_reg(REG_UVC_SATURATION,shiftedValue,2);
717
718    if(0 != ret) {
719        Thread::Err("error writing value. errno: 0x%x\n",  ret);
720        return ;
721    }
722
723    ret = check_uvc_status( __func__);
724    if(0 != ret) {
725        //error has already been printed by check_uvc_status.
726        return;
727    }
728
729    mt9m114_refresh();
730}
731
732void ParrotCamH::SetExposure(float value) {
733    int ret = 0;
734    uint32_t shiftedValue = value*350;//trouve experimentalment
735
736    ret = soc1040_write_reg(REG_UVC_EXPOSURE_TIME,shiftedValue,4);
737
738    if(0 != ret) {
739        Thread::Err("error writing value. errno: 0x%x\n",  ret);
740        return ;
741    }
742
743    ret = check_uvc_status( __func__);
744    if(0 != ret) {
745        //error has already been printed by check_uvc_status.
746        return;
747    }
748
749    mt9m114_refresh();
750}
751
752void ParrotCamH::SetHue(float value) {
753    int8_t shiftedValue = (value-.5)*255;
754
755    int ret = soc1040_write_reg(REG_CAM_HUE_ANGLE, shiftedValue,1);
756
757    if(0 != ret) {
758        Thread::Err("error writing value. errno: 0x%x\n",  ret);
759        return ;
760    }
761
762    mt9m114_refresh();
763}
764//not exactly what the name suggests. See chip documentation
765void ParrotCamH::SetContrast(float value) {
766    int ret = 0;
767    uint16_t shiftedValue = 16+(value*48);//min 16, max 64
768
769    ret = soc1040_write_reg(REG_UVC_CONTRAST,shiftedValue,2);
770
771    if(0 != ret) {
772        Thread::Err("error writing value. errno: 0x%x\n", ret);
773        return ;
774    }
775    ret = check_uvc_status( __func__);
776    if(0 != ret) {
777        //error has already been printed by check_uvc_status.
778        return ;
779    }
780    mt9m114_refresh();
781}
782
783void ParrotCamH::SetAutoExposure(bool value) {
784    if(value == true) {
785        soc1040_write_reg( REG_UVC_AE_MODE, 0x02,1);
786    } else {
787        soc1040_write_reg( REG_UVC_AE_MODE, 0x01,1);
788    }
789
790    mt9m114_refresh();
791}
792
793/**
794 * soc1040_read_reg - Read a value from a register in an soc1040 sensor device
795 * @reg: register address / offset
796 * @val: stores the value that gets read
797 * @data_length: length of data to be read
798 *
799 * Read a value from a register in an soc1040 sensor device.
800 * The value is returned in 'val'.
801 * Returns zero if successful, or non-zero otherwise.
802 */
803int ParrotCamH::soc1040_read_reg(uint16_t reg, uint32_t *val,uint16_t data_length) {
804    int err;
805    unsigned char data[4];
806    unsigned char add[2];
807
808    if (data_length != I2C_8BIT && data_length != I2C_16BIT
809            && data_length != I2C_32BIT)
810        return -EINVAL;
811
812    /* Write addr - high byte goes out first */
813    add[0] = (uint8_t) (reg >> 8);;
814    add[1] = (uint8_t) (reg & 0xff);
815    err=i2cport->Write(add,2);
816
817    /* Read back data */
818    if (err >= 0) {
819        err = i2cport->Read(data,data_length);
820    }
821    if (err >= 0) {
822        *val = 0;
823        /* high byte comes first */
824        if (data_length == I2C_8BIT)
825            *val = data[0];
826        else if (data_length == I2C_16BIT)
827            *val = data[1] + (data[0] << 8);
828        else
829            *val = data[3] + (data[2] << 8) +
830                   (data[1] << 16) + (data[0] << 24);
831        return 0;
832    }
833    Thread::Err("read from offset 0x%x error %d\n", reg, err);
834    return err;
835}
836
837/**
838 * Write a value to a register in soc1040 sensor device.
839 * @reg: Address of the register to read value from.
840 * @val: Value to be written to a specific register.
841 * Returns zero if successful, or non-zero otherwise.
842 */
843int ParrotCamH::soc1040_write_reg(uint16_t reg,uint32_t val, uint16_t data_length) {
844    int err = 0;
845    unsigned char data[6];
846
847    if (data_length != I2C_8BIT && data_length != I2C_16BIT
848            && data_length != I2C_32BIT)
849        return -EINVAL;
850
851
852    /* high byte goes out first */
853    data[0] = (uint8_t) (reg >> 8);
854    data[1] = (uint8_t) (reg & 0xff);
855    if (data_length == I2C_8BIT) {
856        data[2] = val & 0xff;
857    } else if (data_length == I2C_16BIT) {
858        data[2] = (val >> 8) & 0xff;
859        data[3] = val & 0xff;
860    } else {
861        data[2] = (val >> 24) & 0xff;
862        data[3] = (val >> 16) & 0xff;
863        data[4] = (val >> 8) & 0xff;
864        data[5] = val & 0xff;
865    }
866
867    err = i2cport->Write(data,2+data_length);
868
869    if (err >= 0) return 0;
870
871    Thread::Err("write from offset 0x%x error %d\n", reg, err);
872
873    return err;
874}
875
876/*
877 * Write a list of register settings; ff/ff stops the process.
878 */
879int ParrotCamH::mt9m114_write_array(struct regval_list *vals) {
880    int i=0;
881
882    while ((vals[i].reg_num != 0xffff) || (vals[i].value != 0xffff)) {
883        int ret = soc1040_write_reg(vals[i].reg_num,  vals[i].value,vals[i].size);
884        if (ret < 0) {
885            return ret;
886        }
887        i++;
888    }
889    return 0;
890}
891/**
892* Reads the uvc_result_status register.
893* If it contains an error the error is printed to dmesg together with funcName.
894*
895* @note uvc_result_status updates on vertical blanking. Therefore it might not
896* contain the correct value if you do not wait until vertical blanking.
897* @param funcName Name of the calling function.
898* @return In case of error: The errno code or -1 if the error code is unknown. 0 Otherwise.
899*/
900int ParrotCamH::check_uvc_status(const char* funcName) {
901    uint32_t result;
902    soc1040_read_reg(REG_UVC_RESULT_STATUS, &result,1);
903    /* uvc_result_status can contain the following error codes:
904    0x00 ENOERR no error - change was accepted and actioned.
905    0x08 EACCES permission denied.
906    0x09 EBUSY entity busy, cannot support operation.
907    0x0C EINVAL invalid argument.
908    0x0E ERANGE parameter out-of-range.
909    0x0F ENOSYS operation not supported.
910    */
911    switch(result) {
912    case 0: //no error
913        //Printf("%s REG_UVC_RESULT_STATUS: ENOERR\n", funcName);
914        return 0;
915    case 0x08: //EACCES
916        Printf("%s REG_UVC_RESULT_STATUS: EACCES\n", funcName);
917        return EACCES;
918    case 0x09: //EBUSY
919        Printf("%s REG_UVC_RESULT_STATUS: EBUSY\n", funcName);
920        return EBUSY;
921    case 0x0C: //EINVAL
922        Printf("%s REG_UVC_RESULT_STATUS: EINVAL\n", funcName);
923        return EINVAL;
924    case 0x0E: //ERANGE
925        Printf("%s REG_UVC_RESULT_STATUS: ERANGE\n", funcName);
926        return ERANGE;
927    case 0x0F: //ENOSYS
928        Printf("%s REG_UVC_RESULT_STATUS: ENOSYS\n", funcName);
929        return ENOSYS;
930    default:
931        Printf("%s REG_UVC_RESULT_STATUS: Unknown error code\n", funcName);
932        return -1;
933    }
934}
935
936/**
937* Refresh subsystems without requiring a sensor configuration change.
938* @note This call blocks till the next frame.
939* @return
940*/
941int ParrotCamH::mt9m114_refresh(void) {
942    //Printf("referesh\n");
943    uint32_t v=0;//a temporary variable used for several read commands
944    //make sure that the refresh command is really processed and that
945    //exposure and user changes are processed as well.
946    soc1040_read_reg(REG_UVC_ALGO, &v,2);
947    v |= 0b111;
948    soc1040_write_reg(REG_UVC_ALGO, v,2);
949    //Changes to REG_UVC_ALGO take effect on vertical blanking, therefore wait one frame.
950    mt9m114_wait_num_frames(1);
951    // First check that the FW is ready to accept a Refresh command
952    soc1040_read_reg(REG_COMMAND_REGISTER, &v,2);
953    if (v & HOST_COMMAND_REFRESH) {
954        Thread::Warn("Refresh cmd bit is already set 0x%x\n",v);
955        return -1;
956    }
957    // Issue the Refresh command, and set the 'OK' bit at the time time so
958    //we can detect if the command fails
959    soc1040_write_reg(REG_COMMAND_REGISTER, HOST_COMMAND_REFRESH|HOST_COMMAND_OK,2);
960    // Wait for the FW to complete the command
961    poll_command_register_bit(HOST_COMMAND_REFRESH);
962    // Check the 'OK' bit to see if the command was successful
963    soc1040_read_reg( REG_COMMAND_REGISTER, &v,2);
964    if ( !(v & HOST_COMMAND_OK)) {
965        Thread::Err("refresh command fail\n");
966        return -1;
967    }
968    //check refresh command error code
969    soc1040_read_reg(REG_SEQ_ERROR_CODE, &v,1);
970    if(v != 0) {
971        Thread::Err("Refresh ERROR: %x\n", v);
972    }
973    soc1040_read_reg( REG_UVC_RESULT_STATUS, &v,1);
974    //Printf("REG_UVC_RESULT_STATUS: %x\n", v);
975    //the refresh command schedules an update on the next end of frame.
976    //It does not wait until the end of frame is actually reached.
977    //Therefore we need to wait until the end of the frame manually.
978    mt9m114_wait_num_frames(1);
979    //Printf("referesh ok\n");
980    return 0;
981}
982
983/**
984* Waits until a number of frames have passed
985* This method can be used to wait for vertical blanking as vertical blanking
986* occurs at the end of a frame.
987* @param sd
988* @param numFrames the number of frames to wait
989* @return -1 in case of error, 0 otherwise.
990*/
991int ParrotCamH::mt9m114_wait_num_frames(uint16_t numFrames) {
992    //Printf("wait\n");
993    //FIXME copy & paste from set_state_command
994    uint32_t v=0;
995    uint32_t frameCountBefore = 0;
996    uint32_t frameCountAfter = 0;
997    //get the current frame count
998    soc1040_read_reg(CAM_MON_HEARTBEAT,&frameCountBefore,2);
999    //Printf("frame %i\n",frameCountBefore);
1000    //specify for which event we want to wait: 2 = start of next frame
1001    soc1040_write_reg(REG_CMD_HANDLER_WAIT_FOR_EVENT, 2, 2);
1002    //specify for how much frames we want to wait
1003    soc1040_write_reg( REG_CMD_HANDLER_NUM_WAIT_EVENTS, numFrames,2);
1004    // (Optional) First check that the FW is ready to accept a new command
1005    soc1040_read_reg(REG_COMMAND_REGISTER, &v,2);
1006    if (v & HOST_COMMAND_WAIT_FOR_EVENT) {
1007        //This should never happen as long as nobody opens the driver device in async mode.
1008        Thread::Warn("Host command wait for event already set 0x%x\n",v);
1009        return -1;
1010    }
1011    // (Mandatory) Issue the wait for command
1012    // We set the 'OK' bit so we can detect if the command fails. The chip will unset the OK bit if everything is ok.
1013    soc1040_write_reg(REG_COMMAND_REGISTER, HOST_COMMAND_WAIT_FOR_EVENT | HOST_COMMAND_OK,2);
1014    // Wait for the FW to complete the command (clear the HOST_COMMAND_1 bit)
1015    poll_command_register_bit(HOST_COMMAND_WAIT_FOR_EVENT);
1016    // Check the 'OK' bit to see if the command was successful
1017    soc1040_read_reg(REG_COMMAND_REGISTER, &v,2);
1018    if ( !(v & HOST_COMMAND_OK)) {
1019        Thread::Warn("wait for end of frame failed: TIMEOUT?!\n");
1020        return -1;
1021    }
1022    //read frame count after
1023    soc1040_read_reg(CAM_MON_HEARTBEAT, &frameCountAfter,2);
1024    if(frameCountBefore == frameCountAfter) {
1025        Thread::Warn("wait for end of frame failed. Frame is still the same.\n");
1026    }
1027    //Printf("wait ok\n");
1028    return 0;
1029}
1030
1031/**
1032*
1033* @param bit_mask
1034* @return 0 if everything is ok, 1 otherwise
1035*/
1036int ParrotCamH::poll_command_register_bit(uint16_t bit_mask) {
1037    int i=0;
1038    uint32_t v=0;
1039    //Printf("poll_command_register_bit\n");
1040    for (i = 0; i < 500; i++) {
1041        SleepMS(10);
1042        soc1040_read_reg(REG_COMMAND_REGISTER, &v,2);
1043        if (!(v & bit_mask)) {
1044            //Printf("poll_command_register_bit ok\n");
1045            return 0;
1046        }
1047    }
1048    Printf("poll_command_register_bit ko\n");
1049    return 1;
1050}
1051
1052} // end namespace sensor
1053} // end namespace flair
Note: See TracBrowser for help on using the repository browser.