InCarTracker

#include <InCarTracker_inferencing.h>
#include <Arduino_LSM9DS1.h>
#include <ArduinoBLE.h>


#define CONVERT_G_TO_MS2    9.80665f
#define MAX_ACCEPTED_RANGE  2.0f


static bool debug_nn = false;
static uint32_t run_inference_every_ms = 200;
static rtos::Thread inference_thread(osPriorityLow);
static float buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE] = { 0 };
static float inference_buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE];

int label = 0;
int old = 1;

BLEDevice central;
BLEService carTrackerService("5dd51778-d603-4d43-a400-1155489704f2");
BLEUnsignedCharCharacteristic carTrackerInt("73829906-105f-48dc-9c8e-e5d69659e9ed", BLERead | BLENotify);


void run_inference_background();


void setup()
{

    Serial.begin(115200);
    Serial.println("Edge Impulse Inferencing Demo");

    if (!IMU.begin()) {
        ei_printf("Failed to initialize IMU!\r\n");
    }
    else {
        ei_printf("IMU initialized\r\n");
    }

    if (EI_CLASSIFIER_RAW_SAMPLES_PER_FRAME != 3) {
        ei_printf("ERR: EI_CLASSIFIER_RAW_SAMPLES_PER_FRAME should be equal to 3 (the 3 sensor axes)\n");
        return;
    }

    inference_thread.start(mbed::callback(&run_inference_background));

    if (!BLE.begin()) {
      Serial.println("starting BLE failed!");
    
      while (1);
    }

    pinMode(LED_BUILTIN, OUTPUT);

    BLE.setLocalName("InCarTracker");
    BLE.setDeviceName("InCarTracker");
    BLE.setAdvertisedService(carTrackerService);
    carTrackerService.addCharacteristic(carTrackerInt);
    BLE.addService(carTrackerService);
    BLE.advertise();

    Serial.println("Bluetooth® device active, waiting for connections...");

    while (1) {
      central = BLE.central();
      if (central) {
        Serial.print("Connected to central: ");
        Serial.println(central.address());
        digitalWrite(LED_BUILTIN, HIGH);
        break;
      }
    } 
}


float ei_get_sign(float number) {
    return (number >= 0.0) ? 1.0 : -1.0;
}



void run_inference_background()
{

    delay((EI_CLASSIFIER_INTERVAL_MS * EI_CLASSIFIER_RAW_SAMPLE_COUNT) + 100);


    ei_classifier_smooth_t smooth;
    ei_classifier_smooth_init(&smooth, 10 /* no. of readings */, 7 /* min. readings the same */, 0.8 /* min. confidence */, 0.3 /* max anomaly */);

    while (1) {

        memcpy(inference_buffer, buffer, EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE * sizeof(float));


        signal_t signal;
        int err = numpy::signal_from_buffer(inference_buffer, EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE, &signal);
        if (err != 0) {
            ei_printf("Failed to create signal from buffer (%d)\n", err);
            return;
        }


        ei_impulse_result_t result = { 0 };

        err = run_classifier(&signal, &result, debug_nn);
        if (err != EI_IMPULSE_OK) {
            ei_printf("ERR: Failed to run classifier (%d)\n", err);
            return;
        }


        ei_printf("Predictions ");
        ei_printf("(DSP: %d ms., Classification: %d ms., Anomaly: %d ms.)",
            result.timing.dsp, result.timing.classification, result.timing.anomaly);
        ei_printf(": ");


        const char *prediction = ei_classifier_smooth_update(&smooth, &result);
        ei_printf("%s ", prediction);

        ei_printf(" [ ");
        for (size_t ix = 0; ix < smooth.count_size; ix++) {
            ei_printf("%u", smooth.count[ix]);
            if (ix != smooth.count_size + 1) {
                ei_printf(", ");
            }
            else {
              ei_printf(" ");
            }
        }
        ei_printf("]\n");

        if (central.connected()) {

            //uncertain
            if (prediction[0] == 'u') {
               label = 0;
            }
            
            // normal
            else if (prediction[0] == 'n' && prediction[1] == 'o') {
              label = 1;
            }
            
            // nazaj
            else if (prediction[0] == 'n' && prediction[1] == 'a') {
              label = 2;
            }
            
            // sovoznik
            else if (prediction[0] == 's') {
              label = 2;
            }
            
            // dol
            else if (prediction[0] == 'd') {
              label = 2;
            }
            
            if (label != old) {
              carTrackerInt.writeValue(byte(label));
              old = label;
            }
        }

        label = 9;
        
        delay(run_inference_every_ms);
    }

    ei_classifier_smooth_free(&smooth);
}



void loop()
{
    while (1) {

        uint64_t next_tick = micros() + (EI_CLASSIFIER_INTERVAL_MS * 1000);


        numpy::roll(buffer, EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE, -3);


        IMU.readAcceleration(
            buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 3],
            buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 2],
            buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 1]
        );

        for (int i = 0; i < 3; i++) {
            if (fabs(buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 3 + i]) > MAX_ACCEPTED_RANGE) {
                buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 3 + i] = ei_get_sign(buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 3 + i]) * MAX_ACCEPTED_RANGE;
            }
        }

        buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 3] *= CONVERT_G_TO_MS2;
        buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 2] *= CONVERT_G_TO_MS2;
        buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 1] *= CONVERT_G_TO_MS2;

        // and wait for next tick
        uint64_t time_to_wait = next_tick - micros();
        delay((int)floor((float)time_to_wait / 1000.0f));
        delayMicroseconds(time_to_wait % 1000);
    }
}

#if !defined(EI_CLASSIFIER_SENSOR) || EI_CLASSIFIER_SENSOR != EI_CLASSIFIER_SENSOR_ACCELEROMETER
#error "Invalid model for current sensor"
#endif