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@0:f5de229c9a00, 2017-01-23 (annotated)

Committer:
dkato
Date:
Mon Jan 23 06:22:08 2017 +0000
Revision:
0:f5de229c9a00
Child:
1:11be006960b1
first commit

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