Hello Alvik!

1/* Edge Impulse ingestion SDK
2 * Copyright (c) 2022 EdgeImpulse Inc.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 * http://www.apache.org/licenses/LICENSE-2.0
8 *
9 * Unless required by applicable law or agreed to in writing, software
10 * distributed under the License is distributed on an "AS IS" BASIS,
11 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 * See the License for the specific language governing permissions and
13 * limitations under the License.
14 *
15 */
16
17/* Includes ---------------------------------------------------------------- */
18// NOTE: Include Edge Impulse Arduino library
19//#include <AMJ-PalmDetector-HaGRID_inferencing.h>
20#include <AMJ-2gest-PalmCall-HaGRID_inferencing.h>
21
22#include "edge-impulse-sdk/dsp/image/image.hpp"
23
24#include "camera.h"
25#include "gc2145.h"
26#include <ea_malloc.h>
27
28/* Constant defines -------------------------------------------------------- */
29#define EI_CAMERA_RAW_FRAME_BUFFER_COLS           320
30#define EI_CAMERA_RAW_FRAME_BUFFER_ROWS           240
31#define EI_CAMERA_RAW_FRAME_BYTE_SIZE             2
32
33// On Nicla Vision, to turn a LED on, you have to use LOW, to turn it OFF -> HIGH
34#define NICLA_LED_HIGH LOW
35#define NICLA_LED_LOW HIGH
36/*
37 ** NOTE: If you run into TFLite arena allocation issue.
38 **
39 ** This may be due to may dynamic memory fragmentation.
40 ** Try defining "-DEI_CLASSIFIER_ALLOCATION_STATIC" in boards.local.txt (create
41 ** if it doesn't exist) and copy this file to
42 ** `<ARDUINO_CORE_INSTALL_PATH>/arduino/hardware/<mbed_core>/<core_version>/`.
43 **
44 ** See
45 ** (https://support.arduino.cc/hc/en-us/articles/360012076960-Where-are-the-installed-cores-located-)
46 ** to find where Arduino installs cores on your machine.
47 **
48 ** If the problem persists then there's not enough memory for this model and application.
49 */
50
51#define ALIGN_PTR(p,a)   ((p & (a-1)) ?(((uintptr_t)p + a) & ~(uintptr_t)(a-1)) : p)
52
53/* Edge Impulse ------------------------------------------------------------- */
54
55typedef struct {
56    size_t width;
57    size_t height;
58} ei_device_resize_resolutions_t;
59
60/**
61 * @brief      Check if new serial data is available
62 *
63 * @return     Returns number of available bytes
64 */
65int ei_get_serial_available(void) {
66    return Serial.available();
67}
68
69/**
70 * @brief      Get next available byte
71 *
72 * @return     byte
73 */
74char ei_get_serial_byte(void) {
75    return Serial.read();
76}
77
78/* Private variables ------------------------------------------------------- */
79static bool debug_nn = false; // Set this to true to see e.g. features generated from the raw signal
80static bool is_initialised = false;
81static bool is_ll_initialised = false;
82
83GC2145 galaxyCore;
84Camera cam(galaxyCore);
85FrameBuffer fb;
86
87/*
88** @brief points to the output of the capture
89*/
90static uint8_t *ei_camera_capture_out = NULL;
91
92/*
93** @brief used to store the raw frame
94*/
95static uint8_t *ei_camera_frame_mem;
96static uint8_t *ei_camera_frame_buffer; // 32-byte aligned
97
98/* Function definitions ------------------------------------------------------- */
99bool ei_camera_init(void);
100void ei_camera_deinit(void);
101bool ei_camera_capture(uint32_t img_width, uint32_t img_height, uint8_t *out_buf) ;
102int calculate_resize_dimensions(uint32_t out_width, uint32_t out_height, uint32_t *resize_col_sz, uint32_t *resize_row_sz, bool *do_resize);
103
104/**
105* @brief      Arduino setup function
106*/
107void setup()
108{
109    // put your setup code here, to run once:
110    Serial.begin(115200);
111    // comment out the below line to cancel the wait for USB connection (needed for native USB)
112    //while (!Serial);
113    Serial.println("Edge Impulse Inferencing Demo");
114
115    // initialise M4 RAM
116    // Arduino Nicla Vision has 512KB of RAM allocated for M7 core
117    // and additional 244k (sic!) on the M4 address space
118    // allocating 288 kB as in the line below was
119    // advised by a member of Arduino team
120    malloc_addblock((void*)0x30000000, 288 * 1024);
121
122    if (ei_camera_init() == false) {
123        ei_printf("Failed to initialize Camera!\r\n");
124    }
125    else {
126        ei_printf("Camera initialized\r\n");
127    }
128
129    Serial1.begin(115200); //PA10-4-D2 (RX), PA9-3-D1 (TX)
130    pinMode(LEDG, OUTPUT);
131    pinMode(LEDR, OUTPUT);
132    pinMode(LEDB, OUTPUT);
133    digitalWrite(LEDR, NICLA_LED_LOW);
134    digitalWrite(LEDG, NICLA_LED_LOW);
135    digitalWrite(LEDB, NICLA_LED_LOW);
136}
137
138/**
139* @brief      Get data and run inferencing
140*
141* @param[in]  debug  Get debug info if true
142*/
143void loop()
144{
145    ei_printf("\nStarting inferencing in 2 seconds...\n");
146
147    // instead of wait_ms, we'll wait on the signal, this allows threads to cancel us...
148    if (ei_sleep(2000) != EI_IMPULSE_OK) {
149        return;
150    }
151
152    ei_printf("Taking photo...\n");
153
154    ei::signal_t signal;
155    signal.total_length = EI_CLASSIFIER_INPUT_WIDTH * EI_CLASSIFIER_INPUT_HEIGHT;
156    signal.get_data = &ei_camera_get_data;
157
158    if (ei_camera_capture((size_t)EI_CLASSIFIER_INPUT_WIDTH, (size_t)EI_CLASSIFIER_INPUT_HEIGHT, NULL) == false) {
159        ei_printf("Failed to capture image\r\n");
160        return;
161    }
162
163    // Run the classifier
164    ei_impulse_result_t result = { 0 };
165
166    EI_IMPULSE_ERROR err = run_classifier(&signal, &result, debug_nn);
167    if (err != EI_IMPULSE_OK) {
168        ei_printf("ERR: Failed to run classifier (%d)\n", err);
169        return;
170    }
171
172    int max_index = -1;
173    float max_value = 0.85;
174    // print the predictions
175    ei_printf("Predictions (DSP: %d ms., Classification: %d ms., Anomaly: %d ms.): \n",
176                result.timing.dsp, result.timing.classification, result.timing.anomaly);
177#if EI_CLASSIFIER_OBJECT_DETECTION == 1
178    ei_printf("Object detection bounding boxes:\r\n");
179    for (uint32_t i = 0; i < result.bounding_boxes_count; i++) {
180        ei_impulse_result_bounding_box_t bb = result.bounding_boxes[i];
181        if (bb.value == 0) {
182            continue;
183        }
184
185        ei_printf("  %s (%f) [ x: %u, y: %u, width: %u, height: %u ]\r\n",
186                bb.label,
187                bb.value,
188                bb.x,
189                bb.y,
190                bb.width,
191                bb.height);
192
193        if ((bb.value >= max_value) && (bb.width >= 16 || bb.height >= 16)) {
194          for (int j=0; j<sizeof(ei_classifier_inferencing_categories); j++) {
195            if (strcmp(bb.label, ei_classifier_inferencing_categories[j]) == 0) {
196              max_value = bb.value;
197              max_index = j;
198            }
199          }   
200        }
201    }
202
203    // Print the prediction results (classification)
204#else
205    ei_printf("Predictions:\r\n");
206    for (uint16_t i = 0; i < EI_CLASSIFIER_LABEL_COUNT; i++) {
207        ei_printf("  %s: ", ei_classifier_inferencing_categories[i]);
208        ei_printf("%.5f\r\n", result.classification[i].value);
209
210      if (result.classification[i].value > max_value) {
211        max_value = result.classification[i].value;
212        max_index = i;
213      }
214    }
215#endif
216
217    // Print anomaly result (if it exists)
218#if EI_CLASSIFIER_HAS_ANOMALY
219    ei_printf("Anomaly prediction: %.3f\r\n", result.anomaly);
220#endif
221
222#if EI_CLASSIFIER_HAS_VISUAL_ANOMALY
223    ei_printf("Visual anomalies:\r\n");
224    for (uint32_t i = 0; i < result.visual_ad_count; i++) {
225        ei_impulse_result_bounding_box_t bb = result.visual_ad_grid_cells[i];
226        if (bb.value == 0) {
227            continue;
228        }
229        ei_printf("  %s (%f) [ x: %u, y: %u, width: %u, height: %u ]\r\n",
230                bb.label,
231                bb.value,
232                bb.x,
233                bb.y,
234                bb.width,
235                bb.height);
236    }
237#endif
238
239    switch (max_index) {
240      case 0: // 2gestures: call, 1gesture: palm
241        Serial.println("send character A");
242        Serial1.print('a');
243        digitalWrite(LEDG, NICLA_LED_HIGH);
244        delay(2000);
245        digitalWrite(LEDG, NICLA_LED_LOW);
246
247        break;
248      case 1: // 2gestures: palm
249        Serial.println("send character G");
250        Serial1.print('g');
251        digitalWrite(LEDG, NICLA_LED_HIGH);
252        delay(2000);
253        digitalWrite(LEDG, NICLA_LED_LOW);
254
255        break;
256      case 2:
257        Serial.println("send character F");
258        Serial1.print('f');
259        break;
260      case 3:
261        Serial.println("send character B");
262        Serial1.print('b');
263        break;
264      case 4:
265        Serial.println("send character L");
266        Serial1.print('l');
267        break;
268      case 5:
269        Serial.println("send character R");
270        Serial1.print('r');
271        break;
272      default:
273        Serial1.print('u'); // unknown
274        break;
275    }
276
277    // Turn off LEDs
278    digitalWrite(LEDR, NICLA_LED_LOW);
279    digitalWrite(LEDG, NICLA_LED_LOW);
280    digitalWrite(LEDB, NICLA_LED_LOW);
281}
282
283/**
284 * @brief   Setup image sensor & start streaming
285 *
286 * @retval  false if initialisation failed
287 */
288bool ei_camera_init(void) {
289    if (is_initialised) return true;
290
291    if (is_ll_initialised == false) {
292        if (!cam.begin(CAMERA_R320x240, CAMERA_RGB565, -1)) {
293            ei_printf("ERR: Failed to initialise camera\r\n");
294            return false;
295        }
296
297    //bugfix: model works with flipped image (?)
298    if(cam.setVerticalFlip(true) != 0) {
299      ei_printf("ERR: Failed to flip image camera\r\n");
300      return false;
301    }
302
303    // initialize frame buffer
304    ei_camera_frame_mem = (uint8_t *) ei_malloc(EI_CAMERA_RAW_FRAME_BUFFER_COLS * EI_CAMERA_RAW_FRAME_BUFFER_ROWS * EI_CAMERA_RAW_FRAME_BYTE_SIZE + 32 /*alignment*/);
305    if(ei_camera_frame_mem == NULL) {
306        ei_printf("failed to create ei_camera_frame_mem\r\n");
307        return false;
308    }
309    ei_camera_frame_buffer = (uint8_t *)ALIGN_PTR((uintptr_t)ei_camera_frame_mem, 32);
310
311    fb.setBuffer(ei_camera_frame_buffer);
312    is_initialised = true;
313    }
314
315    return true;
316}
317
318/**
319 * @brief      Stop streaming of sensor data
320 */
321void ei_camera_deinit(void) {
322
323    ei_free(ei_camera_frame_mem);
324    ei_camera_frame_mem = NULL;
325    ei_camera_frame_buffer = NULL;
326    is_initialised = false;
327}
328
329/**
330 * @brief      Capture, rescale and crop image
331 *
332 * @param[in]  img_width     width of output image
333 * @param[in]  img_height    height of output image
334 * @param[in]  out_buf       pointer to store output image, NULL may be used
335 *                           if ei_camera_frame_buffer is to be used for capture and resize/cropping.
336 *
337 * @retval     false if not initialised, image captured, rescaled or cropped failed
338 *
339 */
340bool ei_camera_capture(uint32_t img_width, uint32_t img_height, uint8_t *out_buf) {
341    bool do_resize = false;
342    bool do_crop = false;
343
344    ei_camera_capture_out = (uint8_t*)ea_malloc(EI_CAMERA_RAW_FRAME_BUFFER_COLS * EI_CAMERA_RAW_FRAME_BUFFER_ROWS * 3 + 32);
345    ei_camera_capture_out = (uint8_t *)ALIGN_PTR((uintptr_t)ei_camera_capture_out, 32);
346
347    if (!is_initialised) {
348        ei_printf("ERR: Camera is not initialized\r\n");
349        return false;
350    }
351
352    int snapshot_response = cam.grabFrame(fb, 100);
353    if (snapshot_response != 0) {
354        ei_printf("ERR: Failed to get snapshot (%d)\r\n", snapshot_response);
355        return false;
356    }
357
358    bool converted = RBG565ToRGB888(ei_camera_frame_buffer, ei_camera_capture_out, cam.frameSize());
359
360    if(!converted){
361        ei_printf("ERR: Conversion failed\n");
362        ei_free(ei_camera_frame_mem);
363        return false;
364    }
365
366    uint32_t resize_col_sz;
367    uint32_t resize_row_sz;
368    // choose resize dimensions
369    int res = calculate_resize_dimensions(img_width, img_height, &resize_col_sz, &resize_row_sz, &do_resize);
370    if (res) {
371        ei_printf("ERR: Failed to calculate resize dimensions (%d)\r\n", res);
372        return false;
373    }
374
375    if ((img_width != resize_col_sz)
376        || (img_height != resize_row_sz)) {
377        do_crop = true;
378    }
379
380    if (do_resize) {
381
382          ei::image::processing::crop_and_interpolate_rgb888(
383          ei_camera_capture_out,
384          EI_CAMERA_RAW_FRAME_BUFFER_COLS,
385          EI_CAMERA_RAW_FRAME_BUFFER_ROWS,
386          ei_camera_capture_out,
387          resize_col_sz,
388          resize_row_sz);
389    }
390
391    ea_free(ei_camera_capture_out);
392    return true;
393}
394
395/**
396 * @brief      Convert rgb565 data to rgb888
397 *
398 * @param[in]  src_buf  The rgb565 data
399 * @param      dst_buf  The rgb888 data
400 * @param      src_len  length of rgb565 data
401 */
402
403bool RBG565ToRGB888(uint8_t *src_buf, uint8_t *dst_buf, uint32_t src_len)
404{
405    uint8_t hb, lb;
406    uint32_t pix_count = src_len / 2;
407
408    for(uint32_t i = 0; i < pix_count; i ++) {
409        hb = *src_buf++;
410        lb = *src_buf++;
411
412        *dst_buf++ = hb & 0xF8;
413        *dst_buf++ = (hb & 0x07) << 5 | (lb & 0xE0) >> 3;
414        *dst_buf++ = (lb & 0x1F) << 3;
415    }
416
417    return true;
418}
419
420static int ei_camera_get_data(size_t offset, size_t length, float *out_ptr)
421{
422    // we already have a RGB888 buffer, so recalculate offset into pixel index
423    size_t pixel_ix = offset * 3;
424    size_t pixels_left = length;
425    size_t out_ptr_ix = 0;
426
427    while (pixels_left != 0) {
428        out_ptr[out_ptr_ix] = (ei_camera_capture_out[pixel_ix] << 16) + (ei_camera_capture_out[pixel_ix + 1] << 8) + ei_camera_capture_out[pixel_ix + 2];
429
430        // go to the next pixel
431        out_ptr_ix++;
432        pixel_ix+=3;
433        pixels_left--;
434    }
435
436    // and done!
437    return 0;
438}
439
440/**
441 * @brief      Determine whether to resize and to which dimension
442 *
443 * @param[in]  out_width     width of output image
444 * @param[in]  out_height    height of output image
445 * @param[out] resize_col_sz       pointer to frame buffer's column/width value
446 * @param[out] resize_row_sz       pointer to frame buffer's rows/height value
447 * @param[out] do_resize     returns whether to resize (or not)
448 *
449 */
450int calculate_resize_dimensions(uint32_t out_width, uint32_t out_height, uint32_t *resize_col_sz, uint32_t *resize_row_sz, bool *do_resize)
451{
452    size_t list_size = 6;
453    const ei_device_resize_resolutions_t list[list_size] = {
454        {64, 64},
455        {96, 96},
456        {160, 120},
457        {160, 160},
458        {320, 240},
459    };
460
461    // (default) conditions
462    *resize_col_sz = EI_CAMERA_RAW_FRAME_BUFFER_COLS;
463    *resize_row_sz = EI_CAMERA_RAW_FRAME_BUFFER_ROWS;
464    *do_resize = false;
465
466    for (size_t ix = 0; ix < list_size; ix++) {
467        if ((out_width <= list[ix].width) && (out_height <= list[ix].height)) {
468            *resize_col_sz = list[ix].width;
469            *resize_row_sz = list[ix].height;
470            *do_resize = true;
471            break;
472        }
473    }
474
475    return 0;
476}
477
478#if !defined(EI_CLASSIFIER_SENSOR) || EI_CLASSIFIER_SENSOR != EI_CLASSIFIER_SENSOR_CAMERA
479#error "Invalid model for current sensor"
480#endif

