Guest User

Untitled

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