Hearing Substitution Using Haptic Feedback

#include <ArduinoBLE.h>
#include "bsec.h"
#include <Arduino_APDS9960.h>
#include <PDM.h>
#include <baby-cry_inferencing.h>

// If your target is limited in memory remove this macro to save 10K RAM
#define EIDSP_QUANTIZE_FILTERBANK   0
#define EI_CLASSIFIER_SLICES_PER_MODEL_WINDOW 3


#define SERVICE_UUID                  "3D7D1101-BA27-40B2-836C-17505C1044D7"
#define ENV_WRITE_CHAR_UUID           "3D7D1102-BA27-40B2-836C-17505C1044D7"
#define ENV_READ_CHAR_UUID            "3D7D1103-BA27-40B2-836C-17505C1044D7"
#define CLASSIFICATION_CHAR_UUID      "3D7D1104-BA27-40B2-836C-17505C1044D7"


BLEService babyService(SERVICE_UUID);
BLECharacteristic bme680WriteChar(ENV_WRITE_CHAR_UUID, BLERead | BLENotify, "00;00;0000;000;000;0");
BLECharCharacteristic bme680ReadChar(ENV_READ_CHAR_UUID, BLERead | BLENotify);
BLECharCharacteristic classificationChar(CLASSIFICATION_CHAR_UUID, BLERead | BLENotify);

// Create an object of the class Bsec
Bsec iaqSensor;
String output;
void checkIaqSensorStatus(void);

int temperature = 0;
int humid = 0;
int airQuality = 0;

int r = 0, g = 0, b = 0, c = 0;


void enableLowPower() {

  digitalWrite(PIN_ENABLE_SENSORS_3V3, LOW);
  digitalWrite(PIN_ENABLE_I2C_PULLUP, LOW);

}

void disableLowPower() {
  digitalWrite(PIN_ENABLE_SENSORS_3V3, HIGH);
  digitalWrite(PIN_ENABLE_I2C_PULLUP, HIGH);

}

/** Audio buffers, pointers and selectors */
typedef struct {
  signed short *buffers[2];
  unsigned char buf_select;
  unsigned char buf_ready;
  unsigned int buf_count;
  unsigned int n_samples;
} inference_t;

static inference_t inference;
static bool record_ready = false;
static signed short *sampleBuffer;
static bool debug_nn = false; // Set this to true to see e.g. features generated from the raw signal
static int print_results = -(EI_CLASSIFIER_SLICES_PER_MODEL_WINDOW);
int predicted_label = 2;
float prediction_threshold = 0.9;

void setup() {
  Serial.begin(9600);

  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_PWR, LOW);

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

  Wire.begin();
  iaqSensor.begin(BME680_I2C_ADDR_SECONDARY, Wire);
  checkIaqSensorStatus();
  bsec_virtual_sensor_t sensorList[10] = {
    BSEC_OUTPUT_RAW_TEMPERATURE,
    BSEC_OUTPUT_RAW_HUMIDITY,
    BSEC_OUTPUT_IAQ

  };

  iaqSensor.updateSubscription(sensorList, 3, BSEC_SAMPLE_RATE_LP);
  checkIaqSensorStatus();

  if (!APDS.begin()) {
    Serial.println("Error initializing APDS9960 sensor.");
  }
  BLE.setLocalName("BabyConnect");
  BLE.setDeviceName("BabyConnect");
  BLE.setAdvertisedService(babyService);
  babyService.addCharacteristic(bme680ReadChar);
  babyService.addCharacteristic(bme680WriteChar);
  babyService.addCharacteristic(classificationChar);

  BLE.addService(babyService);

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

  run_classifier_init();
  if (microphone_inference_start(EI_CLASSIFIER_SLICE_SIZE) == false) {
    ei_printf("ERR: Failed to setup audio sampling\r\n");
    return;
  }
}

void loop()
{
  BLEDevice central = BLE.central();

  if (central)
  {
    Serial.print("Connected to central: ");
    Serial.println(central.address());
    digitalWrite(LED_BUILTIN, HIGH);
    disableLowPower();

    while (central.connected()) {


      if (iaqSensor.run()) { // If new data is available

        temperature = (int) iaqSensor.rawTemperature * 1.8 + 32;
        humid =  iaqSensor.rawHumidity;
        airQuality = iaqSensor.iaq;
      } else {
        checkIaqSensorStatus();
      }

      if (APDS.colorAvailable()) {
        APDS.readColor(r, g, b, c);
      }

      int battery = analogRead(A0);
      int batteryLevel = map(battery, 0, 1023, 0, 100);

      //now run EI inference

      bool m = microphone_inference_record();
      if (!m) {
        ei_printf("ERR: Failed to record audio...\n");
        return;
      }

      signal_t signal;
      signal.total_length = EI_CLASSIFIER_SLICE_SIZE;
      signal.get_data = &microphone_audio_signal_get_data;
      ei_impulse_result_t result = {0};

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

      float max_prediction = 0.0;
      
      if (++print_results >= (EI_CLASSIFIER_SLICES_PER_MODEL_WINDOW)) {
        // print the predictions
        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(": \n");
        for (size_t ix = 0; ix < EI_CLASSIFIER_LABEL_COUNT; ix++) {
          ei_printf("    %s: %.5f\n", result.classification[ix].label, result.classification[ix].value);

          if ( result.classification[ix].value >= max_prediction) {
            max_prediction = result.classification[ix].value;
            predicted_label = ix;
          }

        }
        

        
        
        if(max_prediction < prediction_threshold){
          predicted_label = 2;
        }else{
          ei_printf("Predicted activity = %d with score=%.5f\n", predicted_label, max_prediction);
        }
      }
      //end of EI inference

      char buf[24];
      sprintf(buf, "%d;%d;%d;%d;%d;%d", temperature, humid, airQuality, c, batteryLevel, predicted_label);
      Serial.println(buf);
      bme680WriteChar.writeValue(String(buf).c_str());
      //delay(1 * 1000);

    }
  }
  digitalWrite(LED_BUILTIN, LOW);
  enableLowPower();

}

