conndetect.cpp

Go to the documentation of this file.

#include "conndetect.h"
#include "config.h"
#include "conndetect_features.h"
#include "leds.h"
#include "thresholds_config.h"
#include "timedelay.h"
#include "tools.h"
#include <iostream>
#include "classifier.h"
#include <math.h>
 
#define WRITE_TO_DISK 1
 
const float WAMP_LT = 0.005;
const float WAMP_LW = 0.005;
const float WAMP_LVPU = 0.001;
 
const float ZC_T = 0.01;
 
const float ZC_W = 0.01;
 
const float ZC_VPU = 0.005;
 
using std::cerr;
using std::cout;
using std::endl;
 
// Keep these values in sync with LEDColors in sketch.js
/* const int GUI_LED_COLOR_WHITE = 0; */
/* const int GUI_LED_COLOR_RED = 1; */
/* const int GUI_LED_COLOR_GREEN = 2; */
 
typedef unsigned int us;
#define max(a, b) (((a) > (b) ? a : b))
 
void raw_conndetect_auxtask_callback(void *raw_ptr) {
  ConnDetect *conndetect = static_cast<ConnDetect *>(raw_ptr);
  conndetect->auxTaskCallback();
}
 
ConnDetect::ConnDetect(BelaContext *context) {
  _features = new Features(context->analogSampleRate);
  if(!_features) throw std::runtime_error("Unallocated features");
 
  // Assumed to be already halfly filled
  _bufs[0] = new FilteredDataBuffer(SAMPLING_FREQUENCY, true);
  // Comes a bit later
  _bufs[1] = new FilteredDataBuffer(SAMPLING_FREQUENCY);
  _bufs[1]->timeStamp = 1;
 
  _timedelay = new TimeDelay(context->analogSampleRate);
  /* Initialize logging */
#if WRITE_TO_DISK == 1
  _logfile = new LogFile<NUMBER_FEATURES>(
      logFileName, logFileHeader);
  if(!_logfile) throw std::runtime_error("Could not create log file");
 
#endif
 
  _detect_task = Bela_createAuxiliaryTask(raw_conndetect_auxtask_callback, 55,
      "Conndetect_auxtask_callback", this);
}
ConnDetect::~ConnDetect() {
 
  delete _bufs[0];
  delete _bufs[1];
  delete _features;
#if WRITE_TO_DISK == 1
  delete _logfile;
#endif
  delete _timedelay;
}
 
void ConnDetect::addFilteredFrame(const FilteredFrame &frame) {
 
  int nbufs_full = 0;
  for (auto buf : _bufs) {
    if (!(buf->full())) {
      buf->addFilteredFrame(frame);
    } else {
      nbufs_full++;
    }
  }
  if(nbufs_full == 1) {
    Bela_scheduleAuxiliaryTask(_detect_task);
  }
  else if (nbufs_full == 2) {
    std::cerr << "Two buffers full. Houston we have too much load!" << endl;
    Bela_requestStop();
  }
}
 
bool ConnDetect::connectionDetected() { return _connDetected; }
 
void ConnDetect::auxTaskCallback() {
  for (auto buf : _bufs) {
    if (buf->full()) {
      processFullBuffer((buf->data));
      buf->clear();
    }
  }
}
void ConnDetect::processFullBuffer(const FilteredData &data) {
 
  typedef FeatureType f;
  /* cerr << "P" << endl; */
  ComputedFeatures ft;
  /* std::fill(ft.begin(), ft.end(), -8000); */
 
  _features->setChannel(data[FILT_HP8k_MICT]);
  ft[(int) f::HP_8k_PEAKdB_MICT] = _features->peak_dB();
 
  _features->setChannel(data[FILT_HP8k_MICW]);
  ft[(int) f::HP_8k_PEAKdB_MICW] = _features->peak_dB();
 
  _features->setChannel(data[FILT_LP500_MICT]);
  ft[(int) f::LP_500_PEAKdB_MICT] = _features->peak_dB();
 
  _features->setChannel(data[FILT_LP500_MICW]);
  ft[(int) f::LP_500_PEAKdB_MICW] = _features->peak_dB();
 
  _features->setChannel(data[FILT_HP2k_VPU]);
  ft[(int) f::HP_2k_PEAKdB_VPU] = _features->peak_dB();
 
  _features->setChannel(data[FILT_LP500_VPU]);
  ft[(int) f::LP_500_PEAKdB_VPU] = _features->peak_dB();
 
  _features->setChannel(data[FILT_HP2k_MICT]);
  ft[(int) f::HP_2k_PEAKFREQ_MICT] = _features->peak_freq();
 
  _features->setChannel(data[FILT_RAW_MICT]);
  ft[(int) f::KURTOSIS_MICT] = _features->kurtosis();
  ft[(int) f::INVERSE_FREQ_VARIANCE_MICT] = _features->inverse_variance();
  ft[(int) f::TM3_MICT] = _features->TM3();
  /* ft[(int) f::TM5_MICT] = _features->TM5(); */
  ft[(int) f::WAMP_MICT] = _features->wamp(WAMP_LT);
  ft[(int) f::ZC_MICT] = _features->zc(ZC_T);
 
  _features->setChannel(data[FILT_RAW_MICW]);
  ft[(int) f::KURTOSIS_MICW] = _features->kurtosis();
  ft[(int) f::INVERSE_FREQ_VARIANCE_MICW] = _features->inverse_variance();
  ft[(int) f::TM3_MICW] = _features->TM3();
  /* ft[(int) f::TM5_MICW] = _features->TM5(); */
  ft[(int) f::WAMP_MICW] = _features->wamp(WAMP_LW);
  ft[(int) f::ZC_MICW] = _features->zc(ZC_W);
 
  _features->setChannel(data[FILT_RAW_VPU]);
  ft[(int) f::KURTOSIS_VPU] = _features->kurtosis();
  /* ft[(int) f::INVERSE_FREQ_VARIANCE_VPU] = _features->inverse_variance(); */
  ft[(int) f::TM3_VPU] = _features->TM3();
  /* ft[(int) f::TM5_VPU] = _features->TM5(); */
  ft[(int) f::WAMP_VPU] = _features->wamp(WAMP_LVPU);
  ft[(int) f::ZC_VPU] = _features->zc(ZC_VPU);
 
  ft[(int) f::TIMEDELAY_MICW_MICT] = _timedelay->determineDelay(
      data[FILT_RAW_MICT], data[FILT_RAW_MICW]);
 
  /*/1** IMU sensor average values *1/ */
  ft[(int) f::AVG_IMU_ABS_ROT] = SimpleFeatures::average(data[FILT_IMU_ROT]);
  ft[(int) f::MAX_IMU_ABS_HP_ACC] = SimpleFeatures::maximum(data[FILT_HP_IMU_ACC]);
 
  float pedal = SimpleFeatures::hasNonZero(data[FILT_PEDAL]);
  if(pedal) {
    cerr << "Detected pedal push" << _connDetected << endl;
  }
  ft[(int) f::PEDAL] = pedal;
 
  _connDetected = _classifier->classify(ft);
  ft[(int) f::CLASS] = _connDetected;
  if(_connDetected) {
    cerr << "Detected connection" << endl;
  }
 
#if WRITE_TO_DISK == 1
  _logfile->log(ft);
#else
 
#endif
 
}