Add wave magnitude calculation

earthquake/earthquake.ino

@@ -39,13 +39,14 @@ Quaternion q;           // [w, x, y, z]         quaternion container
 VectorInt16 aa;         // [x, y, z]            accel sensor measurements
 VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
 VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
-VectorInt16 aaWorldPrev;    // [x, y, z]            world-frame accel sensor measurements
+VectorInt16 aaWorldPrev[TIME_WAVE_STORE];    // [x, y, z]            world-frame accel sensor measurements
 VectorFloat gravity;    // [x, y, z]            gravity vector
 float euler[3];         // [psi, theta, phi]    Euler angle container
 float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
 bool isChangeStore[TIME_WAVE_STORE];
 int periode;
 int frequency;
+double amplitude;
 //INTERRUPT DETECTION ROUTINE
 volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
 void dmpDataReady() {
@@ -63,6 +64,7 @@ void setup() {
   loopNumber = 0;
   periode = 0;
   frequency = 0;
+  amplitude = 0;
 
   //LCD
   lcd.begin(16, 2);
@@ -133,49 +135,40 @@ void setup() {
 }
 
 void changeShift() {
-  int offset = 200;
-  int dx = abs(aaWorldPrev.x - aaWorld.x);
-  int dy = abs(aaWorldPrev.y - aaWorld.y);
-  int dz = abs(aaWorldPrev.z - aaWorld.z);
-  bool isChange = (dx > offset || dy > offset || dz > offset);
   int i;
   for(i=0;i<TIME_WAVE_STORE-1;i++){
-    isChangeStore[i] = isChangeStore[i+1];
-  }
-  if(isChange){
-    Serial.println("CHANGE");
+    aaWorldPrev[i] = aaWorldPrev[i+1];
   }
-  isChangeStore[TIME_WAVE_STORE-1] = isChange;
+  aaWorldPrev[TIME_WAVE_STORE-1] = aaWorld;
 }
 
 bool isChange(){
+  int offset = 200;
   int i;
-  int first = -1;
-  int second = -1;
   for(i = TIME_WAVE_STORE-1; i >= 0 ; i--){
-    if(isChangeStore[i]){
-      if(periode!=0) return true;
-      if(first<0){
-        first = i;
-        continue;
-      }
-      if(second<0){
-        second = i;
-        periode = (first - second)*2;
-        frequency = 1*1000/periode;
-        return true;
-      }
+    int dx = abs(aaWorldPrev[i].x - aaWorld.x);
+    int dy = abs(aaWorldPrev[i].y - aaWorld.y);
+    int dz = abs(aaWorldPrev[i].z - aaWorld.z);
+    bool isChange = (dx > offset || dy > offset || dz > offset);
+    if(isChange){
+      double amplitudeNow = sqrt(pow(dx,2)+pow(dy,2)+pow(dz,2))*1000;
+      amplitude = ((amplitudeNow>amplitude || amplitude==0.0)?amplitudeNow:amplitude);
+      return true;
     }
   }
   return false;
 }
 
-int magnitudeConvert(VectorInt16 prev, VectorInt16 curr) {
-  int dx = prev.x - curr.x;
-  int dy = prev.y - curr.y;
-  int dz = prev.z - curr.z;
-  double magnitude = sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2));
-  return floor(magnitude);
+double magnitudeConvert() {
+  // assume the epicentral distance is 1km
+  return log10(amplitude)-0.83+1.73*log10(0.5);
+//  return log10(amplitude)-2.48+2.76*log10(0.5);
+}
+
+void displaySevenSegment(int val){
+    digitalWrite(latchPin, LOW);
+    shiftOut(dataPin, clockPin, MSBFIRST, digit[val]);
+    digitalWrite(latchPin, HIGH);
 }
 
 void loop() {
@@ -267,20 +260,13 @@ void loop() {
     bool check = isChange();
     digitalWrite(LED_PIN, check);
     if(check){
-      Serial.println(periode);
-      Serial.println(frequency);
+      int magnitudeC = (int) magnitudeConvert();
+      Serial.println(amplitude);
+      Serial.println(magnitudeC);
+      displaySevenSegment(magnitudeC%10);
     } else {
-      periode = 0;
-      frequency = 0;
+      amplitude = 0;
+      displaySevenSegment(0);
     }
-//    if (check) {
-//      waveDuration = WAVE_TIMEOUT;
-//    }
-//    if ((waveDuration > 0)) {
-//      digitalWrite(LED_PIN, true);
-//      Serial.println(waveDuration);
-//      waveDuration--;
-//    }
-    aaWorldPrev = aaWorld;
   }
 }