1#include <esp_now.h>
2#include <WiFi.h>
3
4#define SERIAL_CMD Serial1 // [D9-rx,D8-tx]
5
6#define NUM_ADDR 3
7
8// This is the First address (BETT #1 - 3c:84:27:c4:68:40, Christian's Alvik {0x74, 0x4D, 0xBD, 0xA2, 0x20, 0x58})
9uint8_t broadcastAddress1[] = {0x3c, 0x84, 0x27, 0xc4, 0x68, 0x40};
10
11// This is the Second address (BETT #2 - 3c:84:27:c4:8f:18, Leonardo's Alvik (Greeting #1) {0x74, 0x4D, 0xBD, 0xA2, 0x20, 0x58})
12uint8_t broadcastAddress2[] = {0x3c, 0x84, 0x27, 0xc4, 0x8f, 0x18};
13
14// This is the Third address (Greeting #2 - 74:4d:bd:9f:ce:64)
15uint8_t broadcastAddress3[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
16
17// Command definitions
18#define MOVE_FORWARD "MOVE_FORWARD"
19#define MOVE_BACKWARD "MOVE_BACKWARD"
20#define TURN_LEFT "TURN_LEFT"
21#define TURN_RIGHT "TURN_RIGHT"
22#define GREET "GREET"
23#define ACTION "ACTION"
24
25esp_now_peer_info_t peerInfo;
26
27// Callback when data is sent
28void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
29  Serial.print("\r\nLast Packet Send Status:\t");
30  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
31}
32 
33void setup() {
34  // Initialize the serial monitor
35  Serial.begin(115200);
36  SERIAL_CMD.begin(115200, SERIAL_8N1, D9, D8);
37 
38  // Set the device as a Wi-Fi Station
39  WiFi.mode(WIFI_STA);
40
41  // Initialize ESP-NOW
42  if (esp_now_init() != ESP_OK) {
43    Serial.println("Error initializing ESP-NOW");
44    return;
45  }
46
47  // Register the callback to get the status of transmitted packets
48  esp_now_register_send_cb(OnDataSent);
49
50  peerInfo.channel = 0;  
51  peerInfo.encrypt = false; 
52  
53
54#if NUM_ADDR > 0  
55  // Register first peer
56  memcpy(peerInfo.peer_addr, broadcastAddress1, 6);     
57  if (esp_now_add_peer(&peerInfo) != ESP_OK){
58    Serial.println("Failed to add first peer");
59    return;
60  }
61#endif
62
63#if NUM_ADDR > 1  
64  // Register second peer
65  memcpy(peerInfo.peer_addr, broadcastAddress2, 6);     
66  if (esp_now_add_peer(&peerInfo) != ESP_OK){
67    Serial.println("Failed to add second peer");
68    return;
69  }
70#endif
71
72#if NUM_ADDR > 2  
73  // Register third peer
74  memcpy(peerInfo.peer_addr, broadcastAddress3, 6);     
75  if (esp_now_add_peer(&peerInfo) != ESP_OK){
76    Serial.println("Failed to add third peer");
77    return;
78  }
79#endif
80
81  Serial.println("Setup complete. Ready to send commands.");
82}
83 
84void loop() {
85  if (SERIAL_CMD.available() > 0) {
86    char command = SERIAL_CMD.read();
87    Serial.println(command);
88    esp_err_t result;
89
90    switch (command) {
91      case 'f':
92        result = esp_now_send(0, (uint8_t *) MOVE_FORWARD, strlen(MOVE_FORWARD));
93        break;
94      case 'b':
95        result = esp_now_send(0, (uint8_t *) MOVE_BACKWARD, strlen(MOVE_BACKWARD));
96        break;
97      case 'l':
98        result = esp_now_send(0, (uint8_t *) TURN_LEFT, strlen(TURN_LEFT));
99        break;
100      case 'r':
101        result = esp_now_send(0, (uint8_t *) TURN_RIGHT, strlen(TURN_RIGHT));
102        break;
103      case 'g':
104        result = esp_now_send(0, (uint8_t *) GREET, strlen(GREET));
105        break;
106      case 'a':
107        result = esp_now_send(0, (uint8_t *) ACTION, strlen(ACTION));
108        break;
109      default:
110        Serial.println("Unknown command");
111        return;
112    }
113
114    if (result == ESP_OK) {
115      Serial.println("Sent with success");
116    } else {
117      Serial.println("Error sending the data");
118    }
119  }
120}