Guest User

Untitled

a guest
Jan 21st, 2020
862
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <ArduinoWebsockets.h>
  2. #include "esp_http_server.h"
  3. #include <WiFi.h>
  4. #include "camera_index.h"
  5. #include "esp_camera.h"
  6. #include "fb_gfx.h"
  7. #include "fd_forward.h"
  8.  
  9.  
  10. const char* ssid = "NSA";
  11. const char* password = "Orange";
  12.  
  13. // Arduino like analogWrite
  14. // value has to be between 0 and valueMax
  15. void ledcAnalogWrite(uint8_t channel, uint32_t value, uint32_t valueMax = 180)
  16. {
  17.   // calculate duty, 8191 from 2 ^ 13 - 1
  18.   uint32_t duty = (8191 / valueMax) * min(value, valueMax);
  19.   ledcWrite(channel, duty);
  20. }
  21. int pan_center = 90; // center the pan servo
  22. int tilt_center = 90; // center the tilt servo
  23.  
  24.  
  25. #define CAMERA_MODEL_AI_THINKER
  26. #include "camera_pins.h"
  27.  
  28. #define FACE_COLOR_GREEN  0x0000FF00
  29.  
  30. using namespace websockets;
  31. WebsocketsServer socket_server;
  32.  
  33. static inline mtmn_config_t app_mtmn_config()
  34. {
  35.   mtmn_config_t mtmn_config = {0};
  36.   mtmn_config.type = FAST;
  37.   mtmn_config.min_face = 80;
  38.   mtmn_config.pyramid = 0.707;
  39.   mtmn_config.pyramid_times = 4;
  40.   mtmn_config.p_threshold.score = 0.6;
  41.   mtmn_config.p_threshold.nms = 0.7;
  42.   mtmn_config.p_threshold.candidate_number = 20;
  43.   mtmn_config.r_threshold.score = 0.7;
  44.   mtmn_config.r_threshold.nms = 0.7;
  45.   mtmn_config.r_threshold.candidate_number = 10;
  46.   mtmn_config.o_threshold.score = 0.7;
  47.   mtmn_config.o_threshold.nms = 0.7;
  48.   mtmn_config.o_threshold.candidate_number = 1;
  49.   return mtmn_config;
  50. }
  51. mtmn_config_t mtmn_config = app_mtmn_config();
  52.  
  53. httpd_handle_t camera_httpd = NULL;
  54.  
  55. void setup()
  56. {
  57.   Serial.begin(115200);
  58.   Serial.println();
  59.  
  60.   // Ai-Thinker: pins 2 and 12
  61.   ledcSetup(2, 50, 16); //channel, freq, resolution
  62.   ledcAttachPin(2, 2); // pin, channel
  63.  
  64.   ledcSetup(4, 50, 16);
  65.   ledcAttachPin(12, 4);
  66.  
  67.   ledcAnalogWrite(2, 90); // channel, 0-180
  68.   delay(1000);
  69.   ledcAnalogWrite(4, 90);
  70.  
  71.   camera_config_t config;
  72.   config.ledc_channel = LEDC_CHANNEL_0;
  73.   config.ledc_timer = LEDC_TIMER_0;
  74.   config.pin_d0 = Y2_GPIO_NUM;
  75.   config.pin_d1 = Y3_GPIO_NUM;
  76.   config.pin_d2 = Y4_GPIO_NUM;
  77.   config.pin_d3 = Y5_GPIO_NUM;
  78.   config.pin_d4 = Y6_GPIO_NUM;
  79.   config.pin_d5 = Y7_GPIO_NUM;
  80.   config.pin_d6 = Y8_GPIO_NUM;
  81.   config.pin_d7 = Y9_GPIO_NUM;
  82.   config.pin_xclk = XCLK_GPIO_NUM;
  83.   config.pin_pclk = PCLK_GPIO_NUM;
  84.   config.pin_vsync = VSYNC_GPIO_NUM;
  85.   config.pin_href = HREF_GPIO_NUM;
  86.   config.pin_sscb_sda = SIOD_GPIO_NUM;
  87.   config.pin_sscb_scl = SIOC_GPIO_NUM;
  88.   config.pin_pwdn = PWDN_GPIO_NUM;
  89.   config.pin_reset = RESET_GPIO_NUM;
  90.   config.xclk_freq_hz = 20000000;
  91.   config.pixel_format = PIXFORMAT_JPEG;
  92.   //init with high specs to pre-allocate larger buffers
  93.   if (psramFound()) {
  94.     config.frame_size = FRAMESIZE_UXGA;
  95.     config.jpeg_quality = 10;
  96.     config.fb_count = 2;
  97.   } else {
  98.     config.frame_size = FRAMESIZE_SVGA;
  99.     config.jpeg_quality = 12;
  100.     config.fb_count = 1;
  101.   }
  102. #if defined(CAMERA_MODEL_ESP_EYE)
  103.   pinMode(13, INPUT_PULLUP);
  104.   pinMode(14, INPUT_PULLUP);
  105. #endif
  106.   // camera init
  107.   esp_err_t err = esp_camera_init(&config);
  108.   if (err != ESP_OK) {
  109.     Serial.printf("Camera init failed with error 0x%x", err);
  110.     return;
  111.   }
  112.  
  113.   sensor_t * s = esp_camera_sensor_get();
  114.   s->set_framesize(s, FRAMESIZE_QVGA);
  115.  
  116.   WiFi.begin(ssid, password);
  117.  
  118.   while (WiFi.status() != WL_CONNECTED) {
  119.     delay(500);
  120.     Serial.print(".");
  121.   }
  122.   Serial.println("");
  123.   Serial.println("WiFi connected");
  124.  
  125.   app_httpserver_init();
  126.   socket_server.listen(82);
  127.  
  128.   Serial.print("Camera Ready! Use 'http://");
  129.   Serial.print(WiFi.localIP());
  130.   Serial.println("' to connect");
  131. }
  132.  
  133. static esp_err_t index_handler(httpd_req_t *req)
  134. {
  135.   httpd_resp_set_type(req, "text/html");
  136.   httpd_resp_set_hdr(req, "Content-Encoding", "gzip");
  137.   return httpd_resp_send(req, (const char *)index_ov2640_html_gz, index_ov2640_html_gz_len);
  138. }
  139.  
  140. httpd_uri_t index_uri = {
  141.   .uri       = "/",
  142.   .method    = HTTP_GET,
  143.   .handler   = index_handler,
  144.   .user_ctx  = NULL
  145. };
  146.  
  147. void app_httpserver_init ()
  148. {
  149.   httpd_config_t config = HTTPD_DEFAULT_CONFIG();
  150.   if (httpd_start(&camera_httpd, &config) == ESP_OK)
  151.     Serial.println("httpd_start");
  152.   {
  153.     httpd_register_uri_handler(camera_httpd, &index_uri);
  154.   }
  155. }
  156.  
  157. static void draw_face_boxes(dl_matrix3du_t *image_matrix, box_array_t *boxes)
  158. {
  159.   int x, y, w, h, i, half_width, half_height;
  160.   uint32_t color = FACE_COLOR_GREEN;
  161.   fb_data_t fb;
  162.   fb.width = image_matrix->w;
  163.   fb.height = image_matrix->h;
  164.   fb.data = image_matrix->item;
  165.   fb.bytes_per_pixel = 3;
  166.   fb.format = FB_BGR888;
  167.   for (i = 0; i < boxes->len; i++) {
  168.  
  169.     // Convoluted way of finding face centre...
  170.     x = ((int)boxes->box[i].box_p[0]);
  171.     w = (int)boxes->box[i].box_p[2] - x + 1;
  172.     half_width = w / 2;
  173.     int face_center_pan = x + half_width; // image frame face centre x co-ordinate
  174.  
  175.     y = (int)boxes->box[i].box_p[1];
  176.     h = (int)boxes->box[i].box_p[3] - y + 1;
  177.     half_height = h / 2;
  178.     int face_center_tilt = y + half_height;  // image frame face centre y co-ordinate
  179.  
  180.     //    assume QVGA 320x240
  181.     //    int sensor_width = 320;
  182.     //    int sensor_height = 240;
  183.     //    int lens_fov = 45
  184.     //    float diagonal = sqrt(sq(sensor_width) + sq(sensor_height)); // pixels along the diagonal
  185.     //    float pixels_per_degree = diagonal / lens_fov;
  186.     //    400/45 = 8.89
  187.  
  188.     float move_to_x = pan_center + ((-160 + face_center_pan) / 8.89) ;
  189.     float move_to_y = tilt_center + ((-120 + face_center_tilt) / 8.89) ;
  190.  
  191.     pan_center = (pan_center + move_to_x) / 2;
  192.     Serial.println(pan_center);
  193.     ledcAnalogWrite(2, pan_center); // channel, 0-180
  194.  
  195.     tilt_center = (tilt_center + move_to_y) / 2;
  196.     int reversed_tilt_center = map(tilt_center, 0, 180, 180, 0);
  197.     ledcAnalogWrite(4, reversed_tilt_center); // channel, 0-180
  198.  
  199.     fb_gfx_drawFastHLine(&fb, x, y, w, color);
  200.     fb_gfx_drawFastHLine(&fb, x, y + h - 1, w, color);
  201.     fb_gfx_drawFastVLine(&fb, x, y, h, color);
  202.     fb_gfx_drawFastVLine(&fb, x + w - 1, y, h, color);
  203.   }
  204. }
  205.  
  206. void loop()
  207. {
  208.   auto client = socket_server.accept();
  209.   camera_fb_t * fb = NULL;
  210.   dl_matrix3du_t *image_matrix = NULL;
  211.   size_t _jpg_buf_len = 0;
  212.   uint8_t * _jpg_buf = NULL;
  213.  
  214.   while (true) {
  215.     client.poll();
  216.     fb = esp_camera_fb_get();
  217.     _jpg_buf_len = fb->len;
  218.     _jpg_buf = fb->buf;
  219.     image_matrix = dl_matrix3du_alloc(1, fb->width, fb->height, 3);
  220.  
  221.     fmt2rgb888(fb->buf, fb->len, fb->format, image_matrix->item);
  222.  
  223.     box_array_t *net_boxes = NULL;
  224.     net_boxes = face_detect(image_matrix, &mtmn_config);
  225.  
  226.     if (net_boxes) {
  227.       draw_face_boxes(image_matrix, net_boxes);
  228.       free(net_boxes->score);
  229.       free(net_boxes->box);
  230.       free(net_boxes->landmark);
  231.       free(net_boxes);
  232.     }
  233.     fmt2jpg(image_matrix->item, fb->width * fb->height * 3, fb->width, fb->height, PIXFORMAT_RGB888, 90, &_jpg_buf, &_jpg_buf_len);
  234.     client.sendBinary((const char *)_jpg_buf, _jpg_buf_len);
  235.  
  236.     esp_camera_fb_return(fb);
  237.     fb = NULL;
  238.     dl_matrix3du_free(image_matrix);
  239.     free(_jpg_buf);
  240.     _jpg_buf = NULL;
  241.  
  242.   }
  243. }
RAW Paste Data