//TODO add description here
//DONE other 8 sensor readings
//TODO "outer loop" to log last n results
//DONE calculate measured mag strength sqrt(mx^2+my^2+mz^2) and add to stats
//DONE calculate measured acceleration strength and add to stats.
#include <NXPMotionSense.h>
NXPMotionSense imu;
const int ledPin = 13;
int ledState = LOW;
elapsedMillis ledMillis = 0;
//elapsedMillis serialMillis = 0;
void setup() {
Serial.begin(115200);
while (!Serial) ; // wait for serial port open
imu.begin();
pinMode(ledPin, OUTPUT);
Serial.println("SensorStatistics.ino version 0.1");
Serial.println("T = time in milliseconds");
Serial.println("All other lines: First value is mean,");
Serial.println("second value is variance.");
Serial.println("Each reading is calculated from 1000 sensor samples.");
Serial.println("AMag and MMag are absolute magnitude of acceleration");
Serial.println("vector and magnetic field vector respectively.");
}
int dataCount = 0;
double mean[11]; //means of ax, ay, az, gx, gy, gz, mx, my, mz, accMag, magMag;
double variance[11]; //variances of ax, ay, az, gx, gy, gz, mx, my, mz, accMag, magMag;
void loop() {
int data[9]; //sensor readings of ax, ay, az, gx, gy, gz, mx, my, mz;
double accMagnitude; //Magnitude of acceleration vector
double magMagnitude; //Magnitude of magnetic field vector
double delta;
if (imu.available()) {
imu.readMotionSensor( data[0], data[1], data[2], \
data[3], data[4], data[5], \
data[6], data[7], data[8] );
//calculate acceleration and magnetic field magnitudes
accMagnitude = sqrt( pow(data[0],2) + pow(data[1],2) + pow(data[2],2) );
magMagnitude = sqrt( pow(data[6],2) + pow(data[7],2) + pow(data[8],2) );
//if this is the first round, initialize the arrays
if (dataCount == 0) {
for (int i=0; i<11; i++) {
mean[i] = 0.0;
variance[i] = 0.0;
}
}
dataCount++; //It's important to do increment _before_ the following computations!
//Update statistics data
for (int i=0; i<9; i++) {
delta = data[i] - mean[i];
mean[i] += delta / dataCount;
variance[i] += delta * (data[i] - mean[i]); //this accumulates delta^2.
// Final division by sample count is done
// after all samples have been collected
}
delta = accMagnitude - mean[9];
mean[9] += delta / dataCount;
variance[9] += delta * (accMagnitude - mean[9]);
delta = magMagnitude - mean[10];
mean[10] += delta / dataCount;
variance[10] += delta * (magMagnitude - mean[10]);
//print heartbeat every 50 samples
if (dataCount % 50 == 0) {
Serial.print(".");
}
// if 1k values have been sampled,
// finish calculations and print results
if (dataCount >= 999) {
for (int i=0; i<11; i++) {
variance[i] /= (dataCount -1);
}
printStats();
dataCount = 0;
}
}
/*
// print a time stamp every 20 seconds
if (serialMillis >= 20000) {
serialMillis -= 20000;
Serial.print("T ");
Serial.println(millis());
Serial.println("");
}*/
// blink Led
if (ledMillis >= 1000) {
ledMillis -= 1000;
if (ledState == LOW) digitalWrite(ledPin, ledState = HIGH);
else digitalWrite(ledPin, ledState = LOW);
}
}
void printStats() {
Serial.println("");
Serial.print("T ");
Serial.println(millis());
Serial.print("AX ");
Serial.print(mean[0], 5);
Serial.print(" ");
Serial.println(variance[0], 5);
Serial.print("AY ");
Serial.print(mean[1], 5);
Serial.print(" ");
Serial.println(variance[1], 5);
Serial.print("AZ ");
Serial.print(mean[2], 5);
Serial.print(" ");
Serial.println(variance[2], 5);
Serial.print("GX ");
Serial.print(mean[3], 5);
Serial.print(" ");
Serial.println(variance[3], 5);
Serial.print("GY ");
Serial.print(mean[4], 5);
Serial.print(" ");
Serial.println(variance[4], 5);
Serial.print("GZ ");
Serial.print(mean[5], 5);
Serial.print(" ");
Serial.println(variance[5], 5);
Serial.print("MX ");
Serial.print(mean[6], 5);
Serial.print(" ");
Serial.println(variance[6], 5);
Serial.print("MY ");
Serial.print(mean[7], 5);
Serial.print(" ");
Serial.println(variance[7], 5);
Serial.print("MZ ");
Serial.print(mean[8], 5);
Serial.print(" ");
Serial.println(variance[8], 5);
Serial.print("AMag ");
Serial.print(mean[9], 5);
Serial.print(" ");
Serial.println(variance[9], 5);
Serial.print("MMag ");
Serial.print(mean[10], 5);
Serial.print(" ");
Serial.println(variance[10], 5);
Serial.println("");
}