source: azure_iot_hub_riscv/trunk/app_iothub_client/src/kpu_main.c@ 459

Last change on this file since 459 was 459, checked in by coas-nagasima, 4 years ago

Azure IoT への送信内容を、検出したpersonとcarの数を送信するよう変更。
Azure IoT CentralからLEDのON/OFFが出来るよう更新。
  • Property svn:eol-style set to native
  • Property svn:mime-type set to text/x-csrc;charset=UTF-8
File size: 15.9 KB
Line 
1/*
2* TOPPERS/ASP Kernel
3* Toyohashi Open Platform for Embedded Real-Time Systems/
4* Advanced Standard Profile Kernel
5*
6* Copyright (C) 2000-2003 by Embedded and Real-Time Systems Laboratory
7* Toyohashi Univ. of Technology, JAPAN
8* Copyright (C) 2004-2012 by Embedded and Real-Time Systems Laboratory
9* Graduate School of Information Science, Nagoya Univ., JAPAN
10*
11* 上記著作権者は，以下の(1)～(4)の条件を満たす場合に限り，本ソフトウェ
12* ア（本ソフトウェアを改変したものを含む．以下同じ）を使用・複製・改
13* 変・再配布（以下，利用と呼ぶ）することを無償で許諾する．
14* (1) 本ソフトウェアをソースコードの形で利用する場合には，上記の著作
15* 権表示，この利用条件および下記の無保証規定が，そのままの形でソー
16* スコード中に含まれていること．
17* (2) 本ソフトウェアを，ライブラリ形式など，他のソフトウェア開発に使
18* 用できる形で再配布する場合には，再配布に伴うドキュメント（利用
19* 者マニュアルなど）に，上記の著作権表示，この利用条件および下記
20* の無保証規定を掲載すること．
21* (3) 本ソフトウェアを，機器に組み込むなど，他のソフトウェア開発に使
22* 用できない形で再配布する場合には，次のいずれかの条件を満たすこ
23* と．
24* (a) 再配布に伴うドキュメント（利用者マニュアルなど）に，上記の著
25* 作権表示，この利用条件および下記の無保証規定を掲載すること．
26* (b) 再配布の形態を，別に定める方法によって，TOPPERSプロジェクトに
27* 報告すること．
28* (4) 本ソフトウェアの利用により直接的または間接的に生じるいかなる損
29* 害からも，上記著作権者およびTOPPERSプロジェクトを免責すること．
30* また，本ソフトウェアのユーザまたはエンドユーザからのいかなる理
31* 由に基づく請求からも，上記著作権者およびTOPPERSプロジェクトを
32* 免責すること．
33*
34* 本ソフトウェアは，無保証で提供されているものである．上記著作権者お
35* よびTOPPERSプロジェクトは，本ソフトウェアに関して，特定の使用目的
36* に対する適合性も含めて，いかなる保証も行わない．また，本ソフトウェ
37* アの利用により直接的または間接的に生じたいかなる損害に関しても，そ
38* の責任を負わない．
39*
40* $Id: kpu_main.c 2176 2020-08-19 23:50:05Z coas-nagasima $
41*/
42
43#include <kernel.h>
44#include <stdlib.h>
45#include <t_syslog.h>
46#include <t_stdlib.h>
47#include <target_syssvc.h>
48#include "syssvc/serial.h"
49#include "syssvc/syslog.h"
50#include "device.h"
51#include "pinmode.h"
52#include "sipeed_st7789.h"
53#include "sipeed_ov2640.h"
54#include "storagedevice.h"
55#ifdef SDEV_SENSE_ONETIME
56#include "spi_driver.h"
57#include "sddiskio.h"
58#else
59#include "rom_file.h"
60#endif
61#include "kernel_cfg.h"
62#include "kpu_main.h"
63#include "kpu.h"
64#include "region_layer.h"
65
66/*
67* サービスコールのエラーのログ出力
68*/
69Inline void
70svc_perror(const char *file, int_t line, const char *expr, ER ercd)
71{
72 if (ercd < 0) {
73 t_perror(LOG_ERROR, file, line, expr, ercd);
74 }
75}
76
77#define SVC_PERROR(expr) svc_perror(__FILE__, __LINE__, #expr, (expr))
78
79#define SWAP_32(x) ((x >> 24) | (x >> 8 & 0xff00) | (x << 8 & 0xff0000) | (x << 24))
80
81#ifndef SPI1DMATX_SEM
82#define SPI1DMATX_SEM 0
83#endif
84
85#define CLASS_NUMBER 20
86
87LCD_Handler_t LcdHandle;
88LCD_DrawProp_t DrawProp;
89DVP_Handle_t DvpHandle;
90OV2640_t CameraHandle;
91
92uint8_t sTxBuffer[16];
93
94static uint8_t time_string[12];
95
96static void
97set_value(uint8_t *buf, int value)
98{
99 buf[1] = (value % 10) + '0';
100 buf[0] = (value / 10) + '0';
101}
102
103static void
104set_time(uint8_t *buf, struct tm2 *tm)
105{
106 buf[8] = 0;
107 set_value(&buf[6], tm->tm_sec);
108 buf[5] = ':';
109 set_value(&buf[3], tm->tm_min);
110 buf[2] = ':';
111 set_value(&buf[0], tm->tm_hour);
112}
113
114/*
115* ダイレクトデジタルピン設定
116*/
117void
118pinMode(uint8_t Pin, uint8_t dwMode){
119 int gpionum = gpio_get_gpiohno(Pin, false);
120 GPIO_Init_t init = {0};
121
122 syslog_2(LOG_NOTICE, "## pinMode Pin(%d) gpionum(%d) ##", Pin, gpionum);
123 if(gpionum >= 0){
124 uint8_t function = FUNC_GPIOHS0 + gpionum;
125 fpioa_set_function(Pin, function);
126 switch(dwMode){
127 case INPUT:
128 init.mode = GPIO_MODE_INPUT;
129 init.pull = GPIO_NOPULL;
130 break;
131 case INPUT_PULLDOWN:
132 init.mode = GPIO_MODE_INPUT;
133 init.pull = GPIO_PULLDOWN;
134 break;
135 case INPUT_PULLUP:
136 init.mode = GPIO_MODE_INPUT;
137 init.pull = GPIO_PULLUP;
138 break;
139 case OUTPUT:
140 default:
141 init.mode = GPIO_MODE_OUTPUT;
142 init.pull = GPIO_PULLDOWN;
143 break;
144 }
145 gpio_setup(TADR_GPIOHS_BASE, &init, (uint8_t)gpionum);
146 }
147 return ;
148}
149
150/*
151 * ダイレクトデジタルピン出力
152 */
153void
154digitalWrite(uint8_t Pin, int dwVal){
155 int8_t gpio_pin = gpio_get_gpiohno(Pin, false);
156
157 if( gpio_pin >= 0){
158 gpio_set_pin(TADR_GPIOHS_BASE, (uint8_t)gpio_pin, dwVal);
159 }
160}
161
162#if (CLASS_NUMBER > 1)
163typedef struct
164{
165 char *str;
166 uint16_t color;
167 uint16_t height;
168 uint16_t width;
169} class_lable_t;
170
171class_lable_t class_lable[CLASS_NUMBER] =
172{
173 {"aeroplane", ST7789_GREEN},
174 {"bicycle", ST7789_GREEN},
175 {"bird", ST7789_GREEN},
176 {"boat", ST7789_GREEN},
177 {"bottle", 0xF81F},
178 {"bus", ST7789_GREEN},
179 {"car", ST7789_GREEN},
180 {"cat", ST7789_GREEN},
181 {"chair", 0xFD20},
182 {"cow", ST7789_GREEN},
183 {"diningtable", ST7789_GREEN},
184 {"dog", ST7789_GREEN},
185 {"horse", ST7789_GREEN},
186 {"motorbike", ST7789_GREEN},
187 {"person", 0xF800},
188 {"pottedplant", ST7789_GREEN},
189 {"sheep", ST7789_GREEN},
190 {"sofa", ST7789_GREEN},
191 {"train", ST7789_GREEN},
192 {"tvmonitor", 0xF9B6}
193};
194
195static uint32_t lable_string_draw_ram[115 * 16 * 8 / 2];
196#endif
197extern const uint8_t model_data[];
198kpu_model_context_t g_task;
199static region_layer_t detect_rl;
200yolo_result_t yolo_result;
201yolo_result_t yolo_result_frame;
202
203volatile uint8_t g_ai_done_flag;
204
205void ai_done(void *ctx)
206{
207 g_ai_done_flag = 2;
208}
209
210#define ANCHOR_NUM 5
211float g_anchor[ANCHOR_NUM * 2] = {1.08, 1.19, 3.42, 4.41, 6.63, 11.38, 9.42, 5.11, 16.62, 10.52};
212
213static void drawboxes(uint32_t x1, uint32_t y1, uint32_t x2, uint32_t y2, uint32_t class, float prob)
214{
215 if (x1 >= 320)
216 x1 = 319;
217 if (x2 >= 320)
218 x2 = 319;
219 if (y1 >= 240)
220 y1 = 239;
221 if (y2 >= 240)
222 y2 = 239;
223
224#if (CLASS_NUMBER > 1)
225 DrawProp.TextColor = class_lable[class].color;
226 lcd_drawRect(&DrawProp, x1, y1, x2-x1, y2-y1);
227 DrawProp.TextColor = ST7789_WHITE;
228 DrawProp.BackColor = class_lable[class].color;
229 DrawProp.pFont = &Font12;
230 lcd_DisplayStringAt(&DrawProp, x1 + 1, y1 + 1, class_lable[class].str, LEFT_MODE);
231#else
232 lcd_drawRect(&DrawProp, x1, y1, x2, y2, 2, ST7789_RED);
233#endif
234 if (strcmp(class_lable[class].str, "person") == 0)
235 yolo_result_frame.person++;
236 if (strcmp(class_lable[class].str, "car") == 0)
237 yolo_result_frame.car++;
238}
239
240/*
241* メインタスク
242*/
243void kpu_task(intptr_t exinf)
244{
245 SPI_Init_t Init;
246 SPI_Handle_t *hspi;
247 LCD_Handler_t *hlcd;
248 OV2640_t *hcmr;
249 DVP_Handle_t *hdvp;
250 ER_UINT ercd;
251 uint32_t i;
252 struct tm2 time;
253 unsigned long atmp;
254#ifdef SDEV_SENSE_ONETIME
255 StorageDevice_t *psdev;
256 SDCARD_Handler_t *hsd = NULL;
257#else
258 uint32_t esize, fsize, headlen;
259 uint8_t *addr;
260#endif
261
262 SVC_PERROR(syslog_msk_log(LOG_UPTO(LOG_INFO), LOG_UPTO(LOG_EMERG)));
263 syslog(LOG_NOTICE, "Sample program starts (exinf = %d).", (int_t) exinf);
264
265 pinMode(LED_G_PIN, OUTPUT);
266 digitalWrite(LED_G_PIN, HIGH);
267 pinMode(LED_R_PIN, OUTPUT);
268 digitalWrite(LED_R_PIN, HIGH);
269 pinMode(LED_B_PIN, OUTPUT);
270 digitalWrite(LED_B_PIN, HIGH);
271
272 select_spi0_dvp_mode(1);
273
274#ifdef SDEV_SENSE_ONETIME
275 pinMode(SPI_SS_PIN, OUTPUT);
276 digitalWrite(SPI_SS_PIN, HIGH);
277#else /* USE ROMFILE */
278 rom_file_init();
279 esize = 1*1024*1024;
280 addr = (uint8_t *)romfont;
281 while(esize > 4){
282 fsize = (addr[0] << 24) | (addr[1] << 16) | (addr[2] << 8) | addr[3];
283 if((fsize & 3) != 0 || fsize < 16 || fsize > esize)
284 break;
285 for(i = 0 ; i < 28 ; i++){
286 if(addr[i+4] == 0)
287 break;
288 }
289 if(i >= 28)
290 break;
291 if(i < 12)
292 headlen = 16;
293 else
294 headlen = 32;
295 if(create_rom_file((const char *)(addr+4), (uintptr_t)(addr+headlen), fsize-headlen) < 0)
296 break;
297 addr += fsize;
298 esize -= fsize;
299 }
300#endif
301
302 hcmr = &CameraHandle;
303 hcmr->frameSize = FRAMESIZE_QVGA;
304 hcmr->pixFormat = PIXFORMAT_RGB565;
305 ov2640_getResolition(hcmr, FRAMESIZE_QVGA);
306 hcmr->_resetPoliraty = ACTIVE_HIGH;
307 hcmr->_pwdnPoliraty = ACTIVE_HIGH;
308 hcmr->_slaveAddr = 0x00;
309 hcmr->_dataBuffer = NULL;
310 hcmr->_aiBuffer = NULL;
311
312 // just support RGB565 and YUV442 on k210
313
314 // Initialize the camera bus, 8bit reg
315 hdvp = &DvpHandle;
316 hcmr->hdvp = hdvp;
317 hdvp->Init.Freq = 24000000;
318 hdvp->Init.Width = hcmr->_width;
319 hdvp->Init.Height = hcmr->_height;
320 hdvp->Init.Format = DVP_FORMAT_RGB;
321 hdvp->Init.BurstMode = DVP_BURST_ENABLE;
322 hdvp->Init.AutoMode = DVP_AUTOMODE_DISABLE;
323 hdvp->Init.GMMlen = 4;
324
325 hdvp->Init.num_sccb_reg = 8;
326 hdvp->Init.CMosPClkPin = 47;
327 hdvp->Init.CMosXClkPin = 46;
328 hdvp->Init.CMosHRefPin = 45;
329 hdvp->Init.CMosPwDnPin = 44;
330 hdvp->Init.CMosVSyncPin = 43;
331 hdvp->Init.CMosRstPin = 42;
332 hdvp->Init.SccbSClkPin = 41;
333 hdvp->Init.SccbSdaPin = 40;
334 hdvp->Init.IntNo = INTNO_DVP;
335 syslog_1(LOG_NOTICE, "## DvpHandle[%08x] ##", &DvpHandle);
336
337 syslog_3(LOG_NOTICE, "## hcmr->_width(%d) hcmr->_height(%d) size[%08x] ##", hcmr->_width, hcmr->_height, (hcmr->_width * hcmr->_height * 2));
338 atmp = (unsigned long)malloc(hcmr->_width * hcmr->_height * 2 + 64); //RGB565
339 hcmr->_dataBuffer = (uint32_t *)((atmp + 63) & ~63);
340 if(hcmr->_dataBuffer == NULL){
341 hcmr->_width = 0;
342 hcmr->_height = 0;
343 syslog_0(LOG_ERROR, "Can't allocate _dataBuffer !");
344 slp_tsk();
345 }
346 atmp = (unsigned long)malloc(hcmr->_width * hcmr->_height * 3 + 128); //RGB888
347 hcmr->_aiBuffer = (uint32_t *)((atmp + 127) & ~127);
348 if(hcmr->_aiBuffer == NULL){
349 hcmr->_width = 0;
350 hcmr->_height = 0;
351 free(hcmr->_dataBuffer);
352 hcmr->_dataBuffer = NULL;
353 syslog_0(LOG_ERROR, "Can't allocate _aiBuffer !");
354 slp_tsk();
355 }
356 syslog_2(LOG_NOTICE, "## hcmr->_dataBuffer[%08x] hcmr->_aiBuffer[%08x] ##", hcmr->_dataBuffer, hcmr->_aiBuffer);
357 atmp = (unsigned long)hcmr->_aiBuffer - IOMEM;
358 hdvp->Init.RedAddr = (uint32_t)atmp;
359 atmp = (unsigned long)hcmr->_aiBuffer + hcmr->_width * hcmr->_height - IOMEM;
360 hdvp->Init.GreenAddr = (uint32_t)atmp;
361 atmp = (unsigned long)hcmr->_aiBuffer + hcmr->_width * hcmr->_height * 2 - IOMEM;
362 hdvp->Init.BlueAddr = (uint32_t)atmp;
363 atmp = (unsigned long)hcmr->_dataBuffer - IOMEM;
364 hdvp->Init.RGBAddr = (uint32_t)atmp;
365 dvp_init(hdvp);
366
367 if(ov2640_sensor_ov_detect(hcmr) == E_OK){
368 syslog_0(LOG_NOTICE, "find ov sensor !");
369 }
370 else if(ov2640_sensro_gc_detect(hcmr) == E_OK){
371 syslog_0(LOG_NOTICE, "find gc3028 !");
372 }
373 if(ov2640_reset(hcmr) != E_OK){
374 syslog_0(LOG_ERROR, "ov2640 reset error !");
375 slp_tsk();
376 }
377 if(ov2640_set_pixformat(hcmr) != E_OK){
378 syslog_0(LOG_ERROR, "set pixformat error !");
379 slp_tsk();
380 }
381 if(ov2640_set_framesize(hcmr) != E_OK){
382 syslog_0(LOG_ERROR, "set frame size error !");
383 slp_tsk();
384 }
385 if(ov2640_setInvert(hcmr, false) != E_OK){
386 syslog_0(LOG_ERROR, "set invert error !");
387 slp_tsk();
388 }
389 syslog_1(LOG_NOTICE, "OV2640 id(%d)", ov2640_id(hcmr));
390
391 Init.WorkMode = SPI_WORK_MODE_2;
392 Init.FrameFormat = SPI_FF_OCTAL;
393 Init.DataSize = 8;
394 Init.Prescaler = 15000000;
395 Init.SignBit = 0;
396 Init.InstLength = 8;
397 Init.AddrLength = 0;
398 Init.WaitCycles = 0;
399 Init.IATransMode = SPI_AITM_AS_FRAME_FORMAT;
400 Init.SclkPin = SIPEED_ST7789_SCLK_PIN;
401 Init.MosiPin = -1;
402 Init.MisoPin = -1;
403 Init.SsPin = SIPEED_ST7789_SS_PIN;
404 Init.SsNo = SIPEED_ST7789_SS;
405 Init.TxDMAChannel = -1;
406 Init.RxDMAChannel = -1;
407 Init.semid = SPI1TRN_SEM;
408 Init.semlock = SPI1LOCK_SEM;
409 Init.semdmaid = SPI1DMATX_SEM;
410 hspi = spi_init(SPI_PORTID, &Init);
411 if(hspi == NULL){
412 syslog_0(LOG_ERROR, "SPI INIT ERROR");
413 slp_tsk();
414 }
415
416 hlcd = &LcdHandle;
417 hlcd->hspi = hspi;
418// hlcd->spi_lock= SPI1LOCK_SEM;
419 hlcd->dir = DIR_YX_LRUD;
420 hlcd->dcx_pin = SIPEED_ST7789_DCX_PIN;
421 hlcd->rst_pin = SIPEED_ST7789_RST_PIN;
422 hlcd->cs_sel = SIPEED_ST7789_SS;
423 hlcd->rst_no = SIPEED_ST7789_RST_GPIONUM;
424 hlcd->dcx_no = SIPEED_ST7789_DCX_GPIONUM;
425 DrawProp.hlcd = hlcd;
426 lcd_init(hlcd);
427 syslog_2(LOG_NOTICE, "width(%d) height(%d)", hlcd->_width, hlcd->_height);
428
429 DrawProp.BackColor = ST7789_WHITE;
430 DrawProp.TextColor = ST7789_BLACK;
431 lcd_fillScreen(&DrawProp);
432
433#ifdef SDEV_SENSE_ONETIME
434 Init.WorkMode = SPI_WORK_MODE_0;
435 Init.FrameFormat = SPI_FF_STANDARD;
436 Init.DataSize = 8;
437 Init.Prescaler = 5000000;
438 Init.SignBit = 0;
439 Init.InstLength = 0;
440 Init.AddrLength = 0;
441 Init.WaitCycles = 0;
442 Init.IATransMode = SPI_AITM_STANDARD;
443 Init.SclkPin = SPI_SCK_PIN;
444 Init.MosiPin = SPI_MOSI_PIN;
445 Init.MisoPin = SPI_MISO_PIN;
446 Init.SsPin = -1;
447 Init.SsNo = -1;
448 Init.TxDMAChannel = -1;
449 Init.RxDMAChannel = SPI_DMA1_CH;
450 Init.semid = SPI2TRN_SEM;
451 Init.semlock = SPI2LOCK_SEM;
452 Init.semdmaid = SPI2DMATX_SEM;
453 hspi = spi_init(SPICARD_PORTID, &Init);
454 if(hspi == NULL){
455 syslog_0(LOG_ERROR, "SPI-CARD INIT ERROR");
456 slp_tsk();
457 }
458 sdcard_setspi2(SPISDCARD_PORTID, hspi, SPI_SS_PIN);
459
460 /*
461 * SD-CARD SPI通信設定
462 */
463 for(i = 0 ; i < 10 ; i++)
464 sTxBuffer[i] = 0xff;
465 if((ercd = spi_core_transmit(hspi, -1, (uint8_t*)sTxBuffer, 10)) != E_OK){
466 /* Transfer error in transmission process */
467 syslog_1(LOG_NOTICE, "## call Error_Handler(2)(%d) ##", ercd);
468 }
469#if SPI_WAIT_TIME == 0
470 if((ercd = spi_wait(hspi, 100)) != E_OK){
471 syslog_0(LOG_NOTICE, "## call Error_Handler(3) ##");
472 }
473#endif
474 dly_tsk(100);
475 SDMSence_task(0);
476 psdev = SDMGetStorageDevice(SDCARD_DEVNO);
477 hsd = (SDCARD_Handler_t *)psdev->_sdev_local[1];
478 if(hsd == NULL)
479 syslog_0(LOG_ERROR, "SD-CARD INITAIL ERROR !");
480#endif
481 if ((ercd = kpu_init(&g_task)) != E_OK) {
482 syslog_0(LOG_ERROR, "kpu init error");
483 slp_tsk();
484 }
485
486 if (kpu_load_kmodel(&g_task, model_data) != 0) {
487 syslog_0(LOG_ERROR, "kmodel init error");
488 slp_tsk();
489 }
490
491 detect_rl.anchor_number = ANCHOR_NUM;
492 detect_rl.anchor = g_anchor;
493 detect_rl.threshold = 0.7;
494 detect_rl.nms_value = 0.3;
495 if (region_layer_init(&detect_rl, 10, 7, 125, 320, 240) != 0) {
496 syslog_0(LOG_ERROR, "region layer init error");
497 slp_tsk();
498 }
499
500 bool_t camok = true;
501 if((ercd = ov2640_activate(hcmr, true)) != E_OK){
502 syslog_2(LOG_NOTICE, "ov2640 activate error result(%d) id(%d) ##", ercd, ov2640_id(hcmr));
503 camok = false;
504 }
505
506 int tmo = 0;
507 g_ai_done_flag = 0;
508 while (camok) {
509 if (g_ai_done_flag == 0) {
510 ercd = ov2640_snapshot(hcmr);
511 if (ercd != E_OK) {
512 camok = false;
513 continue;
514 }
515 g_ai_done_flag = 1;
516
517 /* start to calculate */
518 atmp = (unsigned long)hcmr->_aiBuffer - IOMEM;
519 kpu_run_kmodel(&g_task, (const uint8_t *)atmp, AI_DMA_CH, ai_done, NULL);
520 }
521
522 if (g_ai_done_flag == 1) {
523 tmo++;
524 if (tmo >= 10) {
525 tmo = 0;
526 g_ai_done_flag = 0;
527 }
528 else {
529 dly_tsk(1);
530 }
531 continue;
532 }
533 else if (g_ai_done_flag == 2) {
534 g_ai_done_flag = 0;
535 float *output;
536 size_t output_size;
537 kpu_get_output(&g_task, 0, &output, &output_size);
538 detect_rl.input = output;
539
540 /* start region layer */
541 region_layer_run(&detect_rl, NULL);
542 }
543
544 lcd_drawPicture(hlcd, 0, 0, hcmr->_width, hcmr->_height, (uint16_t *)hcmr->_dataBuffer);
545
546 yolo_result_frame.person = 0;
547 yolo_result_frame.car = 0;
548
549 /* draw boxs */
550 region_layer_draw_boxes(&detect_rl, drawboxes);
551
552 if (yolo_result.reset) {
553 yolo_result.reset = 0;
554 yolo_result.person = 0;
555 yolo_result.car = 0;
556 }
557 if (yolo_result.person < yolo_result_frame.person)
558 yolo_result.person = yolo_result_frame.person;
559 if (yolo_result.car < yolo_result_frame.car)
560 yolo_result.car = yolo_result_frame.car;
561 }
562 ov2640_activate(hcmr, false);
563
564 syslog_0(LOG_NOTICE, "## STOP ##");
565 slp_tsk();
566 syslog(LOG_NOTICE, "Sample program ends.");
567 SVC_PERROR(ext_ker());
568 assert(0);
569}
Note: See TracBrowser for help on using the repository browser.