void checkIaqSensorStatus(void)
{
  if (iaqSensor.status != BSEC_OK) {
    if (iaqSensor.status < BSEC_OK) {
      output = "BSEC error code : " + String(iaqSensor.status);
      Serial.println(output);
      while (true) {

      }

    } else {
      output = "BSEC warning code : " + String(iaqSensor.status);
      Serial.println(output);
    }
  }

  if (iaqSensor.bme680Status != BME680_OK) {
    if (iaqSensor.bme680Status < BME680_OK) {
      output = "BME680 error code : " + String(iaqSensor.bme680Status);
      Serial.println(output);
      while (true) {

      }
    } else {
      output = "BME680 warning code : " + String(iaqSensor.bme680Status);
      Serial.println(output);
    }
  }
}

/**
 * @brief      Printf function uses vsnprintf and output using Arduino Serial
 *
 * @param[in]  format     Variable argument list
 */
void ei_printf(const char *format, ...) {
    static char print_buf[1024] = { 0 };

    va_list args;
    va_start(args, format);
    int r = vsnprintf(print_buf, sizeof(print_buf), format, args);
    va_end(args);

    if (r > 0) {
        Serial.write(print_buf);
    }
}

/**
 * @brief      PDM buffer full callback
 *             Get data and call audio thread callback
 */
static void pdm_data_ready_inference_callback(void)
{
    int bytesAvailable = PDM.available();

    // read into the sample buffer
    int bytesRead = PDM.read((char *)&sampleBuffer[0], bytesAvailable);

    if (record_ready == true) {
        for (int i = 0; i<bytesRead>> 1; i++) {
            inference.buffers[inference.buf_select][inference.buf_count++] = sampleBuffer[i];

            if (inference.buf_count >= inference.n_samples) {
                inference.buf_select ^= 1;
                inference.buf_count = 0;
                inference.buf_ready = 1;
            }
        }
    }
}

/**
 * @brief      Init inferencing struct and setup/start PDM
 *
 * @param[in]  n_samples  The n samples
 *
 * @return     { description_of_the_return_value }
 */
static bool microphone_inference_start(uint32_t n_samples)
{
    inference.buffers[0] = (signed short *)malloc(n_samples * sizeof(signed short));

    if (inference.buffers[0] == NULL) {
        return false;
    }

    inference.buffers[1] = (signed short *)malloc(n_samples * sizeof(signed short));

    if (inference.buffers[0] == NULL) {
        free(inference.buffers[0]);
        return false;
    }

    sampleBuffer = (signed short *)malloc((n_samples >> 1) * sizeof(signed short));

    if (sampleBuffer == NULL) {
        free(inference.buffers[0]);
        free(inference.buffers[1]);
        return false;
    }

    inference.buf_select = 0;
    inference.buf_count = 0;
    inference.n_samples = n_samples;
    inference.buf_ready = 0;

    // configure the data receive callback
    PDM.onReceive(&pdm_data_ready_inference_callback);

    // optionally set the gain, defaults to 20
    PDM.setGain(80);

    PDM.setBufferSize((n_samples >> 1) * sizeof(int16_t));

    // initialize PDM with:
    // - one channel (mono mode)
    // - a 16 kHz sample rate
    if (!PDM.begin(1, EI_CLASSIFIER_FREQUENCY)) {
        ei_printf("Failed to start PDM!");
    }

    record_ready = true;

    return true;
}

/**
 * @brief      Wait on new data
 *
 * @return     True when finished
 */
static bool microphone_inference_record(void)
{
    bool ret = true;

    if (inference.buf_ready == 1) {
        ei_printf(
            "Error sample buffer overrun. Decrease the number of slices per model window "
            "(EI_CLASSIFIER_SLICES_PER_MODEL_WINDOW)\n");
        ret = false;
    }

    while (inference.buf_ready == 0) {
        delay(1);
    }

    inference.buf_ready = 0;

    return ret;
}

/**
 * Get raw audio signal data
 */
static int microphone_audio_signal_get_data(size_t offset, size_t length, float *out_ptr)
{
    numpy::int16_to_float(&inference.buffers[inference.buf_select ^ 1][offset], out_ptr, length);

    return 0;
}

/**
 * @brief      Stop PDM and release buffers
 */
static void microphone_inference_end(void)
{
    PDM.end();
    free(inference.buffers[0]);
    free(inference.buffers[1]);
    free(sampleBuffer);
}

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