Ryo Hagimoto / Mbed OS GR-PEACH_HVC-P2_sample_20180314

Mar. 14. 2018

Dependencies:   GraphicsFramework GR-PEACH_video LCD_shield_config AsciiFont R_BSP USBHost_custom

recognition_proc/recognition_proc.cpp@5:49a61433290a, 2017-09-05 (annotated)

Committer:
dkato
Date:
Tue Sep 05 10:01:51 2017 +0000
Revision:
5:49a61433290a
Parent:
3:0760680f06d8
Child:
6:8c0c70710090
Add HVC sensing result stabilizing library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dkato 0:f5de229c9a00 1 #include "mbed.h"
dkato 0:f5de229c9a00 2 #include "DisplayBace.h"
dkato 0:f5de229c9a00 3 #include "rtos.h"
dkato 0:f5de229c9a00 4 #include "AsciiFont.h"
dkato 0:f5de229c9a00 5 #include "USBHostSerial.h"
dkato 0:f5de229c9a00 6 #include "LCD_shield_config_4_3inch.h"
dkato 0:f5de229c9a00 7 #include "recognition_proc.h"
dkato 5:49a61433290a 8 #include "STBWrap.h"
dkato 0:f5de229c9a00 9
dkato 0:f5de229c9a00 10 #define UART_SETTING_TIMEOUT 1000 /* HVC setting command signal timeout period */
dkato 0:f5de229c9a00 11 #define UART_REGIST_EXECUTE_TIMEOUT 7000 /* HVC registration command signal timeout period */
dkato 0:f5de229c9a00 12 #define UART_EXECUTE_TIMEOUT 10000 /* HVC execute command signal timeout period */
dkato 0:f5de229c9a00 13
dkato 3:0760680f06d8 14 #define SENSOR_ROLL_ANGLE_DEFAULT 0 /* Camera angle setting */
dkato 0:f5de229c9a00 15 #define USER_ID_NUM_MAX 10
dkato 0:f5de229c9a00 16
dkato 1:11be006960b1 17 #define ERROR_02 "Error: Number of detected faces is 2 or more"
dkato 1:11be006960b1 18
dkato 5:49a61433290a 19 #define STB_RETRYCOUNT_DEFAULT 25 /* Retry Count for STB */
dkato 5:49a61433290a 20 #define STB_POSSTEADINESS_DEFAULT 30 /* Position Steadiness for STB */
dkato 5:49a61433290a 21 #define STB_SIZESTEADINESS_DEFAULT 30 /* Size Steadiness for STB */
dkato 5:49a61433290a 22 #define STB_PE_FRAME_DEFAULT 10 /* Complete Frame Count for property estimation in STB */
dkato 5:49a61433290a 23 #define STB_PE_ANGLEUDMIN_DEFAULT -15 /* Up/Down face angle minimum value for property estimation in STB */
dkato 5:49a61433290a 24 #define STB_PE_ANGLEUDMAX_DEFAULT 20 /* Up/Down face angle maximum value for property estimation in STB */
dkato 5:49a61433290a 25 #define STB_PE_ANGLELRMIN_DEFAULT -20 /* Left/Right face angle minimum value for property estimation in STB */
dkato 5:49a61433290a 26 #define STB_PE_ANGLELRMAX_DEFAULT 20 /* Left/Right face angle maximum value for property estimation in STB */
dkato 5:49a61433290a 27 #define STB_PE_THRESHOLD_DEFAULT 300 /* Threshold for property estimation in STB */
dkato 5:49a61433290a 28
dkato 0:f5de229c9a00 29 #define DISP_PIXEL_WIDTH (320)
dkato 0:f5de229c9a00 30 #define DISP_PIXEL_HEIGHT (240)
dkato 0:f5de229c9a00 31
dkato 0:f5de229c9a00 32 /*! Frame buffer stride: Frame buffer stride should be set to a multiple of 32 or 128
dkato 0:f5de229c9a00 33 in accordance with the frame buffer burst transfer mode. */
dkato 0:f5de229c9a00 34 /* FRAME BUFFER Parameter GRAPHICS_LAYER_0 */
dkato 0:f5de229c9a00 35 #define FRAME_BUFFER_BYTE_PER_PIXEL (2u)
dkato 0:f5de229c9a00 36 #define FRAME_BUFFER_STRIDE (((DISP_PIXEL_WIDTH * FRAME_BUFFER_BYTE_PER_PIXEL) + 31u) & ~31u)
dkato 0:f5de229c9a00 37
dkato 0:f5de229c9a00 38 /* RESULT BUFFER Parameter GRAPHICS_LAYER_1 */
dkato 0:f5de229c9a00 39 #define RESULT_BUFFER_BYTE_PER_PIXEL (2u)
dkato 0:f5de229c9a00 40 #define RESULT_BUFFER_STRIDE (((DISP_PIXEL_WIDTH * RESULT_BUFFER_BYTE_PER_PIXEL) + 31u) & ~31u)
dkato 0:f5de229c9a00 41
dkato 0:f5de229c9a00 42 static bool registrationr_req = false;
dkato 0:f5de229c9a00 43 static bool setting_req = false;
dkato 0:f5de229c9a00 44 static recognition_setting_t setting = {
dkato 0:f5de229c9a00 45 0,
dkato 0:f5de229c9a00 46 { BODY_THRESHOLD_DEFAULT, HAND_THRESHOLD_DEFAULT, FACE_THRESHOLD_DEFAULT, REC_THRESHOLD_DEFAULT},
dkato 0:f5de229c9a00 47 { BODY_SIZE_RANGE_MIN_DEFAULT, BODY_SIZE_RANGE_MAX_DEFAULT, HAND_SIZE_RANGE_MIN_DEFAULT,
dkato 0:f5de229c9a00 48 HAND_SIZE_RANGE_MAX_DEFAULT, FACE_SIZE_RANGE_MIN_DEFAULT, FACE_SIZE_RANGE_MAX_DEFAULT},
dkato 0:f5de229c9a00 49 FACE_POSE_DEFAULT,
dkato 0:f5de229c9a00 50 FACE_ANGLE_DEFAULT
dkato 0:f5de229c9a00 51 };
dkato 0:f5de229c9a00 52 static USBHostSerial serial;
dkato 0:f5de229c9a00 53 static InterruptIn button(USER_BUTTON0);
dkato 0:f5de229c9a00 54
dkato 0:f5de229c9a00 55 #if defined(__ICCARM__)
dkato 0:f5de229c9a00 56 /* 32 bytes aligned */
dkato 0:f5de229c9a00 57 #pragma data_alignment=32
dkato 0:f5de229c9a00 58 static uint8_t user_frame_buffer0[FRAME_BUFFER_STRIDE * DISP_PIXEL_HEIGHT]@ ".mirrorram";
dkato 0:f5de229c9a00 59 #pragma data_alignment=32
dkato 0:f5de229c9a00 60 static uint8_t user_frame_buffer_result[RESULT_BUFFER_STRIDE * DISP_PIXEL_HEIGHT]@ ".mirrorram";
dkato 0:f5de229c9a00 61 #else
dkato 0:f5de229c9a00 62 /* 32 bytes aligned */
dkato 0:f5de229c9a00 63 static uint8_t user_frame_buffer0[FRAME_BUFFER_STRIDE * DISP_PIXEL_HEIGHT]__attribute((section("NC_BSS"),aligned(32)));
dkato 0:f5de229c9a00 64 static uint8_t user_frame_buffer_result[RESULT_BUFFER_STRIDE * LCD_PIXEL_HEIGHT]__attribute((section("NC_BSS"),aligned(32)));
dkato 0:f5de229c9a00 65 #endif
dkato 0:f5de229c9a00 66
dkato 0:f5de229c9a00 67 static AsciiFont ascii_font(user_frame_buffer_result, DISP_PIXEL_WIDTH, LCD_PIXEL_HEIGHT,
dkato 0:f5de229c9a00 68 RESULT_BUFFER_STRIDE, RESULT_BUFFER_BYTE_PER_PIXEL, 0x00000090);
dkato 0:f5de229c9a00 69 static INT32 imageNo_setting = HVC_EXECUTE_IMAGE_QVGA_HALF;
dkato 0:f5de229c9a00 70 static int str_draw_x = 0;
dkato 0:f5de229c9a00 71 static int str_draw_y = 0;
dkato 0:f5de229c9a00 72
dkato 0:f5de229c9a00 73 /****** Image Recognition ******/
dkato 0:f5de229c9a00 74 extern "C" int UART_SendData(int inDataSize, UINT8 *inData) {
dkato 0:f5de229c9a00 75 return serial.writeBuf((char *)inData, inDataSize);
dkato 0:f5de229c9a00 76 }
dkato 0:f5de229c9a00 77
dkato 0:f5de229c9a00 78 extern "C" int UART_ReceiveData(int inTimeOutTime, int inDataSize, UINT8 *outResult) {
dkato 0:f5de229c9a00 79 return serial.readBuf((char *)outResult, inDataSize, inTimeOutTime);
dkato 0:f5de229c9a00 80 }
dkato 0:f5de229c9a00 81
dkato 0:f5de229c9a00 82 void SetRegistrationrReq(void) {
dkato 0:f5de229c9a00 83 registrationr_req = true;
dkato 0:f5de229c9a00 84 }
dkato 0:f5de229c9a00 85
dkato 0:f5de229c9a00 86 void SetSettingReq(void) {
dkato 0:f5de229c9a00 87 setting_req = true;
dkato 0:f5de229c9a00 88 }
dkato 0:f5de229c9a00 89
dkato 0:f5de229c9a00 90 recognition_setting_t * GetRecognitionSettingPointer(void) {
dkato 0:f5de229c9a00 91 return &setting;
dkato 0:f5de229c9a00 92 }
dkato 0:f5de229c9a00 93
dkato 0:f5de229c9a00 94 static void EraseImage(void) {
dkato 0:f5de229c9a00 95 uint32_t i = 0;
dkato 0:f5de229c9a00 96 while (i < sizeof(user_frame_buffer0)) {
dkato 0:f5de229c9a00 97 user_frame_buffer0[i++] = 0x10;
dkato 0:f5de229c9a00 98 user_frame_buffer0[i++] = 0x80;
dkato 0:f5de229c9a00 99 }
dkato 0:f5de229c9a00 100 }
dkato 0:f5de229c9a00 101
dkato 0:f5de229c9a00 102 static void DrawImage(int x, int y, int nWidth, int nHeight, UINT8 *unImageBuffer, int magnification) {
dkato 0:f5de229c9a00 103 int idx_base;
dkato 0:f5de229c9a00 104 int idx_w = 0;
dkato 0:f5de229c9a00 105 int wk_tmp = 0;
dkato 0:f5de229c9a00 106 int i;
dkato 0:f5de229c9a00 107 int j;
dkato 0:f5de229c9a00 108 int k;
dkato 0:f5de229c9a00 109 int idx_r = 0;
dkato 0:f5de229c9a00 110
dkato 0:f5de229c9a00 111 if (magnification <= 0) {
dkato 0:f5de229c9a00 112 return;
dkato 0:f5de229c9a00 113 }
dkato 0:f5de229c9a00 114
dkato 0:f5de229c9a00 115 idx_base = (x + (DISP_PIXEL_WIDTH * y)) * RESULT_BUFFER_BYTE_PER_PIXEL;
dkato 0:f5de229c9a00 116
dkato 0:f5de229c9a00 117 for (i = 0; i < nHeight; i++) {
dkato 0:f5de229c9a00 118 idx_w = idx_base + (DISP_PIXEL_WIDTH * RESULT_BUFFER_BYTE_PER_PIXEL * i) * magnification;
dkato 0:f5de229c9a00 119 wk_tmp = idx_w;
dkato 0:f5de229c9a00 120 for (j = 0; j < nWidth; j++) {
dkato 0:f5de229c9a00 121 for (k = 0; k < magnification; k++) {
dkato 0:f5de229c9a00 122 user_frame_buffer0[idx_w] = unImageBuffer[idx_r];
dkato 0:f5de229c9a00 123 idx_w += 2;
dkato 0:f5de229c9a00 124 }
dkato 0:f5de229c9a00 125 idx_r++;
dkato 0:f5de229c9a00 126 }
dkato 3:0760680f06d8 127 for (k = 1; k < magnification; k++) {
dkato 3:0760680f06d8 128 memcpy(&user_frame_buffer0[wk_tmp + (DISP_PIXEL_WIDTH * RESULT_BUFFER_BYTE_PER_PIXEL * k)], &user_frame_buffer0[wk_tmp], idx_w - wk_tmp);
dkato 0:f5de229c9a00 129 }
dkato 0:f5de229c9a00 130 }
dkato 0:f5de229c9a00 131 }
dkato 0:f5de229c9a00 132
dkato 0:f5de229c9a00 133 static void DrawSquare(int x, int y, int size, uint32_t const colour) {
dkato 0:f5de229c9a00 134 int wk_x;
dkato 0:f5de229c9a00 135 int wk_y;
dkato 0:f5de229c9a00 136 int wk_w = 0;
dkato 0:f5de229c9a00 137 int wk_h = 0;
dkato 0:f5de229c9a00 138 int idx_base;
dkato 0:f5de229c9a00 139 int wk_idx;
dkato 0:f5de229c9a00 140 int i;
dkato 0:f5de229c9a00 141 int j;
dkato 0:f5de229c9a00 142 uint8_t coller_pix[RESULT_BUFFER_BYTE_PER_PIXEL]; /* ARGB4444 */
dkato 0:f5de229c9a00 143 bool l_draw = true;
dkato 0:f5de229c9a00 144 bool r_draw = true;
dkato 0:f5de229c9a00 145 bool t_draw = true;
dkato 0:f5de229c9a00 146 bool b_draw = true;
dkato 0:f5de229c9a00 147
dkato 0:f5de229c9a00 148 if ((x - (size / 2)) < 0) {
dkato 0:f5de229c9a00 149 l_draw = false;
dkato 0:f5de229c9a00 150 wk_w += x;
dkato 0:f5de229c9a00 151 wk_x = 0;
dkato 0:f5de229c9a00 152 } else {
dkato 0:f5de229c9a00 153 wk_w += (size / 2);
dkato 0:f5de229c9a00 154 wk_x = x - (size / 2);
dkato 0:f5de229c9a00 155 }
dkato 0:f5de229c9a00 156
dkato 0:f5de229c9a00 157 if ((x + (size / 2)) >= 1600) {
dkato 0:f5de229c9a00 158 r_draw = false;
dkato 0:f5de229c9a00 159 wk_w += (1600 - x);
dkato 0:f5de229c9a00 160 } else {
dkato 0:f5de229c9a00 161 wk_w += (size / 2);
dkato 0:f5de229c9a00 162 }
dkato 0:f5de229c9a00 163
dkato 0:f5de229c9a00 164 if ((y - (size / 2)) < 0) {
dkato 0:f5de229c9a00 165 t_draw = false;
dkato 0:f5de229c9a00 166 wk_h += y;
dkato 0:f5de229c9a00 167 wk_y = 0;
dkato 0:f5de229c9a00 168 } else {
dkato 0:f5de229c9a00 169 wk_h += (size / 2);
dkato 0:f5de229c9a00 170 wk_y = y - (size / 2);
dkato 0:f5de229c9a00 171 }
dkato 0:f5de229c9a00 172
dkato 0:f5de229c9a00 173 if ((y + (size / 2)) >= 1200) {
dkato 0:f5de229c9a00 174 b_draw = false;
dkato 0:f5de229c9a00 175 wk_h += (1200 - y);
dkato 0:f5de229c9a00 176 } else {
dkato 0:f5de229c9a00 177 wk_h += (size / 2);
dkato 0:f5de229c9a00 178 }
dkato 0:f5de229c9a00 179
dkato 0:f5de229c9a00 180 wk_x = wk_x / 5;
dkato 0:f5de229c9a00 181 wk_y = wk_y / 5;
dkato 0:f5de229c9a00 182 wk_w = wk_w / 5;
dkato 0:f5de229c9a00 183 wk_h = wk_h / 5;
dkato 0:f5de229c9a00 184
dkato 0:f5de229c9a00 185 if ((colour == 0x0000f0f0) || (colour == 0x0000fff4)) {
dkato 0:f5de229c9a00 186 str_draw_x = wk_x;
dkato 0:f5de229c9a00 187 str_draw_y = wk_y + wk_h + 1;
dkato 0:f5de229c9a00 188 }
dkato 0:f5de229c9a00 189
dkato 0:f5de229c9a00 190 idx_base = (wk_x + (DISP_PIXEL_WIDTH * wk_y)) * RESULT_BUFFER_BYTE_PER_PIXEL;
dkato 0:f5de229c9a00 191
dkato 0:f5de229c9a00 192 /* Select color */
dkato 0:f5de229c9a00 193 coller_pix[0] = (colour >> 8) & 0xff; /* 4:Green 4:Blue */
dkato 0:f5de229c9a00 194 coller_pix[1] = colour & 0xff; /* 4:Alpha 4:Red */
dkato 0:f5de229c9a00 195
dkato 0:f5de229c9a00 196 /* top */
dkato 0:f5de229c9a00 197 if (t_draw) {
dkato 0:f5de229c9a00 198 wk_idx = idx_base;
dkato 0:f5de229c9a00 199 for (j = 0; j < wk_w; j++) {
dkato 0:f5de229c9a00 200 user_frame_buffer_result[wk_idx++] = coller_pix[0];
dkato 0:f5de229c9a00 201 user_frame_buffer_result[wk_idx++] = coller_pix[1];
dkato 0:f5de229c9a00 202 }
dkato 0:f5de229c9a00 203 }
dkato 0:f5de229c9a00 204
dkato 0:f5de229c9a00 205 /* middle */
dkato 0:f5de229c9a00 206 for (i = 1; i < (wk_h - 1); i++) {
dkato 0:f5de229c9a00 207 wk_idx = idx_base + (DISP_PIXEL_WIDTH * RESULT_BUFFER_BYTE_PER_PIXEL * i);
dkato 0:f5de229c9a00 208 if (l_draw) {
dkato 0:f5de229c9a00 209 user_frame_buffer_result[wk_idx + 0] = coller_pix[0];
dkato 0:f5de229c9a00 210 user_frame_buffer_result[wk_idx + 1] = coller_pix[1];
dkato 0:f5de229c9a00 211 }
dkato 0:f5de229c9a00 212 wk_idx += (wk_w - 1) * 2;
dkato 0:f5de229c9a00 213 if (r_draw) {
dkato 0:f5de229c9a00 214 user_frame_buffer_result[wk_idx + 0] = coller_pix[0];
dkato 0:f5de229c9a00 215 user_frame_buffer_result[wk_idx + 1] = coller_pix[1];
dkato 0:f5de229c9a00 216 }
dkato 0:f5de229c9a00 217 }
dkato 0:f5de229c9a00 218
dkato 0:f5de229c9a00 219 /* bottom */
dkato 0:f5de229c9a00 220 if (b_draw) {
dkato 0:f5de229c9a00 221 wk_idx = idx_base + (DISP_PIXEL_WIDTH * RESULT_BUFFER_BYTE_PER_PIXEL * (wk_h - 1));
dkato 0:f5de229c9a00 222 for (j = 0; j < wk_w; j++) {
dkato 0:f5de229c9a00 223 user_frame_buffer_result[wk_idx++] = coller_pix[0];
dkato 0:f5de229c9a00 224 user_frame_buffer_result[wk_idx++] = coller_pix[1];
dkato 0:f5de229c9a00 225 }
dkato 0:f5de229c9a00 226 }
dkato 0:f5de229c9a00 227 }
dkato 0:f5de229c9a00 228
dkato 0:f5de229c9a00 229 static void DrawString(const char * str, uint32_t const colour) {
dkato 0:f5de229c9a00 230 ascii_font.Erase(0x00000090, str_draw_x, str_draw_y,
dkato 0:f5de229c9a00 231 (AsciiFont::CHAR_PIX_WIDTH * strlen(str) + 2),
dkato 0:f5de229c9a00 232 (AsciiFont::CHAR_PIX_HEIGHT + 2));
dkato 0:f5de229c9a00 233 ascii_font.DrawStr(str, str_draw_x + 1, str_draw_y + 1, colour, 1);
dkato 0:f5de229c9a00 234 str_draw_y += AsciiFont::CHAR_PIX_HEIGHT + 1;
dkato 0:f5de229c9a00 235 }
dkato 0:f5de229c9a00 236
dkato 0:f5de229c9a00 237 static void button_fall(void) {
dkato 0:f5de229c9a00 238 if (imageNo_setting == HVC_EXECUTE_IMAGE_NONE) {
dkato 0:f5de229c9a00 239 imageNo_setting = HVC_EXECUTE_IMAGE_QVGA_HALF;
dkato 0:f5de229c9a00 240 } else if (imageNo_setting == HVC_EXECUTE_IMAGE_QVGA_HALF) {
dkato 0:f5de229c9a00 241 imageNo_setting = HVC_EXECUTE_IMAGE_QVGA;
dkato 0:f5de229c9a00 242 } else {
dkato 0:f5de229c9a00 243 imageNo_setting = HVC_EXECUTE_IMAGE_NONE;
dkato 0:f5de229c9a00 244 }
dkato 0:f5de229c9a00 245 }
dkato 0:f5de229c9a00 246
dkato 0:f5de229c9a00 247 void init_recognition_layers(DisplayBase * p_display) {
dkato 0:f5de229c9a00 248 DisplayBase::rect_t rect;
dkato 0:f5de229c9a00 249
dkato 0:f5de229c9a00 250 /* The layer by which the image is drawn */
dkato 0:f5de229c9a00 251 rect.vs = 0;
dkato 0:f5de229c9a00 252 rect.vw = DISP_PIXEL_HEIGHT;
dkato 0:f5de229c9a00 253 rect.hs = 0;
dkato 0:f5de229c9a00 254 rect.hw = DISP_PIXEL_WIDTH;
dkato 0:f5de229c9a00 255 p_display->Graphics_Read_Setting(
dkato 0:f5de229c9a00 256 DisplayBase::GRAPHICS_LAYER_0,
dkato 0:f5de229c9a00 257 (void *)user_frame_buffer0,
dkato 0:f5de229c9a00 258 FRAME_BUFFER_STRIDE,
dkato 0:f5de229c9a00 259 DisplayBase::GRAPHICS_FORMAT_YCBCR422,
dkato 0:f5de229c9a00 260 DisplayBase::WR_RD_WRSWA_32_16BIT,
dkato 0:f5de229c9a00 261 &rect
dkato 0:f5de229c9a00 262 );
dkato 0:f5de229c9a00 263 p_display->Graphics_Start(DisplayBase::GRAPHICS_LAYER_0);
dkato 0:f5de229c9a00 264
dkato 0:f5de229c9a00 265 /* The layer by which the image recognition is drawn */
dkato 0:f5de229c9a00 266 rect.vs = 0;
dkato 0:f5de229c9a00 267 rect.vw = LCD_PIXEL_HEIGHT;
dkato 0:f5de229c9a00 268 rect.hs = 0;
dkato 0:f5de229c9a00 269 rect.hw = DISP_PIXEL_WIDTH;
dkato 0:f5de229c9a00 270 p_display->Graphics_Read_Setting(
dkato 0:f5de229c9a00 271 DisplayBase::GRAPHICS_LAYER_1,
dkato 0:f5de229c9a00 272 (void *)user_frame_buffer_result,
dkato 0:f5de229c9a00 273 RESULT_BUFFER_STRIDE,
dkato 0:f5de229c9a00 274 DisplayBase::GRAPHICS_FORMAT_ARGB4444,
dkato 0:f5de229c9a00 275 DisplayBase::WR_RD_WRSWA_32_16BIT,
dkato 0:f5de229c9a00 276 &rect
dkato 0:f5de229c9a00 277 );
dkato 0:f5de229c9a00 278 p_display->Graphics_Start(DisplayBase::GRAPHICS_LAYER_1);
dkato 0:f5de229c9a00 279 }
dkato 0:f5de229c9a00 280
dkato 0:f5de229c9a00 281 void recognition_task(DisplayBase * p_display) {
dkato 0:f5de229c9a00 282 INT32 ret = 0;
dkato 0:f5de229c9a00 283 UINT8 status;
dkato 0:f5de229c9a00 284 HVC_VERSION version;
dkato 0:f5de229c9a00 285 HVC_RESULT *pHVCResult = NULL;
dkato 5:49a61433290a 286
dkato 5:49a61433290a 287 int nSTBFaceCount;
dkato 5:49a61433290a 288 STB_FACE *pSTBFaceResult;
dkato 5:49a61433290a 289 int nSTBBodyCount;
dkato 5:49a61433290a 290 STB_BODY *pSTBBodyResult;
dkato 5:49a61433290a 291 int nIndex;
dkato 5:49a61433290a 292
dkato 0:f5de229c9a00 293 HVC_IMAGE *pImage = NULL;
dkato 0:f5de229c9a00 294 INT32 execFlag;
dkato 0:f5de229c9a00 295 INT32 imageNo;
dkato 0:f5de229c9a00 296 INT32 userID;
dkato 0:f5de229c9a00 297 INT32 next_userID;
dkato 0:f5de229c9a00 298 INT32 dataID;
dkato 0:f5de229c9a00 299 const char *pExStr[] = {"?", "Neutral", "Happiness", "Surprise", "Anger", "Sadness"};
dkato 0:f5de229c9a00 300 uint32_t i;
dkato 3:0760680f06d8 301 char Str_disp[32];
dkato 0:f5de229c9a00 302 Timer resp_time;
dkato 5:49a61433290a 303 INT32 TrackingID;
dkato 0:f5de229c9a00 304
dkato 0:f5de229c9a00 305 /* Register the button */
dkato 0:f5de229c9a00 306 button.fall(&button_fall);
dkato 0:f5de229c9a00 307
dkato 0:f5de229c9a00 308 /* Initializing Recognition layers */
dkato 0:f5de229c9a00 309 EraseImage();
dkato 0:f5de229c9a00 310 memset(user_frame_buffer_result, 0, sizeof(user_frame_buffer_result));
dkato 0:f5de229c9a00 311 init_recognition_layers(p_display);
dkato 0:f5de229c9a00 312
dkato 0:f5de229c9a00 313 /* Result Structure Allocation */
dkato 0:f5de229c9a00 314 pHVCResult = (HVC_RESULT *)malloc(sizeof(HVC_RESULT));
dkato 0:f5de229c9a00 315 if (pHVCResult == NULL) {
dkato 0:f5de229c9a00 316 printf("Memory Allocation Error : %08x\n", sizeof(HVC_RESULT));
dkato 0:f5de229c9a00 317 mbed_die();
dkato 0:f5de229c9a00 318 }
dkato 0:f5de229c9a00 319
dkato 5:49a61433290a 320 /* STB Initialize */
dkato 5:49a61433290a 321 ret = STB_Init(STB_FUNC_BD | STB_FUNC_DT | STB_FUNC_PT | STB_FUNC_AG | STB_FUNC_GN);
dkato 5:49a61433290a 322 if (ret != 0) {
dkato 5:49a61433290a 323 printf("STB_Init Error : %d\n", ret);
dkato 5:49a61433290a 324 mbed_die();
dkato 5:49a61433290a 325 }
dkato 5:49a61433290a 326
dkato 0:f5de229c9a00 327 /* Image Structure allocation */
dkato 0:f5de229c9a00 328 pImage = (HVC_IMAGE *)malloc(sizeof(HVC_IMAGE));
dkato 0:f5de229c9a00 329 if (pImage == NULL) {
dkato 0:f5de229c9a00 330 printf("Memory Allocation Error : %08x\n", sizeof(HVC_RESULT));
dkato 0:f5de229c9a00 331 mbed_die();
dkato 0:f5de229c9a00 332 }
dkato 0:f5de229c9a00 333
dkato 0:f5de229c9a00 334 while (1) {
dkato 0:f5de229c9a00 335 /* try to connect a serial device */
dkato 0:f5de229c9a00 336 while (!serial.connect()) {
dkato 0:f5de229c9a00 337 Thread::wait(500);
dkato 0:f5de229c9a00 338 }
dkato 0:f5de229c9a00 339 serial.baud(921600);
dkato 0:f5de229c9a00 340 setting_req = true;
dkato 0:f5de229c9a00 341
dkato 0:f5de229c9a00 342 do {
dkato 0:f5de229c9a00 343 /* Initializing variables */
dkato 0:f5de229c9a00 344 next_userID = 0;
dkato 0:f5de229c9a00 345 dataID = 0;
dkato 0:f5de229c9a00 346
dkato 0:f5de229c9a00 347 /* Get Model and Version */
dkato 0:f5de229c9a00 348 ret = HVC_GetVersion(UART_SETTING_TIMEOUT, &version, &status);
dkato 0:f5de229c9a00 349 if ((ret != 0) || (status != 0)) {
dkato 0:f5de229c9a00 350 break;
dkato 0:f5de229c9a00 351 }
dkato 0:f5de229c9a00 352
dkato 0:f5de229c9a00 353 while (1) {
dkato 0:f5de229c9a00 354 if (!serial.connected()) {
dkato 0:f5de229c9a00 355 break;
dkato 0:f5de229c9a00 356 }
dkato 0:f5de229c9a00 357
dkato 0:f5de229c9a00 358 /* Execute Setting */
dkato 0:f5de229c9a00 359 if (setting_req) {
dkato 0:f5de229c9a00 360 setting_req = false;
dkato 0:f5de229c9a00 361 /* Set Camera Angle */
dkato 0:f5de229c9a00 362 ret = HVC_SetCameraAngle(UART_SETTING_TIMEOUT, SENSOR_ROLL_ANGLE_DEFAULT, &status);
dkato 0:f5de229c9a00 363 if ((ret != 0) || (status != 0)) {
dkato 0:f5de229c9a00 364 break;
dkato 0:f5de229c9a00 365 }
dkato 0:f5de229c9a00 366 /* Set Threshold Values */
dkato 0:f5de229c9a00 367 ret = HVC_SetThreshold(UART_SETTING_TIMEOUT, &setting.threshold, &status);
dkato 0:f5de229c9a00 368 if ((ret != 0) || (status != 0)) {
dkato 0:f5de229c9a00 369 break;
dkato 0:f5de229c9a00 370 }
dkato 0:f5de229c9a00 371 ret = HVC_GetThreshold(UART_SETTING_TIMEOUT, &setting.threshold, &status);
dkato 0:f5de229c9a00 372 if ((ret != 0) || (status != 0)) {
dkato 0:f5de229c9a00 373 break;
dkato 0:f5de229c9a00 374 }
dkato 0:f5de229c9a00 375 /* Set Detection Size */
dkato 0:f5de229c9a00 376 ret = HVC_SetSizeRange(UART_SETTING_TIMEOUT, &setting.sizeRange, &status);
dkato 0:f5de229c9a00 377 if ((ret != 0) || (status != 0)) {
dkato 0:f5de229c9a00 378 break;
dkato 0:f5de229c9a00 379 }
dkato 0:f5de229c9a00 380 ret = HVC_GetSizeRange(UART_SETTING_TIMEOUT, &setting.sizeRange, &status);
dkato 0:f5de229c9a00 381 if ((ret != 0) || (status != 0)) {
dkato 0:f5de229c9a00 382 break;
dkato 0:f5de229c9a00 383 }
dkato 0:f5de229c9a00 384 /* Set Face Angle */
dkato 0:f5de229c9a00 385 ret = HVC_SetFaceDetectionAngle(UART_SETTING_TIMEOUT, setting.pose, setting.angle, &status);
dkato 0:f5de229c9a00 386 if ((ret != 0) || (status != 0)) {
dkato 0:f5de229c9a00 387 break;
dkato 0:f5de229c9a00 388 }
dkato 0:f5de229c9a00 389 ret = HVC_GetFaceDetectionAngle(UART_SETTING_TIMEOUT, &setting.pose, &setting.angle, &status);
dkato 0:f5de229c9a00 390 if ((ret != 0) || (status != 0)) {
dkato 0:f5de229c9a00 391 break;
dkato 0:f5de229c9a00 392 }
dkato 5:49a61433290a 393
dkato 5:49a61433290a 394 /* Set STB Parameters */
dkato 5:49a61433290a 395 ret = STB_SetTrParam(STB_RETRYCOUNT_DEFAULT, STB_POSSTEADINESS_DEFAULT, STB_SIZESTEADINESS_DEFAULT);
dkato 5:49a61433290a 396 if (ret != 0) {
dkato 5:49a61433290a 397 break;
dkato 5:49a61433290a 398 }
dkato 5:49a61433290a 399 ret = STB_SetPeParam(STB_PE_THRESHOLD_DEFAULT, STB_PE_ANGLEUDMIN_DEFAULT, STB_PE_ANGLEUDMAX_DEFAULT,
dkato 5:49a61433290a 400 STB_PE_ANGLELRMIN_DEFAULT, STB_PE_ANGLELRMAX_DEFAULT, STB_PE_FRAME_DEFAULT);
dkato 5:49a61433290a 401 if (ret != 0) {
dkato 5:49a61433290a 402 break;
dkato 5:49a61433290a 403 }
dkato 0:f5de229c9a00 404 }
dkato 0:f5de229c9a00 405
dkato 0:f5de229c9a00 406 /* Execute Registration */
dkato 0:f5de229c9a00 407 if (registrationr_req) {
dkato 1:11be006960b1 408 int wk_width;
dkato 1:11be006960b1 409
dkato 0:f5de229c9a00 410 if ((pHVCResult->fdResult.num == 1) && (pHVCResult->fdResult.fcResult[0].recognitionResult.uid >= 0)) {
dkato 0:f5de229c9a00 411 userID = pHVCResult->fdResult.fcResult[0].recognitionResult.uid;
dkato 0:f5de229c9a00 412 } else {
dkato 0:f5de229c9a00 413 userID = next_userID;
dkato 0:f5de229c9a00 414 }
dkato 0:f5de229c9a00 415 ret = HVC_Registration(UART_REGIST_EXECUTE_TIMEOUT, userID, dataID, pImage, &status);
dkato 0:f5de229c9a00 416 if ((ret == 0) && (status == 0)) {
dkato 0:f5de229c9a00 417 if (userID == next_userID) {
dkato 0:f5de229c9a00 418 next_userID++;
dkato 0:f5de229c9a00 419 if (next_userID >= USER_ID_NUM_MAX) {
dkato 0:f5de229c9a00 420 next_userID = 0;
dkato 0:f5de229c9a00 421 }
dkato 0:f5de229c9a00 422 }
dkato 0:f5de229c9a00 423 memset(user_frame_buffer_result, 0, sizeof(user_frame_buffer_result));
dkato 0:f5de229c9a00 424 DrawImage(128, 88, pImage->width, pImage->height, pImage->image, 1);
dkato 0:f5de229c9a00 425 memset(Str_disp, 0, sizeof(Str_disp));
dkato 0:f5de229c9a00 426 sprintf(Str_disp, "USER%03d", userID + 1);
dkato 1:11be006960b1 427 wk_width = (AsciiFont::CHAR_PIX_WIDTH * strlen(Str_disp)) + 2;
dkato 0:f5de229c9a00 428 ascii_font.Erase(0x00000090, (DISP_PIXEL_WIDTH - wk_width) / 2, 153, wk_width, (AsciiFont::CHAR_PIX_HEIGHT + 2));
dkato 1:11be006960b1 429 ascii_font.DrawStr(Str_disp, (DISP_PIXEL_WIDTH - wk_width) / 2 + 1, 154, 0x0000ffff, 1);
dkato 0:f5de229c9a00 430 Thread::wait(1200);
dkato 1:11be006960b1 431 } else {
dkato 1:11be006960b1 432 if (status == 0x02) {
dkato 1:11be006960b1 433 wk_width = (AsciiFont::CHAR_PIX_WIDTH * (sizeof(ERROR_02) - 1)) + 4;
dkato 1:11be006960b1 434 ascii_font.Erase(0x00000090, (DISP_PIXEL_WIDTH - wk_width) / 2, 120, wk_width, (AsciiFont::CHAR_PIX_HEIGHT + 3));
dkato 1:11be006960b1 435 ascii_font.DrawStr(ERROR_02, (DISP_PIXEL_WIDTH - wk_width) / 2 + 2, 121, 0x0000ffff, 1);
dkato 1:11be006960b1 436 Thread::wait(1500);
dkato 1:11be006960b1 437 }
dkato 0:f5de229c9a00 438 }
dkato 0:f5de229c9a00 439 registrationr_req = false;
dkato 0:f5de229c9a00 440 }
dkato 0:f5de229c9a00 441
dkato 0:f5de229c9a00 442 /* Execute Detection */
dkato 0:f5de229c9a00 443 execFlag = setting.execFlag;
dkato 0:f5de229c9a00 444 if ((execFlag & HVC_ACTIV_FACE_DETECTION) == 0) {
dkato 0:f5de229c9a00 445 execFlag &= ~(HVC_ACTIV_AGE_ESTIMATION | HVC_ACTIV_GENDER_ESTIMATION | HVC_ACTIV_EXPRESSION_ESTIMATION);
dkato 0:f5de229c9a00 446 }
dkato 5:49a61433290a 447 if (execFlag != 0) { // for STB
dkato 5:49a61433290a 448 execFlag |= HVC_ACTIV_FACE_DIRECTION;
dkato 5:49a61433290a 449 }
dkato 0:f5de229c9a00 450 imageNo = imageNo_setting;
dkato 0:f5de229c9a00 451 resp_time.reset();
dkato 0:f5de229c9a00 452 resp_time.start();
dkato 0:f5de229c9a00 453 ret = HVC_ExecuteEx(UART_EXECUTE_TIMEOUT, execFlag, imageNo, pHVCResult, &status);
dkato 0:f5de229c9a00 454 resp_time.stop();
dkato 5:49a61433290a 455
dkato 5:49a61433290a 456 /* STB */
dkato 5:49a61433290a 457 if (STB_Exec(pHVCResult->executedFunc, pHVCResult, &nSTBFaceCount, &pSTBFaceResult, &nSTBBodyCount, &pSTBBodyResult) == 0) {
dkato 5:49a61433290a 458 for ( i = 0; i < nSTBBodyCount; i++ )
dkato 5:49a61433290a 459 {
dkato 5:49a61433290a 460 if ( pHVCResult->bdResult.num <= i ) break;
dkato 5:49a61433290a 461
dkato 5:49a61433290a 462 nIndex = pSTBBodyResult[i].nDetectID;
dkato 5:49a61433290a 463 pHVCResult->bdResult.bdResult[nIndex].posX = (short)pSTBBodyResult[i].center.x;
dkato 5:49a61433290a 464 pHVCResult->bdResult.bdResult[nIndex].posY = (short)pSTBBodyResult[i].center.y;
dkato 5:49a61433290a 465 pHVCResult->bdResult.bdResult[nIndex].size = pSTBBodyResult[i].nSize;
dkato 5:49a61433290a 466 }
dkato 5:49a61433290a 467 for (i = 0; i < nSTBFaceCount; i++)
dkato 5:49a61433290a 468 {
dkato 5:49a61433290a 469 if (pHVCResult->fdResult.num <= i) break;
dkato 5:49a61433290a 470
dkato 5:49a61433290a 471 nIndex = pSTBFaceResult[i].nDetectID;
dkato 5:49a61433290a 472 pHVCResult->fdResult.fcResult[nIndex].dtResult.posX = (short)pSTBFaceResult[i].center.x;
dkato 5:49a61433290a 473 pHVCResult->fdResult.fcResult[nIndex].dtResult.posY = (short)pSTBFaceResult[i].center.y;
dkato 5:49a61433290a 474 pHVCResult->fdResult.fcResult[nIndex].dtResult.size = pSTBFaceResult[i].nSize;
dkato 5:49a61433290a 475
dkato 5:49a61433290a 476 if (pHVCResult->executedFunc & HVC_ACTIV_AGE_ESTIMATION) {
dkato 5:49a61433290a 477 pHVCResult->fdResult.fcResult[nIndex].ageResult.confidence += 10000; // During
dkato 5:49a61433290a 478 if (pSTBFaceResult[i].age.status >= STB_STATUS_COMPLETE) {
dkato 5:49a61433290a 479 pHVCResult->fdResult.fcResult[nIndex].ageResult.age = pSTBFaceResult[i].age.value;
dkato 5:49a61433290a 480 pHVCResult->fdResult.fcResult[nIndex].ageResult.confidence += 10000; // Complete
dkato 5:49a61433290a 481 }
dkato 5:49a61433290a 482 }
dkato 5:49a61433290a 483 if (pHVCResult->executedFunc & HVC_ACTIV_GENDER_ESTIMATION) {
dkato 5:49a61433290a 484 pHVCResult->fdResult.fcResult[nIndex].genderResult.confidence += 10000; // During
dkato 5:49a61433290a 485 if (pSTBFaceResult[i].gender.status >= STB_STATUS_COMPLETE) {
dkato 5:49a61433290a 486 pHVCResult->fdResult.fcResult[nIndex].genderResult.gender = pSTBFaceResult[i].gender.value;
dkato 5:49a61433290a 487 pHVCResult->fdResult.fcResult[nIndex].genderResult.confidence += 10000; // Complete
dkato 5:49a61433290a 488 }
dkato 5:49a61433290a 489 }
dkato 5:49a61433290a 490 }
dkato 5:49a61433290a 491 }
dkato 5:49a61433290a 492
dkato 0:f5de229c9a00 493 if ((ret == 0) && (status == 0)) {
dkato 0:f5de229c9a00 494 if (imageNo == HVC_EXECUTE_IMAGE_QVGA_HALF) {
dkato 0:f5de229c9a00 495 DrawImage(0, 0, pHVCResult->image.width, pHVCResult->image.height, pHVCResult->image.image, 2);
dkato 0:f5de229c9a00 496 } else if (imageNo == HVC_EXECUTE_IMAGE_QVGA) {
dkato 0:f5de229c9a00 497 DrawImage(0, 0, pHVCResult->image.width, pHVCResult->image.height, pHVCResult->image.image, 1);
dkato 0:f5de229c9a00 498 } else {
dkato 0:f5de229c9a00 499 EraseImage();
dkato 0:f5de229c9a00 500 }
dkato 0:f5de229c9a00 501 memset(user_frame_buffer_result, 0, sizeof(user_frame_buffer_result));
dkato 0:f5de229c9a00 502 if (pHVCResult->executedFunc & HVC_ACTIV_BODY_DETECTION) {
dkato 0:f5de229c9a00 503 /* Body Detection result */
dkato 0:f5de229c9a00 504 for (i = 0; i < pHVCResult->bdResult.num; i++) {
dkato 0:f5de229c9a00 505 DrawSquare(pHVCResult->bdResult.bdResult[i].posX,
dkato 0:f5de229c9a00 506 pHVCResult->bdResult.bdResult[i].posY,
dkato 0:f5de229c9a00 507 pHVCResult->bdResult.bdResult[i].size,
dkato 0:f5de229c9a00 508 0x000000ff);
dkato 0:f5de229c9a00 509 }
dkato 0:f5de229c9a00 510 }
dkato 0:f5de229c9a00 511
dkato 0:f5de229c9a00 512 /* Face Detection result */
dkato 0:f5de229c9a00 513 if (pHVCResult->executedFunc &
dkato 0:f5de229c9a00 514 (HVC_ACTIV_FACE_DETECTION | HVC_ACTIV_FACE_DIRECTION |
dkato 0:f5de229c9a00 515 HVC_ACTIV_AGE_ESTIMATION | HVC_ACTIV_GENDER_ESTIMATION |
dkato 0:f5de229c9a00 516 HVC_ACTIV_GAZE_ESTIMATION | HVC_ACTIV_BLINK_ESTIMATION |
dkato 0:f5de229c9a00 517 HVC_ACTIV_EXPRESSION_ESTIMATION | HVC_ACTIV_FACE_RECOGNITION)){
dkato 0:f5de229c9a00 518 for (i = 0; i < pHVCResult->fdResult.num; i++) {
dkato 0:f5de229c9a00 519 if (pHVCResult->executedFunc & HVC_ACTIV_FACE_DETECTION) {
dkato 0:f5de229c9a00 520 uint32_t detection_colour = 0x0000f0f0; /* green */
dkato 0:f5de229c9a00 521
dkato 0:f5de229c9a00 522 if (pHVCResult->executedFunc & HVC_ACTIV_FACE_RECOGNITION) {
dkato 0:f5de229c9a00 523 if (pHVCResult->fdResult.fcResult[i].recognitionResult.uid >= 0) {
dkato 0:f5de229c9a00 524 detection_colour = 0x0000fff4; /* blue */
dkato 0:f5de229c9a00 525 }
dkato 0:f5de229c9a00 526 }
dkato 0:f5de229c9a00 527 /* Detection */
dkato 0:f5de229c9a00 528 DrawSquare(pHVCResult->fdResult.fcResult[i].dtResult.posX,
dkato 0:f5de229c9a00 529 pHVCResult->fdResult.fcResult[i].dtResult.posY,
dkato 0:f5de229c9a00 530 pHVCResult->fdResult.fcResult[i].dtResult.size,
dkato 0:f5de229c9a00 531 detection_colour);
dkato 0:f5de229c9a00 532 }
dkato 5:49a61433290a 533 TrackingID = 0;
dkato 5:49a61433290a 534 for (int j = 0; j < nSTBFaceCount; j++)
dkato 5:49a61433290a 535 {
dkato 5:49a61433290a 536 if (pSTBFaceResult[j].nDetectID == i) {
dkato 5:49a61433290a 537 TrackingID = pSTBFaceResult[j].nTrackingID;
dkato 5:49a61433290a 538 break;
dkato 5:49a61433290a 539 }
dkato 5:49a61433290a 540 }
dkato 5:49a61433290a 541 memset(Str_disp, 0, sizeof(Str_disp));
dkato 0:f5de229c9a00 542 if (pHVCResult->executedFunc & HVC_ACTIV_FACE_RECOGNITION) {
dkato 0:f5de229c9a00 543 /* Recognition */
dkato 5:49a61433290a 544 if (pHVCResult->fdResult.fcResult[i].recognitionResult.uid < 0) {
dkato 5:49a61433290a 545 sprintf(Str_disp, "ID:%d", TrackingID);
dkato 0:f5de229c9a00 546 } else {
dkato 0:f5de229c9a00 547 sprintf(Str_disp, "USER%03d", pHVCResult->fdResult.fcResult[i].recognitionResult.uid + 1);
dkato 0:f5de229c9a00 548 }
dkato 5:49a61433290a 549 } else {
dkato 5:49a61433290a 550 sprintf(Str_disp, "ID:%d", TrackingID);
dkato 0:f5de229c9a00 551 }
dkato 5:49a61433290a 552 DrawString(Str_disp, 0x0000f0ff);
dkato 0:f5de229c9a00 553 if (pHVCResult->executedFunc & HVC_ACTIV_AGE_ESTIMATION) {
dkato 0:f5de229c9a00 554 /* Age */
dkato 0:f5de229c9a00 555 if (-128 != pHVCResult->fdResult.fcResult[i].ageResult.age) {
dkato 0:f5de229c9a00 556 memset(Str_disp, 0, sizeof(Str_disp));
dkato 0:f5de229c9a00 557 sprintf(Str_disp, "Age:%d", pHVCResult->fdResult.fcResult[i].ageResult.age);
dkato 5:49a61433290a 558 if (pHVCResult->fdResult.fcResult[i].ageResult.confidence < 20000) {
dkato 5:49a61433290a 559 strcat(Str_disp, " (?)");
dkato 5:49a61433290a 560 }
dkato 0:f5de229c9a00 561 DrawString(Str_disp, 0x0000f0ff);
dkato 0:f5de229c9a00 562 }
dkato 0:f5de229c9a00 563 }
dkato 0:f5de229c9a00 564 if (pHVCResult->executedFunc & HVC_ACTIV_GENDER_ESTIMATION) {
dkato 0:f5de229c9a00 565 /* Gender */
dkato 0:f5de229c9a00 566 if (-128 != pHVCResult->fdResult.fcResult[i].genderResult.gender) {
dkato 5:49a61433290a 567 uint32_t wk_color;
dkato 5:49a61433290a 568 memset(Str_disp, 0, sizeof(Str_disp));
dkato 0:f5de229c9a00 569 if (1 == pHVCResult->fdResult.fcResult[i].genderResult.gender) {
dkato 5:49a61433290a 570 sprintf(Str_disp, "Male");
dkato 5:49a61433290a 571 wk_color = 0x0000fff4;
dkato 0:f5de229c9a00 572 } else {
dkato 5:49a61433290a 573 sprintf(Str_disp, "Female");
dkato 5:49a61433290a 574 wk_color = 0x00006dff;
dkato 0:f5de229c9a00 575 }
dkato 5:49a61433290a 576 if ( pHVCResult->fdResult.fcResult[i].genderResult.confidence < 20000 ) {
dkato 5:49a61433290a 577 strcat(Str_disp, " (?)");
dkato 5:49a61433290a 578 }
dkato 5:49a61433290a 579 DrawString(Str_disp, wk_color);
dkato 0:f5de229c9a00 580 }
dkato 0:f5de229c9a00 581 }
dkato 0:f5de229c9a00 582 if (pHVCResult->executedFunc & HVC_ACTIV_EXPRESSION_ESTIMATION) {
dkato 0:f5de229c9a00 583 /* Expression */
dkato 0:f5de229c9a00 584 if (-128 != pHVCResult->fdResult.fcResult[i].expressionResult.score[0]) {
dkato 0:f5de229c9a00 585 uint32_t colour;
dkato 0:f5de229c9a00 586
dkato 0:f5de229c9a00 587 if (pHVCResult->fdResult.fcResult[i].expressionResult.topExpression > EX_SADNESS) {
dkato 0:f5de229c9a00 588 pHVCResult->fdResult.fcResult[i].expressionResult.topExpression = 0;
dkato 0:f5de229c9a00 589 }
dkato 0:f5de229c9a00 590 switch (pHVCResult->fdResult.fcResult[i].expressionResult.topExpression) {
dkato 0:f5de229c9a00 591 case 1: colour = 0x0000ffff; break; /* white */
dkato 0:f5de229c9a00 592 case 2: colour = 0x0000f0ff; break; /* yellow */
dkato 0:f5de229c9a00 593 case 3: colour = 0x000060ff; break; /* orange */
dkato 0:f5de229c9a00 594 case 4: colour = 0x00000fff; break; /* purple */
dkato 0:f5de229c9a00 595 case 5: colour = 0x0000fff4; break; /* blue */
dkato 0:f5de229c9a00 596 default: colour = 0x0000ffff; break; /* white */
dkato 0:f5de229c9a00 597 }
dkato 0:f5de229c9a00 598 DrawString(pExStr[pHVCResult->fdResult.fcResult[i].expressionResult.topExpression], colour);
dkato 0:f5de229c9a00 599 }
dkato 0:f5de229c9a00 600 }
dkato 0:f5de229c9a00 601 }
dkato 0:f5de229c9a00 602 }
dkato 0:f5de229c9a00 603 }
dkato 0:f5de229c9a00 604 /* Response time */
dkato 0:f5de229c9a00 605 memset(Str_disp, 0, sizeof(Str_disp));
dkato 0:f5de229c9a00 606 sprintf(Str_disp, "Response time:%dms", resp_time.read_ms());
dkato 0:f5de229c9a00 607 ascii_font.Erase(0, 0, 0, 0, 0);
dkato 0:f5de229c9a00 608 ascii_font.DrawStr(Str_disp, 0, LCD_PIXEL_HEIGHT - AsciiFont::CHAR_PIX_HEIGHT, 0x0000ffff, 1);
dkato 0:f5de229c9a00 609 }
dkato 0:f5de229c9a00 610 } while(0);
dkato 0:f5de229c9a00 611
dkato 0:f5de229c9a00 612 EraseImage();
dkato 0:f5de229c9a00 613 memset(user_frame_buffer_result, 0, sizeof(user_frame_buffer_result));
dkato 0:f5de229c9a00 614 }
dkato 0:f5de229c9a00 615 }