Sumio Morioka / Mbed 2 deprecated if201411_section10

Sample code of section 10 in Nov 2014 issue of the Interface Magazine, published by CQ publishing in Japan. CQ出版社インターフェース誌 2014年11月号10章に掲載のサンプルコードです． LPC1768にトラ技OV7670モジュールを接続したうえで，省メモリ画像処理（色による簡易顔検出）を行う事例です．画像フレームバッファを確保できないとき，ストリーム処理で画像を加工するのがテーマです．このコードのうちカメラ制御部には，Sadaei Osakabe氏のコードを流用させていただいています．

Dependencies:   TextLCD mbed

main.cpp@0:d2c78848026d, 2014-09-24 (annotated)

Committer:
smorioka
Date:
Wed Sep 24 20:48:23 2014 +0000
Revision:
0:d2c78848026d
First release;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
smorioka 0:d2c78848026d 1 /*
smorioka 0:d2c78848026d 2 * Copyright (c) 2014, Sumio Morioka
smorioka 0:d2c78848026d 3 * All rights reserved.
smorioka 0:d2c78848026d 4 *
smorioka 0:d2c78848026d 5 * This source code was originally written by Dr.Sumio Morioka for use in the Nov 2014 issue of
smorioka 0:d2c78848026d 6 * "the Interface magazine", published by CQ publishing Co.Ltd in Japan (http://www.cqpub.co.jp).
smorioka 0:d2c78848026d 7 * The author has no responsibility on any results caused by using this code.
smorioka 0:d2c78848026d 8 *
smorioka 0:d2c78848026d 9 * - Distribution date of this code: Sep 24, 2014
smorioka 0:d2c78848026d 10 * - Author: Dr.Sumio Morioka (http://www002.upp.so-net.ne.jp/morioka)
smorioka 0:d2c78848026d 11 *
smorioka 0:d2c78848026d 12 *
smorioka 0:d2c78848026d 13 * IMPORTANT NOTICE:
smorioka 0:d2c78848026d 14 * Redistribution and use in source and binary forms, with or without
smorioka 0:d2c78848026d 15 * modification, are permitted provided that the following conditions are met:
smorioka 0:d2c78848026d 16 * * Redistributions of source code must retain the above copyright
smorioka 0:d2c78848026d 17 * notice, this list of conditions and the following disclaimer.
smorioka 0:d2c78848026d 18 * * Redistributions in binary form must reproduce the above copyright
smorioka 0:d2c78848026d 19 * notice, this list of conditions and the following disclaimer in the
smorioka 0:d2c78848026d 20 * documentation and/or other materials provided with the distribution.
smorioka 0:d2c78848026d 21 * * Neither the name of the copyright holder nor the
smorioka 0:d2c78848026d 22 * names of its contributors may be used to endorse or promote products
smorioka 0:d2c78848026d 23 * derived from this software without specific prior written permission.
smorioka 0:d2c78848026d 24 *
smorioka 0:d2c78848026d 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
smorioka 0:d2c78848026d 26 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
smorioka 0:d2c78848026d 27 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
smorioka 0:d2c78848026d 28 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
smorioka 0:d2c78848026d 29 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
smorioka 0:d2c78848026d 30 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
smorioka 0:d2c78848026d 31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
smorioka 0:d2c78848026d 32 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
smorioka 0:d2c78848026d 33 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
smorioka 0:d2c78848026d 34 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
smorioka 0:d2c78848026d 35 */
smorioka 0:d2c78848026d 36
smorioka 0:d2c78848026d 37 #include "mbed.h"
smorioka 0:d2c78848026d 38 #include "TextLCD.h"
smorioka 0:d2c78848026d 39 #include "LocalFileSystem.h"
smorioka 0:d2c78848026d 40
smorioka 0:d2c78848026d 41 #include "ov7670.h"
smorioka 0:d2c78848026d 42
smorioka 0:d2c78848026d 43 TextLCD lcd(p24, p26, p27, p28, p29, p30);
smorioka 0:d2c78848026d 44 DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
smorioka 0:d2c78848026d 45 PwmOut sv0(p21), sv1(p22), sv2(p23);
smorioka 0:d2c78848026d 46
smorioka 0:d2c78848026d 47 LocalFileSystem local("webfs");
smorioka 0:d2c78848026d 48
smorioka 0:d2c78848026d 49 OV7670 camera(
smorioka 0:d2c78848026d 50 p9,p10, // SDA,SCL(I2C / SCCB)
smorioka 0:d2c78848026d 51 p5,p6,p7, // VSYNC,HREF,WEN(FIFO)
smorioka 0:d2c78848026d 52 p20,p19,p18,p17,p16,p15,p14,p13, // D7-D0
smorioka 0:d2c78848026d 53 p8,p11,p12); // RRST,OE,RCLK
smorioka 0:d2c78848026d 54
smorioka 0:d2c78848026d 55 Timer tmr;
smorioka 0:d2c78848026d 56
smorioka 0:d2c78848026d 57 //#define QQVGA
smorioka 0:d2c78848026d 58 #define QVGA
smorioka 0:d2c78848026d 59 //#define VGA34
smorioka 0:d2c78848026d 60
smorioka 0:d2c78848026d 61 #ifdef QQVGA
smorioka 0:d2c78848026d 62 #define SIZEX 160
smorioka 0:d2c78848026d 63 #define SIZEY 120
smorioka 0:d2c78848026d 64 #endif
smorioka 0:d2c78848026d 65
smorioka 0:d2c78848026d 66 #ifdef QVGA
smorioka 0:d2c78848026d 67 #define SIZEX 320
smorioka 0:d2c78848026d 68 #define SIZEY 240
smorioka 0:d2c78848026d 69 #endif
smorioka 0:d2c78848026d 70
smorioka 0:d2c78848026d 71 #ifdef VGA34
smorioka 0:d2c78848026d 72 #define SIZEX 480
smorioka 0:d2c78848026d 73 #define SIZEY 360
smorioka 0:d2c78848026d 74 #endif
smorioka 0:d2c78848026d 75
smorioka 0:d2c78848026d 76
smorioka 0:d2c78848026d 77 #define SHUTTER_THRESHOLD 3500 // # of skin color pixels
smorioka 0:d2c78848026d 78
smorioka 0:d2c78848026d 79
smorioka 0:d2c78848026d 80 /////////////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 81
smorioka 0:d2c78848026d 82 void rgb2hsv(
smorioka 0:d2c78848026d 83 unsigned char rval,
smorioka 0:d2c78848026d 84 unsigned char gval,
smorioka 0:d2c78848026d 85 unsigned char bval,
smorioka 0:d2c78848026d 86
smorioka 0:d2c78848026d 87 unsigned char *hval,
smorioka 0:d2c78848026d 88 unsigned char *sval,
smorioka 0:d2c78848026d 89 unsigned char *vval
smorioka 0:d2c78848026d 90 )
smorioka 0:d2c78848026d 91 {
smorioka 0:d2c78848026d 92 unsigned char max, min;
smorioka 0:d2c78848026d 93 int ret;
smorioka 0:d2c78848026d 94
smorioka 0:d2c78848026d 95 max = (rval >= gval) ? rval : gval;
smorioka 0:d2c78848026d 96 max = (bval >= max) ? bval : max;
smorioka 0:d2c78848026d 97
smorioka 0:d2c78848026d 98 min = (rval <= gval) ? rval : gval;
smorioka 0:d2c78848026d 99 min = (bval <= min) ? bval : min;
smorioka 0:d2c78848026d 100
smorioka 0:d2c78848026d 101 ///////////////////////////////////////////
smorioka 0:d2c78848026d 102 // compute H
smorioka 0:d2c78848026d 103 ///////////////////////////////////////////
smorioka 0:d2c78848026d 104 if (max == min)
smorioka 0:d2c78848026d 105 ret = 0; // (undef)
smorioka 0:d2c78848026d 106 else if (rval >= gval && rval >= bval) // max: R
smorioka 0:d2c78848026d 107 ret = ((gval - bval) * 60) / (max - min);
smorioka 0:d2c78848026d 108 else if (gval >= rval && gval >= bval) // max: G
smorioka 0:d2c78848026d 109 ret = ((bval - rval) * 60) / (max - min) + 120;
smorioka 0:d2c78848026d 110 else // max: B
smorioka 0:d2c78848026d 111 ret = ((rval - gval) * 60) / (max - min) + 240;
smorioka 0:d2c78848026d 112
smorioka 0:d2c78848026d 113 ret %= 360;
smorioka 0:d2c78848026d 114 if (ret < 0)
smorioka 0:d2c78848026d 115 ret += 360;
smorioka 0:d2c78848026d 116
smorioka 0:d2c78848026d 117 *hval = (ret * 255) / 360;
smorioka 0:d2c78848026d 118
smorioka 0:d2c78848026d 119 ///////////////////////////////////////////
smorioka 0:d2c78848026d 120 // compute S
smorioka 0:d2c78848026d 121 ///////////////////////////////////////////
smorioka 0:d2c78848026d 122 if (max == 0)
smorioka 0:d2c78848026d 123 ret = 0; // (undef)
smorioka 0:d2c78848026d 124 else
smorioka 0:d2c78848026d 125 ret = ((max - min) * 255) / max;
smorioka 0:d2c78848026d 126
smorioka 0:d2c78848026d 127 *sval = ret;
smorioka 0:d2c78848026d 128
smorioka 0:d2c78848026d 129 ///////////////////////////////////////////
smorioka 0:d2c78848026d 130 // compute V
smorioka 0:d2c78848026d 131 ///////////////////////////////////////////
smorioka 0:d2c78848026d 132 *vval = max;
smorioka 0:d2c78848026d 133 }
smorioka 0:d2c78848026d 134
smorioka 0:d2c78848026d 135 /////////////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 136
smorioka 0:d2c78848026d 137 unsigned char judge_skin_color(
smorioka 0:d2c78848026d 138 unsigned char hval,
smorioka 0:d2c78848026d 139 unsigned char sval,
smorioka 0:d2c78848026d 140 unsigned char vval
smorioka 0:d2c78848026d 141 )
smorioka 0:d2c78848026d 142 {
smorioka 0:d2c78848026d 143 if (hval <= 70 && sval >= 25 && sval <= 70 && vval >= 180 && vval <= 250) // <rgain,ggain,bgain> == <0x48,0x40,0x60>
smorioka 0:d2c78848026d 144 return (255);
smorioka 0:d2c78848026d 145 else
smorioka 0:d2c78848026d 146 return (0);
smorioka 0:d2c78848026d 147 }
smorioka 0:d2c78848026d 148
smorioka 0:d2c78848026d 149 /////////////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 150
smorioka 0:d2c78848026d 151 typedef unsigned char (*kernel_func)(unsigned char *linebuf, int x, int y, int line_size, int kernel_size, void *param); // kernel operation function
smorioka 0:d2c78848026d 152
smorioka 0:d2c78848026d 153 typedef struct _stream_filter
smorioka 0:d2c78848026d 154 {
smorioka 0:d2c78848026d 155 // input pixel data
smorioka 0:d2c78848026d 156 unsigned char d_in0; // plane 0
smorioka 0:d2c78848026d 157 unsigned char d_in1; // plane 1
smorioka 0:d2c78848026d 158 unsigned char d_in2; // plane 2
smorioka 0:d2c78848026d 159 unsigned char d_in_attr; // data attribute (see the definitions below)
smorioka 0:d2c78848026d 160
smorioka 0:d2c78848026d 161 // output pixel data
smorioka 0:d2c78848026d 162 unsigned char d_out0; // plane 0
smorioka 0:d2c78848026d 163 unsigned char d_out1; // plane 1
smorioka 0:d2c78848026d 164 unsigned char d_out2; // plane 2
smorioka 0:d2c78848026d 165 unsigned char d_out_attr; // data attribute (see the definitions below)
smorioka 0:d2c78848026d 166
smorioka 0:d2c78848026d 167 // output data
smorioka 0:d2c78848026d 168 char is_3_planes; // 0: single plane, Others: 3 planes
smorioka 0:d2c78848026d 169
smorioka 0:d2c78848026d 170 ////////////////////////////////////////
smorioka 0:d2c78848026d 171 // internal variables (do not touch)
smorioka 0:d2c78848026d 172 ////////////////////////////////////////
smorioka 0:d2c78848026d 173 char state; // internal
smorioka 0:d2c78848026d 174
smorioka 0:d2c78848026d 175 int d_in_cnt; // # of total input
smorioka 0:d2c78848026d 176 int d_in_x; // x-coordinate of input position on "line_buf" (not on entire screen)
smorioka 0:d2c78848026d 177 int d_in_y; // y-coordinate
smorioka 0:d2c78848026d 178
smorioka 0:d2c78848026d 179 int d_out_cnt; // # of total output
smorioka 0:d2c78848026d 180 int d_out_x; // x-coordinate of center of kernel on "line_buf" (not on entire screen)
smorioka 0:d2c78848026d 181 int d_out_y; // y-coordinate
smorioka 0:d2c78848026d 182
smorioka 0:d2c78848026d 183 int line_size; // width
smorioka 0:d2c78848026d 184 int kernel_size; // kernel size (MUST be odd)
smorioka 0:d2c78848026d 185
smorioka 0:d2c78848026d 186 unsigned char *line_buf0; // line buffer for plane 0
smorioka 0:d2c78848026d 187 unsigned char *line_buf1; // plane 1
smorioka 0:d2c78848026d 188 unsigned char *line_buf2; // plane 2
smorioka 0:d2c78848026d 189
smorioka 0:d2c78848026d 190 kernel_func func; // pointer to a kernel operation function
smorioka 0:d2c78848026d 191 void *func_param; // parameter to the above function (ex. pointer to coefficient matrix etc.)
smorioka 0:d2c78848026d 192 } stream_filter;
smorioka 0:d2c78848026d 193
smorioka 0:d2c78848026d 194
smorioka 0:d2c78848026d 195 #define ATTR_1ST 0 // 1st pixel data
smorioka 0:d2c78848026d 196 #define ATTR_VALID 1 // valid pixel data (not 1st nor last)
smorioka 0:d2c78848026d 197 #define ATTR_LAST 2 // last pixel data
smorioka 0:d2c78848026d 198 #define ATTR_INVALID 3 // invalid pixel data
smorioka 0:d2c78848026d 199
smorioka 0:d2c78848026d 200 #define STATE_PROCESS 0 // do not touch
smorioka 0:d2c78848026d 201 #define STATE_FLUSH 1 // do not touch
smorioka 0:d2c78848026d 202
smorioka 0:d2c78848026d 203 #ifdef DEBUG
smorioka 0:d2c78848026d 204 FILE *fp_his;
smorioka 0:d2c78848026d 205 #endif // DEBUG
smorioka 0:d2c78848026d 206
smorioka 0:d2c78848026d 207
smorioka 0:d2c78848026d 208 ///////////////////--> kernel func
smorioka 0:d2c78848026d 209 unsigned char comb_filter(unsigned char *linebuf, int x, int y, int line_size, int kernel_size, void *matrix_void)
smorioka 0:d2c78848026d 210 {
smorioka 0:d2c78848026d 211 float acc = 0.0f;
smorioka 0:d2c78848026d 212 int mat_idx = 0;
smorioka 0:d2c78848026d 213
smorioka 0:d2c78848026d 214 ////////////////////////////////////////////// ---> customize here
smorioka 0:d2c78848026d 215 float *matrix = (float *)matrix_void;
smorioka 0:d2c78848026d 216 ////////////////////////////////////////////// <--- customize here
smorioka 0:d2c78848026d 217
smorioka 0:d2c78848026d 218 #ifdef DEBUG
smorioka 0:d2c78848026d 219 fprintf(fp_his, "\tpixel: ");
smorioka 0:d2c78848026d 220 #endif
smorioka 0:d2c78848026d 221
smorioka 0:d2c78848026d 222 // scan kernel
smorioka 0:d2c78848026d 223 for (int ky = y - ((kernel_size - 1) / 2); ky <= y + (kernel_size - 1) / 2; ky++) {
smorioka 0:d2c78848026d 224 int ly;
smorioka 0:d2c78848026d 225 // (rounding linebuf)
smorioka 0:d2c78848026d 226 if (ky < 0)
smorioka 0:d2c78848026d 227 ly = ky + (kernel_size + 1);
smorioka 0:d2c78848026d 228 else if (ky >= kernel_size + 1)
smorioka 0:d2c78848026d 229 ly = ky - (kernel_size + 1);
smorioka 0:d2c78848026d 230 else
smorioka 0:d2c78848026d 231 ly = ky;
smorioka 0:d2c78848026d 232
smorioka 0:d2c78848026d 233 for (int kx = x - ((kernel_size - 1) / 2); kx <= x + (kernel_size - 1) / 2; kx++, mat_idx++) {
smorioka 0:d2c78848026d 234 if (kx >= 0 && kx < line_size) {
smorioka 0:d2c78848026d 235 float pixel;
smorioka 0:d2c78848026d 236 pixel = (float)(*(linebuf + (ly * line_size) + kx));
smorioka 0:d2c78848026d 237
smorioka 0:d2c78848026d 238 ////////////////////////////////////////////// ---> customize here
smorioka 0:d2c78848026d 239 #ifdef DEBUG
smorioka 0:d2c78848026d 240 fprintf(fp_his, "%02x ", *(linebuf + (ly * line_size) + kx));
smorioka 0:d2c78848026d 241 #endif
smorioka 0:d2c78848026d 242 acc += pixel * (*(matrix + mat_idx));
smorioka 0:d2c78848026d 243 ////////////////////////////////////////////// <--- customize here
smorioka 0:d2c78848026d 244 }
smorioka 0:d2c78848026d 245 }
smorioka 0:d2c78848026d 246
smorioka 0:d2c78848026d 247 #ifdef DEBUG
smorioka 0:d2c78848026d 248 fprintf(fp_his, ", ");
smorioka 0:d2c78848026d 249 #endif
smorioka 0:d2c78848026d 250 }
smorioka 0:d2c78848026d 251
smorioka 0:d2c78848026d 252 #ifdef DEBUG
smorioka 0:d2c78848026d 253 fprintf(fp_his, "(ret %d)\n", (unsigned char)(acc + 0.5f));
smorioka 0:d2c78848026d 254 #endif
smorioka 0:d2c78848026d 255
smorioka 0:d2c78848026d 256 return ((unsigned char)(acc + 0.5f));
smorioka 0:d2c78848026d 257 }
smorioka 0:d2c78848026d 258 ///////////////////<-- kernel func
smorioka 0:d2c78848026d 259
smorioka 0:d2c78848026d 260
smorioka 0:d2c78848026d 261 void dump_stream_filter(FILE *fp, stream_filter *flt) // for debug; dump contents of the structure data
smorioka 0:d2c78848026d 262 {
smorioka 0:d2c78848026d 263 fprintf(fp, "d_in %02x ", flt->d_in0);
smorioka 0:d2c78848026d 264 if (flt->d_in_attr == ATTR_1ST) fprintf(fp, "ATTR_1ST");
smorioka 0:d2c78848026d 265 else if (flt->d_in_attr == ATTR_VALID) fprintf(fp, "ATTR_VALID");
smorioka 0:d2c78848026d 266 else if (flt->d_in_attr == ATTR_LAST) fprintf(fp, "ATTR_LAST");
smorioka 0:d2c78848026d 267 else if (flt->d_in_attr == ATTR_INVALID) fprintf(fp, "ATTR_INVALID");
smorioka 0:d2c78848026d 268 else fprintf(fp, "illegal attribute");
smorioka 0:d2c78848026d 269 fprintf(fp, "\tcnt %d, x %d, y %d\n", flt->d_in_cnt, flt->d_in_x, flt->d_in_y);
smorioka 0:d2c78848026d 270
smorioka 0:d2c78848026d 271 fprintf(fp, "d_out %02x ", flt->d_out0);
smorioka 0:d2c78848026d 272 if (flt->d_out_attr == ATTR_1ST) fprintf(fp, "ATTR_1ST");
smorioka 0:d2c78848026d 273 else if (flt->d_out_attr == ATTR_VALID) fprintf(fp, "ATTR_VALID");
smorioka 0:d2c78848026d 274 else if (flt->d_out_attr == ATTR_LAST) fprintf(fp, "ATTR_LAST");
smorioka 0:d2c78848026d 275 else if (flt->d_out_attr == ATTR_INVALID) fprintf(fp, "ATTR_INVALID");
smorioka 0:d2c78848026d 276 else fprintf(fp, "illegal attribute");
smorioka 0:d2c78848026d 277 fprintf(fp, "\tcnt %d, x %d, y %d\n", flt->d_out_cnt, flt->d_out_x, flt->d_out_y);
smorioka 0:d2c78848026d 278
smorioka 0:d2c78848026d 279 if (flt->state == STATE_PROCESS) fprintf(fp, "state: STATE_PROCESS\n");
smorioka 0:d2c78848026d 280 else fprintf(fp, "state: STATE_FLUSH\n");
smorioka 0:d2c78848026d 281
smorioka 0:d2c78848026d 282 fprintf(fp, "\n");
smorioka 0:d2c78848026d 283 for (int y = 0; y < flt->kernel_size + 1; y++) {
smorioka 0:d2c78848026d 284 for (int x = 0; x < flt->line_size; x++) {
smorioka 0:d2c78848026d 285 fprintf(fp, "%02x ", *(flt->line_buf0 + (y * flt->line_size) + x) );
smorioka 0:d2c78848026d 286 }
smorioka 0:d2c78848026d 287 fprintf(fp, "\n");
smorioka 0:d2c78848026d 288 }
smorioka 0:d2c78848026d 289
smorioka 0:d2c78848026d 290 if (flt->is_3_planes != 0) {
smorioka 0:d2c78848026d 291 fprintf(fp, "\n");
smorioka 0:d2c78848026d 292 for (int y = 0; y < flt->kernel_size + 1; y++) {
smorioka 0:d2c78848026d 293 for (int x = 0; x < flt->line_size; x++) {
smorioka 0:d2c78848026d 294 fprintf(fp, "%02x ", *(flt->line_buf1 + (y * flt->line_size) + x) );
smorioka 0:d2c78848026d 295 }
smorioka 0:d2c78848026d 296 fprintf(fp, "\n");
smorioka 0:d2c78848026d 297 }
smorioka 0:d2c78848026d 298
smorioka 0:d2c78848026d 299 fprintf(fp, "\n");
smorioka 0:d2c78848026d 300 for (int y = 0; y < flt->kernel_size + 1; y++) {
smorioka 0:d2c78848026d 301 for (int x = 0; x < flt->line_size; x++) {
smorioka 0:d2c78848026d 302 fprintf(fp, "%02x ", *(flt->line_buf1 + (y * flt->line_size) + x) );
smorioka 0:d2c78848026d 303 }
smorioka 0:d2c78848026d 304 fprintf(fp, "\n");
smorioka 0:d2c78848026d 305 }
smorioka 0:d2c78848026d 306 }
smorioka 0:d2c78848026d 307
smorioka 0:d2c78848026d 308 fprintf(fp, "====================================================\n");
smorioka 0:d2c78848026d 309 fflush(fp);
smorioka 0:d2c78848026d 310 }
smorioka 0:d2c78848026d 311
smorioka 0:d2c78848026d 312
smorioka 0:d2c78848026d 313 // reset data structure for reuse
smorioka 0:d2c78848026d 314 void reset_stream_filter(stream_filter *flt)
smorioka 0:d2c78848026d 315 {
smorioka 0:d2c78848026d 316 if (flt == (stream_filter *)NULL)
smorioka 0:d2c78848026d 317 return;
smorioka 0:d2c78848026d 318
smorioka 0:d2c78848026d 319 flt->state = STATE_PROCESS;
smorioka 0:d2c78848026d 320
smorioka 0:d2c78848026d 321 flt->d_in_cnt = 0; // total # of data
smorioka 0:d2c78848026d 322 flt->d_in_x = 0; // linebuf pointer
smorioka 0:d2c78848026d 323 flt->d_in_y = (flt->kernel_size - 1) / 2; // linebuf pointer
smorioka 0:d2c78848026d 324
smorioka 0:d2c78848026d 325 flt->d_out_cnt = 0; // total # of data
smorioka 0:d2c78848026d 326 flt->d_out_x = 0; // linebuf pointer
smorioka 0:d2c78848026d 327 flt->d_out_y = (flt->kernel_size - 1) / 2; // linebuf pointer
smorioka 0:d2c78848026d 328
smorioka 0:d2c78848026d 329 // zero clear buffer
smorioka 0:d2c78848026d 330 if (flt->line_buf0 != (unsigned char *)NULL) {
smorioka 0:d2c78848026d 331 for (int i = 0; i < flt->line_size * (flt->kernel_size + 1); i++) {
smorioka 0:d2c78848026d 332 *(flt->line_buf0 + i) = 0;
smorioka 0:d2c78848026d 333 }
smorioka 0:d2c78848026d 334 }
smorioka 0:d2c78848026d 335
smorioka 0:d2c78848026d 336 if (flt->line_buf1 != (unsigned char *)NULL) {
smorioka 0:d2c78848026d 337 for (int i = 0; i < flt->line_size * (flt->kernel_size + 1); i++) {
smorioka 0:d2c78848026d 338 *(flt->line_buf1 + i) = 0;
smorioka 0:d2c78848026d 339 }
smorioka 0:d2c78848026d 340 }
smorioka 0:d2c78848026d 341
smorioka 0:d2c78848026d 342 if (flt->line_buf2 != (unsigned char *)NULL) {
smorioka 0:d2c78848026d 343 for (int i = 0; i < flt->line_size * (flt->kernel_size + 1); i++) {
smorioka 0:d2c78848026d 344 *(flt->line_buf2 + i) = 0;
smorioka 0:d2c78848026d 345 }
smorioka 0:d2c78848026d 346 }
smorioka 0:d2c78848026d 347 }
smorioka 0:d2c78848026d 348
smorioka 0:d2c78848026d 349
smorioka 0:d2c78848026d 350 // allocate and initialize data structure
smorioka 0:d2c78848026d 351 stream_filter *create_stream_filter(char is_3_planes, int line_size, int kernel_size, kernel_func func, void *func_param)
smorioka 0:d2c78848026d 352 {
smorioka 0:d2c78848026d 353 stream_filter *ret;
smorioka 0:d2c78848026d 354
smorioka 0:d2c78848026d 355 // allocate memory
smorioka 0:d2c78848026d 356 if ((ret = (stream_filter *)malloc(sizeof (stream_filter))) == (stream_filter *)NULL)
smorioka 0:d2c78848026d 357 return ((stream_filter *)NULL);
smorioka 0:d2c78848026d 358
smorioka 0:d2c78848026d 359 if ((ret->line_buf0 = (unsigned char *)malloc(sizeof (unsigned char) * line_size * (kernel_size + 1))) == (unsigned char *)NULL) {
smorioka 0:d2c78848026d 360 free(ret);
smorioka 0:d2c78848026d 361 return ((stream_filter *)NULL);
smorioka 0:d2c78848026d 362 }
smorioka 0:d2c78848026d 363
smorioka 0:d2c78848026d 364 if (is_3_planes != 0) {
smorioka 0:d2c78848026d 365 if ((ret->line_buf1 = (unsigned char *)malloc(sizeof (unsigned char) * line_size * (kernel_size + 1))) == (unsigned char *)NULL) {
smorioka 0:d2c78848026d 366 free(ret->line_buf0);
smorioka 0:d2c78848026d 367 free(ret);
smorioka 0:d2c78848026d 368 return ((stream_filter *)NULL);
smorioka 0:d2c78848026d 369 }
smorioka 0:d2c78848026d 370
smorioka 0:d2c78848026d 371 if ((ret->line_buf2 = (unsigned char *)malloc(sizeof (unsigned char) * line_size * (kernel_size + 1))) == (unsigned char *)NULL) {
smorioka 0:d2c78848026d 372 free(ret->line_buf0);
smorioka 0:d2c78848026d 373 free(ret->line_buf1);
smorioka 0:d2c78848026d 374 free(ret);
smorioka 0:d2c78848026d 375 return ((stream_filter *)NULL);
smorioka 0:d2c78848026d 376 }
smorioka 0:d2c78848026d 377 }
smorioka 0:d2c78848026d 378 else {
smorioka 0:d2c78848026d 379 ret->line_buf1 = (unsigned char *)NULL;
smorioka 0:d2c78848026d 380 ret->line_buf2 = (unsigned char *)NULL;
smorioka 0:d2c78848026d 381 }
smorioka 0:d2c78848026d 382
smorioka 0:d2c78848026d 383 // init internal valiables
smorioka 0:d2c78848026d 384 ret->is_3_planes = is_3_planes;
smorioka 0:d2c78848026d 385 ret->line_size = line_size;
smorioka 0:d2c78848026d 386 ret->kernel_size = kernel_size;
smorioka 0:d2c78848026d 387
smorioka 0:d2c78848026d 388 reset_stream_filter(ret);
smorioka 0:d2c78848026d 389
smorioka 0:d2c78848026d 390 // init kernel operation func
smorioka 0:d2c78848026d 391 ret->func = func;
smorioka 0:d2c78848026d 392 ret->func_param = func_param;
smorioka 0:d2c78848026d 393
smorioka 0:d2c78848026d 394 return (ret);
smorioka 0:d2c78848026d 395 }
smorioka 0:d2c78848026d 396
smorioka 0:d2c78848026d 397
smorioka 0:d2c78848026d 398 // process 1 pixel; Set d_in & d_in_attr before calling. The result will be set to d_out & d_out_attr.
smorioka 0:d2c78848026d 399 void apply_filter(stream_filter *flt)
smorioka 0:d2c78848026d 400 {
smorioka 0:d2c78848026d 401 if (flt == (stream_filter *)NULL)
smorioka 0:d2c78848026d 402 return;
smorioka 0:d2c78848026d 403
smorioka 0:d2c78848026d 404 if ((flt->state == STATE_PROCESS && flt->d_in_attr != ATTR_INVALID)
smorioka 0:d2c78848026d 405 || (flt->state == STATE_FLUSH && flt->d_in_attr == ATTR_INVALID)) {
smorioka 0:d2c78848026d 406 /////////////////////////////////////////////
smorioka 0:d2c78848026d 407 // store d_in to linebuf
smorioka 0:d2c78848026d 408 /////////////////////////////////////////////
smorioka 0:d2c78848026d 409 if (flt->d_in_attr == ATTR_INVALID) {
smorioka 0:d2c78848026d 410 flt->d_in0 = 0;
smorioka 0:d2c78848026d 411 flt->d_in1 = 0;
smorioka 0:d2c78848026d 412 flt->d_in2 = 0;
smorioka 0:d2c78848026d 413 }
smorioka 0:d2c78848026d 414
smorioka 0:d2c78848026d 415 *(flt->line_buf0 + (flt->d_in_y * flt->line_size) + flt->d_in_x) = flt->d_in0;
smorioka 0:d2c78848026d 416 if (flt->is_3_planes != 0) {
smorioka 0:d2c78848026d 417 *(flt->line_buf1 + (flt->d_in_y * flt->line_size) + flt->d_in_x) = flt->d_in1;
smorioka 0:d2c78848026d 418 *(flt->line_buf2 + (flt->d_in_y * flt->line_size) + flt->d_in_x) = flt->d_in2;
smorioka 0:d2c78848026d 419 }
smorioka 0:d2c78848026d 420
smorioka 0:d2c78848026d 421 // update input counter
smorioka 0:d2c78848026d 422 if (flt->state == STATE_PROCESS && flt->d_in_attr != ATTR_LAST)
smorioka 0:d2c78848026d 423 (flt->d_in_cnt)++;
smorioka 0:d2c78848026d 424
smorioka 0:d2c78848026d 425 // update input pointer
smorioka 0:d2c78848026d 426 (flt->d_in_x)++;
smorioka 0:d2c78848026d 427 if (flt->d_in_x >= flt->line_size) {
smorioka 0:d2c78848026d 428 flt->d_in_x = 0;
smorioka 0:d2c78848026d 429 (flt->d_in_y)++;
smorioka 0:d2c78848026d 430 if (flt->d_in_y >= flt->kernel_size + 1) {
smorioka 0:d2c78848026d 431 flt->d_in_y = 0;
smorioka 0:d2c78848026d 432 }
smorioka 0:d2c78848026d 433 }
smorioka 0:d2c78848026d 434
smorioka 0:d2c78848026d 435 /////////////////////////////////////////////
smorioka 0:d2c78848026d 436 // calc output (NOTE: do after updating input counter)
smorioka 0:d2c78848026d 437 /////////////////////////////////////////////
smorioka 0:d2c78848026d 438 if ((flt->state == STATE_PROCESS && flt->d_in_cnt > flt->line_size * (flt->kernel_size + 1) / 2)
smorioka 0:d2c78848026d 439 || (flt->state == STATE_FLUSH && flt->d_in_cnt >= flt->d_out_cnt)) { // normal operation
smorioka 0:d2c78848026d 440
smorioka 0:d2c78848026d 441 // generate output (call kernel_operation func)
smorioka 0:d2c78848026d 442 flt->d_out0 = (flt->func)(flt->line_buf0, flt->d_out_x, flt->d_out_y, flt->line_size, flt->kernel_size, flt->func_param);
smorioka 0:d2c78848026d 443 if (flt->is_3_planes != 0) {
smorioka 0:d2c78848026d 444 flt->d_out1 = (flt->func)(flt->line_buf1, flt->d_out_x, flt->d_out_y, flt->line_size, flt->kernel_size, flt->func_param);
smorioka 0:d2c78848026d 445 flt->d_out2 = (flt->func)(flt->line_buf2, flt->d_out_x, flt->d_out_y, flt->line_size, flt->kernel_size, flt->func_param);
smorioka 0:d2c78848026d 446 }
smorioka 0:d2c78848026d 447
smorioka 0:d2c78848026d 448 if (flt->d_out_cnt == 0)
smorioka 0:d2c78848026d 449 flt->d_out_attr = ATTR_1ST;
smorioka 0:d2c78848026d 450 else if (flt->d_out_cnt == flt->d_in_cnt)
smorioka 0:d2c78848026d 451 flt->d_out_attr = ATTR_LAST;
smorioka 0:d2c78848026d 452 else
smorioka 0:d2c78848026d 453 flt->d_out_attr = ATTR_VALID;
smorioka 0:d2c78848026d 454
smorioka 0:d2c78848026d 455 // update output counter
smorioka 0:d2c78848026d 456 (flt->d_out_cnt)++;
smorioka 0:d2c78848026d 457
smorioka 0:d2c78848026d 458 // update output pointer
smorioka 0:d2c78848026d 459 (flt->d_out_x)++;
smorioka 0:d2c78848026d 460 if (flt->d_out_x >= flt->line_size) {
smorioka 0:d2c78848026d 461 flt->d_out_x = 0;
smorioka 0:d2c78848026d 462 (flt->d_out_y)++;
smorioka 0:d2c78848026d 463 if (flt->d_out_y >= flt->kernel_size + 1) {
smorioka 0:d2c78848026d 464 flt->d_out_y = 0;
smorioka 0:d2c78848026d 465 }
smorioka 0:d2c78848026d 466 }
smorioka 0:d2c78848026d 467 }
smorioka 0:d2c78848026d 468 else { // initial operation (still filling buffer) or completion of data output
smorioka 0:d2c78848026d 469 // no output
smorioka 0:d2c78848026d 470 flt->d_out0 = 0;
smorioka 0:d2c78848026d 471 flt->d_out1 = 0;
smorioka 0:d2c78848026d 472 flt->d_out2 = 0;
smorioka 0:d2c78848026d 473 flt->d_out_attr = ATTR_INVALID;
smorioka 0:d2c78848026d 474 }
smorioka 0:d2c78848026d 475
smorioka 0:d2c78848026d 476 /////////////////////////////////////////////
smorioka 0:d2c78848026d 477 // change state
smorioka 0:d2c78848026d 478 /////////////////////////////////////////////
smorioka 0:d2c78848026d 479 if (flt->state == STATE_PROCESS && flt->d_in_attr == ATTR_LAST) {
smorioka 0:d2c78848026d 480 flt->state = STATE_FLUSH;
smorioka 0:d2c78848026d 481 }
smorioka 0:d2c78848026d 482 }
smorioka 0:d2c78848026d 483 }
smorioka 0:d2c78848026d 484
smorioka 0:d2c78848026d 485
smorioka 0:d2c78848026d 486 // release data structure
smorioka 0:d2c78848026d 487 void release_stream_filter(stream_filter *flt)
smorioka 0:d2c78848026d 488 {
smorioka 0:d2c78848026d 489 if (flt == (stream_filter *)NULL)
smorioka 0:d2c78848026d 490 return;
smorioka 0:d2c78848026d 491 if (flt->line_buf0 != (unsigned char *)NULL)
smorioka 0:d2c78848026d 492 free(flt->line_buf0);
smorioka 0:d2c78848026d 493 free(flt);
smorioka 0:d2c78848026d 494 }
smorioka 0:d2c78848026d 495
smorioka 0:d2c78848026d 496
smorioka 0:d2c78848026d 497 /////////////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 498
smorioka 0:d2c78848026d 499
smorioka 0:d2c78848026d 500 //void cam_cap(void);
smorioka 0:d2c78848026d 501 int cam_cap(char, stream_filter *);
smorioka 0:d2c78848026d 502
smorioka 0:d2c78848026d 503
smorioka 0:d2c78848026d 504 int memfree(void)
smorioka 0:d2c78848026d 505 {
smorioka 0:d2c78848026d 506 int ret = 1;
smorioka 0:d2c78848026d 507 while (1) {
smorioka 0:d2c78848026d 508 char *p = (char *)malloc(ret);
smorioka 0:d2c78848026d 509 if (p == NULL)
smorioka 0:d2c78848026d 510 break;
smorioka 0:d2c78848026d 511 free(p);
smorioka 0:d2c78848026d 512 ret++;
smorioka 0:d2c78848026d 513 }
smorioka 0:d2c78848026d 514 return (ret - 1);
smorioka 0:d2c78848026d 515 }
smorioka 0:d2c78848026d 516
smorioka 0:d2c78848026d 517
smorioka 0:d2c78848026d 518 int main()
smorioka 0:d2c78848026d 519 {
smorioka 0:d2c78848026d 520 led1 = 0;
smorioka 0:d2c78848026d 521 led2 = 0;
smorioka 0:d2c78848026d 522 led3 = 0;
smorioka 0:d2c78848026d 523 led4 = 0;
smorioka 0:d2c78848026d 524
smorioka 0:d2c78848026d 525 sv0.period_us(20000); // 20ms
smorioka 0:d2c78848026d 526 sv0.pulsewidth_us(5000); // 5ms
smorioka 0:d2c78848026d 527
smorioka 0:d2c78848026d 528 sv1.period_us(20000); // 20ms
smorioka 0:d2c78848026d 529 sv1.pulsewidth_us(10000); // 10ms
smorioka 0:d2c78848026d 530
smorioka 0:d2c78848026d 531 sv2.period_us(20000); // 20ms
smorioka 0:d2c78848026d 532 sv2.pulsewidth_us(15000); // 15ms
smorioka 0:d2c78848026d 533
smorioka 0:d2c78848026d 534 ////////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 535 camera.WriteReg(0x12, 0x80); // com7; reset
smorioka 0:d2c78848026d 536 wait_ms(200);
smorioka 0:d2c78848026d 537
smorioka 0:d2c78848026d 538 camera.InitDefaultReg();
smorioka 0:d2c78848026d 539
smorioka 0:d2c78848026d 540 // negate vsync
smorioka 0:d2c78848026d 541 camera.WriteReg(0x15, 0x02); // com10; negative vsync
smorioka 0:d2c78848026d 542
smorioka 0:d2c78848026d 543 #ifdef QQVGA
smorioka 0:d2c78848026d 544 camera.InitQQVGA();
smorioka 0:d2c78848026d 545 #endif
smorioka 0:d2c78848026d 546 #ifdef QVGA
smorioka 0:d2c78848026d 547 camera.InitQVGA();
smorioka 0:d2c78848026d 548 #endif
smorioka 0:d2c78848026d 549 #ifdef VGA34
smorioka 0:d2c78848026d 550 camera.InitVGA_3_4();
smorioka 0:d2c78848026d 551 #endif
smorioka 0:d2c78848026d 552
smorioka 0:d2c78848026d 553 // data format
smorioka 0:d2c78848026d 554 camera.WriteReg(0x12, 0x04 + 0); // com7 RGB (bit1...test pattern)
smorioka 0:d2c78848026d 555 camera.WriteReg(0x40, 0xD0); // com15 RGB565
smorioka 0:d2c78848026d 556 camera.WriteReg(0x8c, 0x00); // RGB444
smorioka 0:d2c78848026d 557
smorioka 0:d2c78848026d 558 // turn off AWB
smorioka 0:d2c78848026d 559 unsigned char tmp = camera.ReadReg(0x13); // COM8
smorioka 0:d2c78848026d 560 camera.WriteReg(0x13, tmp & 0xFD); // COM8[1] = 0
smorioka 0:d2c78848026d 561 camera.WriteReg(0x02, 0x48); // R gain
smorioka 0:d2c78848026d 562 camera.WriteReg(0x6A, 0x40); // G gain
smorioka 0:d2c78848026d 563 camera.WriteReg(0x01, 0x60); // B gain
smorioka 0:d2c78848026d 564
smorioka 0:d2c78848026d 565 wait_ms(300);
smorioka 0:d2c78848026d 566
smorioka 0:d2c78848026d 567 // discard some first frames
smorioka 0:d2c78848026d 568 for (int i = 0; i < 10; i++) {
smorioka 0:d2c78848026d 569 camera.CaptureNext(); // sample start!
smorioka 0:d2c78848026d 570 while(camera.CaptureDone() == false)
smorioka 0:d2c78848026d 571 ;
smorioka 0:d2c78848026d 572 }
smorioka 0:d2c78848026d 573
smorioka 0:d2c78848026d 574 //////////////////////////////////////////////////
smorioka 0:d2c78848026d 575 int blur_kernel_size = 7;
smorioka 0:d2c78848026d 576
smorioka 0:d2c78848026d 577 float blur_matrix[] = {
smorioka 0:d2c78848026d 578 // Average filter
smorioka 0:d2c78848026d 579 // Change here in order to use a different filter
smorioka 0:d2c78848026d 580 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f,
smorioka 0:d2c78848026d 581 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f,
smorioka 0:d2c78848026d 582 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f,
smorioka 0:d2c78848026d 583 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f,
smorioka 0:d2c78848026d 584 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f,
smorioka 0:d2c78848026d 585 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f,
smorioka 0:d2c78848026d 586 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f
smorioka 0:d2c78848026d 587 };
smorioka 0:d2c78848026d 588
smorioka 0:d2c78848026d 589 int prev_mem = memfree();
smorioka 0:d2c78848026d 590
smorioka 0:d2c78848026d 591 stream_filter *average_flt;
smorioka 0:d2c78848026d 592 average_flt = create_stream_filter(0, SIZEX, blur_kernel_size, comb_filter, (void *)blur_matrix); // 0: 1 plane
smorioka 0:d2c78848026d 593
smorioka 0:d2c78848026d 594 // tmr.start(); // timer
smorioka 0:d2c78848026d 595 // cam_cap(0, average_flt);
smorioka 0:d2c78848026d 596 // tmr.stop(); // timer
smorioka 0:d2c78848026d 597
smorioka 0:d2c78848026d 598 int cur_mem = memfree();
smorioka 0:d2c78848026d 599
smorioka 0:d2c78848026d 600 // lcd.locate(0, 0);
smorioka 0:d2c78848026d 601 // lcd.printf("%dms", tmr.read_ms());
smorioka 0:d2c78848026d 602
smorioka 0:d2c78848026d 603 // lcd.locate(0, 1);
smorioka 0:d2c78848026d 604 // lcd.printf("m %d, %d", prev_mem, cur_mem);
smorioka 0:d2c78848026d 605
smorioka 0:d2c78848026d 606 //char shot_flag = 0;
smorioka 0:d2c78848026d 607
smorioka 0:d2c78848026d 608 //////////////////////////////////////////////////
smorioka 0:d2c78848026d 609 while (1) {
smorioka 0:d2c78848026d 610 // int rgain, ggain, bgain;
smorioka 0:d2c78848026d 611 int skin_pixel_num;
smorioka 0:d2c78848026d 612
smorioka 0:d2c78848026d 613 led1 = 1;
smorioka 0:d2c78848026d 614 reset_stream_filter(average_flt);
smorioka 0:d2c78848026d 615
smorioka 0:d2c78848026d 616 tmr.reset();
smorioka 0:d2c78848026d 617 tmr.start();
smorioka 0:d2c78848026d 618 skin_pixel_num = cam_cap(0, average_flt);
smorioka 0:d2c78848026d 619 tmr.stop();
smorioka 0:d2c78848026d 620
smorioka 0:d2c78848026d 621 if (skin_pixel_num >= SHUTTER_THRESHOLD) { // threshold
smorioka 0:d2c78848026d 622 cam_cap(1, average_flt); // shot
smorioka 0:d2c78848026d 623
smorioka 0:d2c78848026d 624 lcd.locate(0, 0);
smorioka 0:d2c78848026d 625 lcd.printf("CAPTURED ");
smorioka 0:d2c78848026d 626 lcd.locate(0, 1);
smorioka 0:d2c78848026d 627 lcd.printf("val %d ", skin_pixel_num);
smorioka 0:d2c78848026d 628
smorioka 0:d2c78848026d 629 while (1) {
smorioka 0:d2c78848026d 630 wait(1);
smorioka 0:d2c78848026d 631 }
smorioka 0:d2c78848026d 632 }
smorioka 0:d2c78848026d 633 // cur_mem = memfree();
smorioka 0:d2c78848026d 634
smorioka 0:d2c78848026d 635 // rgain = camera.ReadReg(0x02);
smorioka 0:d2c78848026d 636 // ggain = camera.ReadReg(0x6A);
smorioka 0:d2c78848026d 637 // bgain = camera.ReadReg(0x01);
smorioka 0:d2c78848026d 638
smorioka 0:d2c78848026d 639 lcd.locate(0, 0);
smorioka 0:d2c78848026d 640 lcd.printf("%dms ", tmr.read_ms());
smorioka 0:d2c78848026d 641 lcd.locate(0, 1);
smorioka 0:d2c78848026d 642 lcd.printf("val %d ", skin_pixel_num);
smorioka 0:d2c78848026d 643 // lcd.printf("%dms, M%d ", tmr.read_ms(), cur_mem);
smorioka 0:d2c78848026d 644 // lcd.printf("R%02x G%02x B%02x", rgain, ggain, bgain);
smorioka 0:d2c78848026d 645
smorioka 0:d2c78848026d 646 // wait_ms(100);
smorioka 0:d2c78848026d 647 }
smorioka 0:d2c78848026d 648
smorioka 0:d2c78848026d 649 }
smorioka 0:d2c78848026d 650
smorioka 0:d2c78848026d 651
smorioka 0:d2c78848026d 652 void write_bmp_header(FILE *fp_bmp)
smorioka 0:d2c78848026d 653 {
smorioka 0:d2c78848026d 654 /////////////////////////
smorioka 0:d2c78848026d 655 // file header
smorioka 0:d2c78848026d 656 /////////////////////////
smorioka 0:d2c78848026d 657 fprintf(fp_bmp, "BM");
smorioka 0:d2c78848026d 658 int val = 14 + 40 + SIZEX * SIZEY * 3; // file size
smorioka 0:d2c78848026d 659 fprintf(fp_bmp, "%c%c%c%c", val % 0x100, val / 0x100, val / 0x10000, val / 0x1000000);
smorioka 0:d2c78848026d 660 fprintf(fp_bmp, "%c%c%c%c%c%c%c%c", 0, 0, 0, 0, 0x36, 0, 0, 0);
smorioka 0:d2c78848026d 661
smorioka 0:d2c78848026d 662 /////////////////////////
smorioka 0:d2c78848026d 663 // information header
smorioka 0:d2c78848026d 664 /////////////////////////
smorioka 0:d2c78848026d 665 fprintf(fp_bmp, "%c%c%c%c", 0x28, 0, 0, 0); // header size
smorioka 0:d2c78848026d 666 fprintf(fp_bmp, "%c%c%c%c", SIZEX % 0x100, SIZEX / 0x100, SIZEX / 0x10000, SIZEX / 0x1000000);
smorioka 0:d2c78848026d 667 fprintf(fp_bmp, "%c%c%c%c", SIZEY % 0x100, SIZEY / 0x100, SIZEY / 0x10000, SIZEY / 0x1000000);
smorioka 0:d2c78848026d 668 fprintf(fp_bmp, "%c%c", 1, 0); // # of plane
smorioka 0:d2c78848026d 669 fprintf(fp_bmp, "%c%c", 24, 0); // bit count
smorioka 0:d2c78848026d 670 fprintf(fp_bmp, "%c%c%c%c", 0, 0, 0, 0); // compression
smorioka 0:d2c78848026d 671 val = SIZEX * SIZEY * 3; // data size
smorioka 0:d2c78848026d 672 fprintf(fp_bmp, "%c%c%c%c", val % 0x100, val / 0x100, val / 0x10000, val / 0x1000000);
smorioka 0:d2c78848026d 673 fprintf(fp_bmp, "%c%c%c%c", 0, 0, 0, 0);
smorioka 0:d2c78848026d 674 fprintf(fp_bmp, "%c%c%c%c", 0, 0, 0, 0);
smorioka 0:d2c78848026d 675 fprintf(fp_bmp, "%c%c%c%c", 0, 0, 0, 0);
smorioka 0:d2c78848026d 676 fprintf(fp_bmp, "%c%c%c%c", 0, 0, 0, 0);
smorioka 0:d2c78848026d 677 }
smorioka 0:d2c78848026d 678
smorioka 0:d2c78848026d 679
smorioka 0:d2c78848026d 680 void write_bmp_data(FILE *fp_bmp, unsigned char b, unsigned char g, unsigned char r)
smorioka 0:d2c78848026d 681 {
smorioka 0:d2c78848026d 682 fprintf(fp_bmp, "%c%c%c", b, g, r);
smorioka 0:d2c78848026d 683 }
smorioka 0:d2c78848026d 684
smorioka 0:d2c78848026d 685
smorioka 0:d2c78848026d 686 //void cam_cap(Arguments* input, Reply* output)
smorioka 0:d2c78848026d 687 int cam_cap(char file_flag, stream_filter *average_flt)
smorioka 0:d2c78848026d 688 {
smorioka 0:d2c78848026d 689 FILE *fp_bmp;
smorioka 0:d2c78848026d 690 unsigned int d1, d2;
smorioka 0:d2c78848026d 691 unsigned char sort[3];
smorioka 0:d2c78848026d 692
smorioka 0:d2c78848026d 693 int dx = 0;
smorioka 0:d2c78848026d 694 int dy = 0;
smorioka 0:d2c78848026d 695
smorioka 0:d2c78848026d 696 unsigned int skin_pixel_num = 0;
smorioka 0:d2c78848026d 697 unsigned int skin_x = 0;
smorioka 0:d2c78848026d 698 unsigned int skin_y = 0;
smorioka 0:d2c78848026d 699
smorioka 0:d2c78848026d 700
smorioka 0:d2c78848026d 701 ////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 702 led2 = 0;
smorioka 0:d2c78848026d 703 led3 = 0;
smorioka 0:d2c78848026d 704 led4 = 0;
smorioka 0:d2c78848026d 705
smorioka 0:d2c78848026d 706 if (file_flag != 0) {
smorioka 0:d2c78848026d 707 fp_bmp = fopen("/webfs/cam.bmp", "wb");
smorioka 0:d2c78848026d 708 write_bmp_header(fp_bmp);
smorioka 0:d2c78848026d 709 }
smorioka 0:d2c78848026d 710
smorioka 0:d2c78848026d 711 camera.CaptureNext(); // sample start!
smorioka 0:d2c78848026d 712
smorioka 0:d2c78848026d 713 while(camera.CaptureDone() == false)
smorioka 0:d2c78848026d 714 ;
smorioka 0:d2c78848026d 715
smorioka 0:d2c78848026d 716 camera.ReadStart(); // reset pointer
smorioka 0:d2c78848026d 717
smorioka 0:d2c78848026d 718 led2 = 1;
smorioka 0:d2c78848026d 719
smorioka 0:d2c78848026d 720 /////////////////////////////////////////////
smorioka 0:d2c78848026d 721 // main loop
smorioka 0:d2c78848026d 722 /////////////////////////////////////////////
smorioka 0:d2c78848026d 723 for (int y = 0; y < SIZEY; y++) {
smorioka 0:d2c78848026d 724 for (int x = 0; x < SIZEX; x++) {
smorioka 0:d2c78848026d 725 d1 = camera.ReadOneByte() ; // upper nibble is XXX , lower nibble is B
smorioka 0:d2c78848026d 726 d2 = camera.ReadOneByte() ; // upper nibble is G , lower nibble is R
smorioka 0:d2c78848026d 727
smorioka 0:d2c78848026d 728 /////////////////////////////////////////////
smorioka 0:d2c78848026d 729 // RGB565
smorioka 0:d2c78848026d 730 /////////////////////////////////////////////
smorioka 0:d2c78848026d 731 sort[0] = ((d1 & 0xF8) >> 3) << 3; // R
smorioka 0:d2c78848026d 732 sort[1] = ( ((d1 & 0x07) << 3) + ((d2 & 0xE0) >> 5) ) << 2; // G
smorioka 0:d2c78848026d 733 sort[2] = (d2 & 0x1F) << 3; // B
smorioka 0:d2c78848026d 734
smorioka 0:d2c78848026d 735 //////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 736 // 1. get input
smorioka 0:d2c78848026d 737 //////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 738 unsigned char b0 = sort[2];
smorioka 0:d2c78848026d 739 unsigned char g0 = sort[1];
smorioka 0:d2c78848026d 740 unsigned char r0 = sort[0];
smorioka 0:d2c78848026d 741
smorioka 0:d2c78848026d 742 if (file_flag != 0) {
smorioka 0:d2c78848026d 743 write_bmp_data(fp_bmp, sort[2], sort[1], sort[0]); // B,G,R
smorioka 0:d2c78848026d 744 }
smorioka 0:d2c78848026d 745 else {
smorioka 0:d2c78848026d 746 //////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 747 // 2. RGB2HSV
smorioka 0:d2c78848026d 748 //////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 749 unsigned char h1, s1, v1;
smorioka 0:d2c78848026d 750 rgb2hsv(r0, g0, b0, &h1, &s1, &v1);
smorioka 0:d2c78848026d 751
smorioka 0:d2c78848026d 752 //////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 753 // 3. judge
smorioka 0:d2c78848026d 754 //////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 755 unsigned char val2 = judge_skin_color(h1, s1, v1);
smorioka 0:d2c78848026d 756
smorioka 0:d2c78848026d 757 //////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 758 // 4. blur (average)
smorioka 0:d2c78848026d 759 //////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 760 average_flt->d_in0 = val2; // (1 plane mode; use d_in0 only)
smorioka 0:d2c78848026d 761
smorioka 0:d2c78848026d 762 if (y == 0 && x == 0)
smorioka 0:d2c78848026d 763 average_flt->d_in_attr = ATTR_1ST;
smorioka 0:d2c78848026d 764 else if (y == SIZEY - 1 && x == SIZEX - 1)
smorioka 0:d2c78848026d 765 average_flt->d_in_attr = ATTR_LAST;
smorioka 0:d2c78848026d 766 else
smorioka 0:d2c78848026d 767 average_flt->d_in_attr = ATTR_VALID;
smorioka 0:d2c78848026d 768
smorioka 0:d2c78848026d 769 // apply filter
smorioka 0:d2c78848026d 770 apply_filter(average_flt);
smorioka 0:d2c78848026d 771
smorioka 0:d2c78848026d 772 //////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 773 // 5. limit
smorioka 0:d2c78848026d 774 //////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 775 unsigned char val3 = average_flt->d_out0;
smorioka 0:d2c78848026d 776
smorioka 0:d2c78848026d 777 if (val3 < 220)
smorioka 0:d2c78848026d 778 val3 = 0;
smorioka 0:d2c78848026d 779 else {
smorioka 0:d2c78848026d 780 skin_pixel_num++;
smorioka 0:d2c78848026d 781 skin_x += dx;
smorioka 0:d2c78848026d 782 skin_y += dy;
smorioka 0:d2c78848026d 783 }
smorioka 0:d2c78848026d 784
smorioka 0:d2c78848026d 785 //////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 786 // 6. put output
smorioka 0:d2c78848026d 787 //////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 788 if (average_flt->d_out_attr != ATTR_INVALID) {
smorioka 0:d2c78848026d 789 sort[2] = val3 & b0;
smorioka 0:d2c78848026d 790 sort[1] = val3 & g0;
smorioka 0:d2c78848026d 791 sort[0] = val3 & r0;
smorioka 0:d2c78848026d 792
smorioka 0:d2c78848026d 793 dx++;
smorioka 0:d2c78848026d 794 if (dx == SIZEX) {
smorioka 0:d2c78848026d 795 dx = 0;
smorioka 0:d2c78848026d 796 dy++;
smorioka 0:d2c78848026d 797 }
smorioka 0:d2c78848026d 798 }
smorioka 0:d2c78848026d 799 } // file_flag
smorioka 0:d2c78848026d 800
smorioka 0:d2c78848026d 801 }
smorioka 0:d2c78848026d 802 }
smorioka 0:d2c78848026d 803
smorioka 0:d2c78848026d 804 led3 = 1;
smorioka 0:d2c78848026d 805
smorioka 0:d2c78848026d 806 if (file_flag != 0) {
smorioka 0:d2c78848026d 807 /////////////////////////////////////////////
smorioka 0:d2c78848026d 808 // flush filter
smorioka 0:d2c78848026d 809 /////////////////////////////////////////////
smorioka 0:d2c78848026d 810 average_flt->d_in_attr = ATTR_INVALID;
smorioka 0:d2c78848026d 811
smorioka 0:d2c78848026d 812 while (1) {
smorioka 0:d2c78848026d 813 apply_filter(average_flt);
smorioka 0:d2c78848026d 814
smorioka 0:d2c78848026d 815 if (average_flt->d_out_attr == ATTR_INVALID)
smorioka 0:d2c78848026d 816 break;
smorioka 0:d2c78848026d 817
smorioka 0:d2c78848026d 818 //////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 819 // 5. limit
smorioka 0:d2c78848026d 820 //////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 821 unsigned char val3 = average_flt->d_out0;
smorioka 0:d2c78848026d 822
smorioka 0:d2c78848026d 823 if (val3 < 220)
smorioka 0:d2c78848026d 824 val3 = 0;
smorioka 0:d2c78848026d 825 else {
smorioka 0:d2c78848026d 826 skin_pixel_num++;
smorioka 0:d2c78848026d 827 skin_x += dx;
smorioka 0:d2c78848026d 828 skin_y += dy;
smorioka 0:d2c78848026d 829 }
smorioka 0:d2c78848026d 830
smorioka 0:d2c78848026d 831 //////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 832 // 6. put output
smorioka 0:d2c78848026d 833 //////////////////////////////////////////////////////////////////////////
smorioka 0:d2c78848026d 834 sort[2] = val3;
smorioka 0:d2c78848026d 835 sort[1] = val3;
smorioka 0:d2c78848026d 836 sort[0] = val3;
smorioka 0:d2c78848026d 837
smorioka 0:d2c78848026d 838 dx++;
smorioka 0:d2c78848026d 839 if (dx == SIZEX) {
smorioka 0:d2c78848026d 840 dx = 0;
smorioka 0:d2c78848026d 841 dy++;
smorioka 0:d2c78848026d 842 }
smorioka 0:d2c78848026d 843 }
smorioka 0:d2c78848026d 844 }
smorioka 0:d2c78848026d 845
smorioka 0:d2c78848026d 846 led4 = 1;
smorioka 0:d2c78848026d 847
smorioka 0:d2c78848026d 848 /////////////////////////////////////////////
smorioka 0:d2c78848026d 849 // end
smorioka 0:d2c78848026d 850 /////////////////////////////////////////////
smorioka 0:d2c78848026d 851 camera.ReadStop();
smorioka 0:d2c78848026d 852
smorioka 0:d2c78848026d 853 // release_stream_filter(average_flt);
smorioka 0:d2c78848026d 854
smorioka 0:d2c78848026d 855 if (skin_pixel_num != 0) {
smorioka 0:d2c78848026d 856 skin_x /= skin_pixel_num;
smorioka 0:d2c78848026d 857 skin_y /= skin_pixel_num;
smorioka 0:d2c78848026d 858 }
smorioka 0:d2c78848026d 859 else {
smorioka 0:d2c78848026d 860 skin_x = SIZEX / 2;
smorioka 0:d2c78848026d 861 skin_y = SIZEY / 2;
smorioka 0:d2c78848026d 862 }
smorioka 0:d2c78848026d 863
smorioka 0:d2c78848026d 864 if (file_flag != 0) {
smorioka 0:d2c78848026d 865 fclose(fp_bmp);
smorioka 0:d2c78848026d 866 }
smorioka 0:d2c78848026d 867
smorioka 0:d2c78848026d 868 // led2 = 0;
smorioka 0:d2c78848026d 869 // led3 = 0;
smorioka 0:d2c78848026d 870 // led4 = 0;
smorioka 0:d2c78848026d 871
smorioka 0:d2c78848026d 872 return (skin_pixel_num);
smorioka 0:d2c78848026d 873 }
smorioka 0:d2c78848026d 874
smorioka 0:d2c78848026d 875 // end of file