diff --git a/earthquake/earthquake.ino b/earthquake/earthquake.ino index fe839490d3e776ad1da087991e146513ac67dbe6..4cca062b2b3fe0fb253f812dc13beb57e308180c 100644 --- a/earthquake/earthquake.ino +++ b/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; } }