1 | // created: 2014/07/16
|
---|
2 | // filename: ParrotCamV.cpp
|
---|
3 | //
|
---|
4 | // author: Guillaume Sanahuja
|
---|
5 | //
|
---|
6 | // version: $Id: $
|
---|
7 | //
|
---|
8 | // purpose: class for ardrone2 horizontal camera
|
---|
9 | //
|
---|
10 | //
|
---|
11 | /*********************************************************************/
|
---|
12 |
|
---|
13 | //control functions from V4L2 driver:
|
---|
14 |
|
---|
15 | /*
|
---|
16 | * A V4L2 driver for OmniVision OV7670 cameras.
|
---|
17 | *
|
---|
18 | * Copyright 2006 One Laptop Per Child Association, Inc. Written
|
---|
19 | * by Jonathan Corbet with substantial inspiration from Mark
|
---|
20 | * McClelland's ovcamchip code.
|
---|
21 | *
|
---|
22 | * Copyright 2006-7 Jonathan Corbet <corbet@lwn.net>
|
---|
23 | *
|
---|
24 | * This file may be distributed under the terms of the GNU General
|
---|
25 | * Public License, version 2.
|
---|
26 | */
|
---|
27 |
|
---|
28 | #include "ParrotCamV.h"
|
---|
29 | #include <Unix_I2cPort.h>
|
---|
30 |
|
---|
31 | #define PI ((float)3.14159265358979323846)
|
---|
32 |
|
---|
33 | /* Registers */
|
---|
34 | #define REG_GAIN 0x00 /* Gain lower 8 bits (rest in vref) */
|
---|
35 | #define REG_BLUE 0x01 /* blue gain */
|
---|
36 | #define REG_RED 0x02 /* red gain */
|
---|
37 | #define REG_VREF 0x03 /* Pieces of GAIN, VSTART, VSTOP */
|
---|
38 | #define REG_COM1 0x04 /* Control 1 */
|
---|
39 | #define COM1_CCIR656 0x40 /* CCIR656 enable */
|
---|
40 | #define REG_BAVE 0x05 /* U/B Average level */
|
---|
41 | #define REG_GbAVE 0x06 /* Y/Gb Average level */
|
---|
42 | #define REG_AECHH 0x07 /* AEC MS 5 bits */
|
---|
43 | #define REG_RAVE 0x08 /* V/R Average level */
|
---|
44 | #define REG_COM2 0x09 /* Control 2 */
|
---|
45 | #define COM2_SSLEEP 0x10 /* Soft sleep mode */
|
---|
46 | #define REG_PID 0x0a /* Product ID MSB */
|
---|
47 | #define REG_VER 0x0b /* Product ID LSB */
|
---|
48 | #define REG_COM3 0x0c /* Control 3 */
|
---|
49 | #define COM3_SWAP 0x40 /* Byte swap */
|
---|
50 | #define COM3_SCALEEN 0x08 /* Enable scaling */
|
---|
51 | #define COM3_DCWEN 0x04 /* Enable downsamp/crop/window */
|
---|
52 | #define REG_COM4 0x0d /* Control 4 */
|
---|
53 | #define REG_COM5 0x0e /* All "reserved" */
|
---|
54 | #define REG_COM6 0x0f /* Control 6 */
|
---|
55 | #define REG_AECH 0x10 /* More bits of AEC value */
|
---|
56 | #define REG_CLKRC 0x11 /* Clocl control */
|
---|
57 | #define CLK_EXT 0x40 /* Use external clock directly */
|
---|
58 | #define CLK_SCALE 0x3f /* Mask for internal clock scale */
|
---|
59 | #define REG_COM7 0x12 /* Control 7 */
|
---|
60 | #define COM7_RESET 0x80 /* Register reset */
|
---|
61 | #define COM7_FMT_MASK 0x38
|
---|
62 | #define COM7_FMT_VGA 0x00
|
---|
63 | #define COM7_FMT_CIF 0x20 /* CIF format */
|
---|
64 | #define COM7_FMT_QVGA 0x00 /* QVGA format */
|
---|
65 | #define COM7_FMT_QCIF 0x08 /* QCIF format */
|
---|
66 | #define COM7_RGB 0x04 /* bits 0 and 2 - RGB format */
|
---|
67 | #define COM7_YUV 0x00 /* YUV */
|
---|
68 | #define COM7_BAYER 0x01 /* Bayer format */
|
---|
69 | #define COM7_PBAYER 0x05 /* "Processed bayer" */
|
---|
70 | #define REG_COM8 0x13 /* Control 8 */
|
---|
71 | #define COM8_FASTAEC 0x80 /* Enable fast AGC/AEC */
|
---|
72 | #define COM8_AECSTEP 0x40 /* Unlimited AEC step size */
|
---|
73 | #define COM8_BFILT 0x20 /* Band filter enable */
|
---|
74 | #define COM8_AGC 0x04 /* Auto gain enable */
|
---|
75 | #define COM8_AWB 0x02 /* White balance enable */
|
---|
76 | #define COM8_AEC 0x01 /* Auto exposure enable */
|
---|
77 | #define REG_COM9 0x14 /* Control 9 - gain ceiling */
|
---|
78 | #define REG_COM10 0x15 /* Control 10 */
|
---|
79 | #define COM10_HSYNC 0x40 /* HSYNC instead of HREF */
|
---|
80 | #define COM10_PCLK_HB 0x20 /* Suppress PCLK on horiz blank */
|
---|
81 | #define COM10_HREF_REV 0x08 /* Reverse HREF */
|
---|
82 | #define COM10_VS_LEAD 0x04 /* VSYNC on clock leading edge */
|
---|
83 | #define COM10_VS_NEG 0x02 /* VSYNC negative */
|
---|
84 | #define COM10_HS_NEG 0x01 /* HSYNC negative */
|
---|
85 | #define REG_HSTART 0x17 /* Horiz start high bits */
|
---|
86 | #define REG_HSTOP 0x18 /* Horiz stop high bits */
|
---|
87 | #define REG_VSTART 0x19 /* Vert start high bits */
|
---|
88 | #define REG_VSTOP 0x1a /* Vert stop high bits */
|
---|
89 | #define REG_PSHFT 0x1b /* Pixel delay after HREF */
|
---|
90 | #define REG_MIDH 0x1c /* Manuf. ID high */
|
---|
91 | #define REG_MIDL 0x1d /* Manuf. ID low */
|
---|
92 | #define REG_MVFP 0x1e /* Mirror / vflip */
|
---|
93 | #define MVFP_MIRROR 0x20 /* Mirror image */
|
---|
94 | #define MVFP_FLIP 0x10 /* Vertical flip */
|
---|
95 |
|
---|
96 | #define REG_AEW 0x24 /* AGC upper limit */
|
---|
97 | #define REG_AEB 0x25 /* AGC lower limit */
|
---|
98 | #define REG_VPT 0x26 /* AGC/AEC fast mode op region */
|
---|
99 | #define REG_HSYST 0x30 /* HSYNC rising edge delay */
|
---|
100 | #define REG_HSYEN 0x31 /* HSYNC falling edge delay */
|
---|
101 | #define REG_HREF 0x32 /* HREF pieces */
|
---|
102 | #define REG_TSLB 0x3a /* lots of stuff */
|
---|
103 | #define TSLB_YLAST 0x04 /* UYVY or VYUY - see com13 */
|
---|
104 | #define REG_COM11 0x3b /* Control 11 */
|
---|
105 | #define COM11_NIGHT 0x80 /* NIght mode enable */
|
---|
106 | #define COM11_NMFR 0x60 /* Two bit NM frame rate */
|
---|
107 | #define COM11_HZAUTO 0x10 /* Auto detect 50/60 Hz */
|
---|
108 | #define COM11_50HZ 0x08 /* Manual 50Hz select */
|
---|
109 | #define COM11_EXP 0x02
|
---|
110 | #define REG_COM12 0x3c /* Control 12 */
|
---|
111 | #define COM12_HREF 0x80 /* HREF always */
|
---|
112 | #define REG_COM13 0x3d /* Control 13 */
|
---|
113 | #define COM13_GAMMA 0x80 /* Gamma enable */
|
---|
114 | #define COM13_UVSAT 0x40 /* UV saturation auto adjustment */
|
---|
115 | #define COM13_UVSWAP 0x01 /* V before U - w/TSLB */
|
---|
116 | #define REG_COM14 0x3e /* Control 14 */
|
---|
117 | #define COM14_DCWEN 0x10 /* DCW/PCLK-scale enable */
|
---|
118 | #define REG_EDGE 0x3f /* Edge enhancement factor */
|
---|
119 | #define REG_COM15 0x40 /* Control 15 */
|
---|
120 | #define COM15_R10F0 0x00 /* Data range 10 to F0 */
|
---|
121 | #define COM15_R01FE 0x80 /* 01 to FE */
|
---|
122 | #define COM15_R00FF 0xc0 /* 00 to FF */
|
---|
123 | #define COM15_RGB565 0x10 /* RGB565 output */
|
---|
124 | #define COM15_RGB555 0x30 /* RGB555 output */
|
---|
125 | #define REG_COM16 0x41 /* Control 16 */
|
---|
126 | #define COM16_AWBGAIN 0x08 /* AWB gain enable */
|
---|
127 | #define REG_COM17 0x42 /* Control 17 */
|
---|
128 | #define COM17_AECWIN 0xc0 /* AEC window - must match COM4 */
|
---|
129 | #define COM17_CBAR 0x08 /* DSP Color bar */
|
---|
130 |
|
---|
131 | /*
|
---|
132 | * This matrix defines how the colors are generated, must be
|
---|
133 | * tweaked to adjust hue and saturation.
|
---|
134 | *
|
---|
135 | * Order: v-red, v-green, v-blue, u-red, u-green, u-blue
|
---|
136 | *
|
---|
137 | * They are nine-bit signed quantities, with the sign bit
|
---|
138 | * stored in 0x58. Sign for v-red is bit 0, and up from there.
|
---|
139 | */
|
---|
140 | #define REG_CMATRIX_BASE 0x4f
|
---|
141 | //#define CMATRIX_LEN 6
|
---|
142 | #define REG_CMATRIX_SIGN 0x58
|
---|
143 |
|
---|
144 |
|
---|
145 | #define REG_BRIGHT 0x55 /* Brightness */
|
---|
146 | #define REG_CONTRAS 0x56 /* Contrast control */
|
---|
147 |
|
---|
148 | #define REG_GFIX 0x69 /* Fix gain control */
|
---|
149 |
|
---|
150 | #define REG_REG76 0x76 /* OV's name */
|
---|
151 | #define R76_BLKPCOR 0x80 /* Black pixel correction enable */
|
---|
152 | #define R76_WHTPCOR 0x40 /* White pixel correction enable */
|
---|
153 |
|
---|
154 | #define REG_RGB444 0x8c /* RGB 444 control */
|
---|
155 | #define R444_ENABLE 0x02 /* Turn on RGB444, overrides 5x5 */
|
---|
156 | #define R444_RGBX 0x01 /* Empty nibble at end */
|
---|
157 |
|
---|
158 | #define REG_HAECC1 0x9f /* Hist AEC/AGC control 1 */
|
---|
159 | #define REG_HAECC2 0xa0 /* Hist AEC/AGC control 2 */
|
---|
160 |
|
---|
161 | #define REG_BD50MAX 0xa5 /* 50hz banding step limit */
|
---|
162 | #define REG_HAECC3 0xa6 /* Hist AEC/AGC control 3 */
|
---|
163 | #define REG_HAECC4 0xa7 /* Hist AEC/AGC control 4 */
|
---|
164 | #define REG_HAECC5 0xa8 /* Hist AEC/AGC control 5 */
|
---|
165 | #define REG_HAECC6 0xa9 /* Hist AEC/AGC control 6 */
|
---|
166 | #define REG_HAECC7 0xaa /* Hist AEC/AGC control 7 */
|
---|
167 | #define REG_BD60MAX 0xab /* 60hz banding step limit */
|
---|
168 |
|
---|
169 | using std::string;
|
---|
170 | using namespace flair::core;
|
---|
171 |
|
---|
172 | namespace flair { namespace sensor {
|
---|
173 |
|
---|
174 | ParrotCamV::ParrotCamV(const FrameworkManager* parent,string name,uint8_t priority) : V4LCamera(parent,name,2,320,240,cvimage::Type::Format::UYVY,priority) {
|
---|
175 | //i2cport
|
---|
176 | //we assume there is no other device on this bus
|
---|
177 | i2cport=new Unix_I2cPort((Thread*)this,"camv-i2c","/dev/i2c-2");
|
---|
178 | i2cport->SetSlave(0x42>>1);
|
---|
179 | }
|
---|
180 |
|
---|
181 | ParrotCamV::~ParrotCamV() {
|
---|
182 |
|
---|
183 | }
|
---|
184 |
|
---|
185 | void ParrotCamV::SetGain(float value) {
|
---|
186 | if(ov7670_write(REG_GAIN, (int)(value*255) & 0xff)!=2) {
|
---|
187 | Thread::Err("error writting gain\n");
|
---|
188 | }
|
---|
189 | }
|
---|
190 |
|
---|
191 | void ParrotCamV::SetAutoGain(bool value) {
|
---|
192 | unsigned char com8;
|
---|
193 |
|
---|
194 | if(ov7670_read(REG_COM8, &com8)!=1) {
|
---|
195 | Thread::Err("error reading com8\n");
|
---|
196 | return;
|
---|
197 | }
|
---|
198 |
|
---|
199 | if (value) com8 |= COM8_AGC;
|
---|
200 | else com8 &= ~COM8_AGC;
|
---|
201 |
|
---|
202 | if(ov7670_write(REG_COM8, com8)!=2) {
|
---|
203 | Thread::Err("error writting com8\n");
|
---|
204 | }
|
---|
205 | }
|
---|
206 |
|
---|
207 | void ParrotCamV::SetAutoExposure(bool value) {
|
---|
208 | unsigned char com8;
|
---|
209 |
|
---|
210 | if(ov7670_read(REG_COM8, &com8)!=1) {
|
---|
211 | Thread::Err("error reading com8\n");
|
---|
212 | return;
|
---|
213 | }
|
---|
214 |
|
---|
215 | if (value) com8 |= COM8_AEC;
|
---|
216 | else com8 &= ~COM8_AEC;
|
---|
217 |
|
---|
218 | if(ov7670_write(REG_COM8, com8)!=2) {
|
---|
219 | Thread::Err("error writting com8\n");
|
---|
220 | }
|
---|
221 | }
|
---|
222 |
|
---|
223 | void ParrotCamV::SetExposure(float value) {
|
---|
224 | unsigned char com1,aech, aechh;
|
---|
225 | uint16_t value16=value*255.;//16 bit register but values after 255 does not change anything...
|
---|
226 |
|
---|
227 | if(ov7670_read(REG_COM1, &com1)!=1) {
|
---|
228 | Thread::Err("error reading com1\n");
|
---|
229 | return;
|
---|
230 | }
|
---|
231 |
|
---|
232 | if(ov7670_read(REG_AECHH, &aechh)!=1) {
|
---|
233 | Thread::Err("error reading aechh\n");
|
---|
234 | return;
|
---|
235 | }
|
---|
236 |
|
---|
237 | com1 = (com1 & 0xfc) | (value16 & 0x03);
|
---|
238 | aech = (value16 >> 2) & 0xff;
|
---|
239 | aechh = (aechh & 0xc0) | ((value16 >> 10) & 0x3f);
|
---|
240 |
|
---|
241 | if(ov7670_write(REG_COM1, com1)!=2) {
|
---|
242 | Thread::Err("error writting com1\n");
|
---|
243 | }
|
---|
244 | if(ov7670_write(REG_AECH, aech)!=2) {
|
---|
245 | Thread::Err("error writting aech\n");
|
---|
246 | }
|
---|
247 | if(ov7670_write(REG_AECHH, aechh)!=2) {
|
---|
248 | Thread::Err("error writting aechh\n");
|
---|
249 | }
|
---|
250 | }
|
---|
251 |
|
---|
252 | void ParrotCamV::SetBrightness(float value) {
|
---|
253 | uint8_t v;
|
---|
254 | v = ov7670_abs_to_sm(value*255.);
|
---|
255 |
|
---|
256 | if(ov7670_write(REG_BRIGHT, v)!=2) {
|
---|
257 | Thread::Err("error writting REG_BRIGHT\n");
|
---|
258 | }
|
---|
259 | }
|
---|
260 |
|
---|
261 | void ParrotCamV::SetSaturation(float value) {
|
---|
262 | int matrix[CMATRIX_LEN];
|
---|
263 |
|
---|
264 | sat=value;
|
---|
265 |
|
---|
266 | ov7670_calc_cmatrix(matrix);
|
---|
267 | ov7670_store_cmatrix(matrix);
|
---|
268 | }
|
---|
269 |
|
---|
270 | void ParrotCamV::SetHue(float value) {
|
---|
271 | int matrix[CMATRIX_LEN];
|
---|
272 |
|
---|
273 | hue = value*2*PI-PI;
|
---|
274 |
|
---|
275 | ov7670_calc_cmatrix(matrix);
|
---|
276 | ov7670_store_cmatrix(matrix);
|
---|
277 | }
|
---|
278 |
|
---|
279 | void ParrotCamV::SetContrast(float value) {
|
---|
280 | ov7670_write(REG_CONTRAS, (unsigned char)(value*255));
|
---|
281 | }
|
---|
282 |
|
---|
283 | unsigned char ParrotCamV::ov7670_abs_to_sm(uint8_t v) {
|
---|
284 | if(v==0) v=1;//0 non gére sinon...
|
---|
285 | if (v > 127) return v & 0x7f;
|
---|
286 | return (128 - v) | 0x80;
|
---|
287 | }
|
---|
288 |
|
---|
289 | int ParrotCamV::ov7670_store_cmatrix(int matrix[CMATRIX_LEN]) {
|
---|
290 | int i, ret;
|
---|
291 | unsigned char signbits = 0;
|
---|
292 |
|
---|
293 | /*
|
---|
294 | * Weird crap seems to exist in the upper part of
|
---|
295 | * the sign bits register, so let's preserve it.
|
---|
296 | */
|
---|
297 | ret = ov7670_read(REG_CMATRIX_SIGN, &signbits);
|
---|
298 | signbits &= 0xc0;
|
---|
299 |
|
---|
300 | for (i = 0; i < CMATRIX_LEN; i++) {
|
---|
301 | unsigned char raw;
|
---|
302 |
|
---|
303 | if (matrix[i] < 0) {
|
---|
304 | signbits |= (1 << i);
|
---|
305 | if (matrix[i] < -255)
|
---|
306 | raw = 0xff;
|
---|
307 | else
|
---|
308 | raw = (-1 * matrix[i]) & 0xff;
|
---|
309 | }
|
---|
310 | else {
|
---|
311 | if (matrix[i] > 255)
|
---|
312 | raw = 0xff;
|
---|
313 | else
|
---|
314 | raw = matrix[i] & 0xff;
|
---|
315 | }
|
---|
316 | ret += ov7670_write(REG_CMATRIX_BASE + i, raw);
|
---|
317 | }
|
---|
318 | ret += ov7670_write(REG_CMATRIX_SIGN, signbits);
|
---|
319 | return ret;
|
---|
320 | }
|
---|
321 |
|
---|
322 | void ParrotCamV::ov7670_calc_cmatrix(int matrix[CMATRIX_LEN]) {
|
---|
323 | int i;
|
---|
324 | /*
|
---|
325 | * Apply the current saturation setting first.
|
---|
326 | */
|
---|
327 | for (i = 0; i < CMATRIX_LEN; i++)
|
---|
328 | matrix[i] = (cmatrix[i]*(int)(sat*255)) >> 7;
|
---|
329 | /*
|
---|
330 | * Then, if need be, rotate the hue value.
|
---|
331 | */
|
---|
332 | if (hue != 0) {
|
---|
333 | float sinth, costh;
|
---|
334 |
|
---|
335 | sinth = sinf(hue);
|
---|
336 | costh = cosf(hue);
|
---|
337 |
|
---|
338 | matrix[0] = matrix[3]*sinth + matrix[0]*costh;
|
---|
339 | matrix[1] = matrix[4]*sinth + matrix[1]*costh;
|
---|
340 | matrix[2] = matrix[5]*sinth + matrix[2]*costh;
|
---|
341 | matrix[3] = matrix[3]*costh - matrix[0]*sinth;
|
---|
342 | matrix[4] = matrix[4]*costh - matrix[1]*sinth;
|
---|
343 | matrix[5] = matrix[5]*costh - matrix[2]*sinth;
|
---|
344 | }
|
---|
345 | }
|
---|
346 |
|
---|
347 | int ParrotCamV::ov7670_write(uint8_t reg,uint8_t value) {
|
---|
348 | uint8_t tx[2];
|
---|
349 |
|
---|
350 | tx[0]=reg;
|
---|
351 | tx[1]=value;
|
---|
352 |
|
---|
353 | return i2cport->Write(tx, 2);
|
---|
354 | }
|
---|
355 |
|
---|
356 | int ParrotCamV::ov7670_read(uint8_t reg,uint8_t *value) {
|
---|
357 | uint8_t tx=reg;
|
---|
358 |
|
---|
359 | if(i2cport->Write(&tx,1)!=1) {
|
---|
360 | Thread::Err("error writing register address\n");
|
---|
361 | }
|
---|
362 | if(i2cport->Read(value,1)!=1) {
|
---|
363 | Thread::Err("error reading register\n");
|
---|
364 | }
|
---|
365 |
|
---|
366 | return 1;
|
---|
367 | }
|
---|
368 |
|
---|
369 | } // end namespace sensor
|
---|
370 | } // end namespace flair
|
---|