tremble.ino

#include <SFE_MMA8452Q.h>

#define DEBUG false

#define CACHE_SIZE 16
#define IDX_ACT 0
#define IDX_X 1
#define IDX_Y 2
#define IDX_Z 3
#define IDX_LIMXYZ 4
#define IDX_PL 1
#define IDX_LIMPL 2

#define CYCLE_DELAY 10
#define RATIO 10

MMA8452Q accel;

unsigned long count = 0;

unsigned long cache_idx = 0;
float xyz_cache[CACHE_SIZE][IDX_LIMXYZ];
byte pl_cache[CACHE_SIZE][IDX_LIMPL];
float xyz_stddev_cache[IDX_LIMXYZ];

void setup() {
  delay(10000);
  Serial.begin(9600);
  Serial.println("INFO: MMA8452Q init");
  accel.init();
  Serial.println("INFO: MMA8452Q inited");
  
  clearCache();
}

void loop() {
  float array[IDX_LIMXYZ];
  byte pl = 0;
  
  if (accel.available()) {
    accel.read();
    
    getCurrentXYZCalculatedAccels(array);
    pl = getCurrentPl();
    
    fillCache(array, pl);

    if (DEBUG) {
      Serial.println("DEBUG: current data >>");
      Serial.print(count);
      Serial.print(",");
      printData(array, pl);
      Serial.println();
      Serial.println("DEBUG: << current data");
    }
    
    if (anomalyDetected(array, pl) == true) {
      printAnonymity(array, pl);
    }
    count++;
  }
  delay(CYCLE_DELAY);
}

/* i2c operation */
void getCurrentXYZCalculatedAccels(float * array) {
  array[IDX_X] = accel.cx; 
  array[IDX_Y] = accel.cy;
  array[IDX_Z] = accel.cz;
}

byte getCurrentPl()
{
  return accel.readPL();
}

/* cache operation */
unsigned long getNextIdx() {
  return (cache_idx + 1) % CACHE_SIZE;
}

unsigned long getPrevIdx() {
  return (cache_idx + CACHE_SIZE - 1) % CACHE_SIZE;
}

void forwardIdx() {
  cache_idx = getNextIdx();
}

void fillCache(float * array, byte pl) {
  unsigned long next_idx = getNextIdx();
  xyz_cache[next_idx][IDX_X] = array[IDX_X];
  xyz_cache[next_idx][IDX_Y] = array[IDX_Y];
  xyz_cache[next_idx][IDX_Z] = array[IDX_Z];
  xyz_cache[next_idx][IDX_ACT] = 1;
  
  pl_cache[next_idx][IDX_PL] = pl;
  pl_cache[next_idx][IDX_ACT] = 1;
  
  forwardIdx();
}

void clearCache() {
  for (int i = 0 ; i < CACHE_SIZE; i++) {
    xyz_cache[i][IDX_ACT] = 0;
    xyz_cache[i][IDX_X] = 0.0;
    xyz_cache[i][IDX_Y] = 0.0;
    xyz_cache[i][IDX_Z] = 0.0;
    
    pl_cache[i][IDX_ACT] = 0;
    pl_cache[i][IDX_PL] = 0;
  }
  cache_idx = 0;
}

void getAverageXYZData(float * average) {
  int num = 0;
  float sum_x = 0, sum_y = 0, sum_z = 0;
  float avg_x = 0, avg_y = 0, avg_z = 0;
  for (int i = 0; i < CACHE_SIZE; i++) {
    if (i == cache_idx)
      continue; /* don't include me */
    if (xyz_cache[i][IDX_ACT] == 0)
      continue; /* not valid cache */
    sum_x += xyz_cache[i][IDX_X];
    sum_y += xyz_cache[i][IDX_Y];
    sum_z += xyz_cache[i][IDX_Z];
    num ++;
  }
  
  average[IDX_X] = sum_x / num;
  average[IDX_Y] = sum_y / num;
  average[IDX_Z] = sum_z / num;
}

void getStdDevXYZData(float * stddev) {
  float sum_x = 0, sum_y = 0, sum_z = 0;
  float average[IDX_LIMXYZ];
  int num = 0;
  
  getAverageXYZData(average);
  for (int i = 0; i < CACHE_SIZE; i++) {
    if (i == cache_idx)
      continue;
    if (xyz_cache[i][IDX_ACT] == 0)
      continue;
    sum_x += pow(xyz_cache[i][IDX_X] - average[IDX_X], 2);
    sum_y += pow(xyz_cache[i][IDX_Y] - average[IDX_Y], 2);
    sum_z += pow(xyz_cache[i][IDX_Z] - average[IDX_Z], 2);
    num++;
  }
  
  stddev[IDX_X] = sqrt(sum_x/num);
  stddev[IDX_Y] = sqrt(sum_y/num);
  stddev[IDX_Z] = sqrt(sum_z/num);
}

void getPrevXYZData(float * prev) {
  int prev_idx = getPrevIdx();
  prev[IDX_X] = xyz_cache[prev_idx][IDX_X];
  prev[IDX_Y] = xyz_cache[prev_idx][IDX_Y];
  prev[IDX_Z] = xyz_cache[prev_idx][IDX_Z];
}

byte getPrevPl() {
  int prev_idx = getPrevIdx();
  return pl_cache[prev_idx][IDX_PL];
}

bool isCurrentExceedStddev(float * array) {
  float stddev[IDX_LIMXYZ], average[IDX_LIMXYZ];
  getStdDevXYZData(stddev);
  getAverageXYZData(average);
  
  for (int i = IDX_X; i < IDX_LIMXYZ; i++) {
    if (abs(array[i] - average[i]) > (stddev[i] * RATIO)) {
      return true;
    }
  }
  return false;
}

bool anomalyDetected(float * array, byte pl) {
  byte prev_pl = getPrevPl();
  /* if PL is different, it's anomaly */
  if (prev_pl != pl) {
    return true;
  }
  
  /* if current is more than stddev * RATIO, it's anomaly */
  if (isCurrentExceedStddev(array)) {
    return true;
  }
 
  return false;
}

void printAnonymity(float * array, byte pl) {
  byte prev_pl = getPrevPl();
  float prev_xyz[IDX_LIMXYZ], average[IDX_LIMXYZ], stddev[IDX_LIMXYZ];
  
  Serial.print("DETECT,");
  Serial.print(count);
  Serial.print(",");
  getPrevXYZData(prev_xyz);
  getAverageXYZData(average);
  getStdDevXYZData(stddev);
  
  /* print current */
  printData(array, pl);
  /* print prev */
  printData(prev_xyz, prev_pl);
  /* average */
  printData(average, 0);
  /* stdddev */
  printData(stddev, 0);
  
  Serial.println();
}

void printData(float * array, byte pl) {
  /* print current */
  Serial.print("");
  Serial.print(pl);
  Serial.print(",");
  for (int i = IDX_X; i < IDX_LIMXYZ; i++) {
    Serial.print(array[i], 3);
    Serial.print(",");
  }
}