Merge pull request #3 from ingen084/format-and-enable-adjust-button

コードフォーマットを改めて行いadjustボタンを有効化

include/Filter.hpp

@@ -123,6 +123,16 @@ class Filter
         }
     }
 
+    void reset()
+    {
+        for (int i = 0; i < 3; i++)
+        {
+            hpFilters[i].reset();
+            for (int j = 0; j < 6; j++)
+                filters[j][i].reset();
+        }
+    }
+
     void filterForShindo(float *newSample)
     {
         for (int i = 0; i < 3; i++)

include/JmaIntensity.hpp

@@ -1,5 +1,4 @@
-#if !defined(PIDAS_JMA_INTENSITY_HPP)
-#define PIDAS_JMA_INTENSITY_HPP
+#pragma once
 
 typedef enum
 {
@@ -17,27 +16,44 @@ typedef enum
 
 JmaIntensity getJmaIntensity(float rawIntensity)
 {
-    if (rawIntensity >= 6.5) {
+    if (rawIntensity >= 6.5)
+    {
         return JMA_INT_7;
-    } else if (rawIntensity >= 6.0) {
+    }
+    else if (rawIntensity >= 6.0)
+    {
         return JMA_INT_6_UPPER;
-    } else if (rawIntensity >= 5.5) {
+    }
+    else if (rawIntensity >= 5.5)
+    {
         return JMA_INT_6_LOWER;
-    } else if (rawIntensity >= 5.0) {
+    }
+    else if (rawIntensity >= 5.0)
+    {
         return JMA_INT_5_UPPER;
-    } else if (rawIntensity >= 4.5) {
+    }
+    else if (rawIntensity >= 4.5)
+    {
         return JMA_INT_5_LOWER;
-    } else if (rawIntensity >= 3.5) {
+    }
+    else if (rawIntensity >= 3.5)
+    {
         return JMA_INT_4;
-    } else if (rawIntensity >= 2.5) {
+    }
+    else if (rawIntensity >= 2.5)
+    {
         return JMA_INT_3;
-    } else if (rawIntensity >= 1.5) {
+    }
+    else if (rawIntensity >= 1.5)
+    {
         return JMA_INT_2;
-    } else if (rawIntensity >= 0.5) {
+    }
+    else if (rawIntensity >= 0.5)
+    {
         return JMA_INT_1;
-    } else {
+    }
+    else
+    {
         return JMA_INT_0;
     }
 }
-
-#endif

include/LED.hpp

@@ -1,10 +1,9 @@
+#pragma once
+
 #include <Arduino.h>
 
 #include "JmaIntensity.hpp"
 
-#if !defined(PIDAS_PLUS_LED_HPP)
-#define PIDAS_PLUS_LED_HPP
-
 #define PIDAS_PLUS_LED_PIN_OFFSET D6
 #define PIDAS_PLUS_LED_COUNT 10
 #define PIDAS_PLUS_LED_CHECK_COUNT(i)           \
@@ -81,5 +80,3 @@ class Led
             off(i);
     }
 };
-
-#endif

include/MCP3204.hpp

@@ -1,3 +1,5 @@
+#pragma once
+
 #include <Arduino.h>
 #include <SPI.h>
 
@@ -26,12 +28,12 @@ class MCP3204
 
         digitalWrite(csPin, LOW);
         SPI.beginTransaction(spiSettings);
-        (void) SPI.transfer(0x06 | (ch >> 2));
+        (void)SPI.transfer(0x06 | (ch >> 2));
         t.msb = SPI.transfer(0xff & (ch << 6));
         t.lsb = SPI.transfer(0);
         SPI.endTransaction();
         digitalWrite(csPin, HIGH);
-        
+
         return t.val;
     }
 

include/SimpleIir.hpp

@@ -3,20 +3,20 @@
 class SimpleIir
 {
 private:
-	int coefALen;
-	float *coefsA;
-	int coefBLen;
-	float *coefsB;
-	float *dlyX;
-	float *dlyY;
+    int coefALen;
+    float *coefsA;
+    int coefBLen;
+    float *coefsB;
+    float *dlyX;
+    float *dlyY;
 
 public:
     SimpleIir() {}
     SimpleIir(int coefBLen, float *coefsB, int coefALen, float *coefsA)
     {
         /* memory allocation for H and delay values */
         dlyX = new float[coefBLen];
-        dlyY = new float[coefBLen];
+        dlyY = new float[coefALen];
         /* init parameters */
         this->coefBLen = coefBLen;
         this->coefALen = coefALen;
@@ -28,6 +28,14 @@ class SimpleIir
             dlyY[0] = 0.0;
     }
 
+    void reset()
+    {
+        for (int i = 0; i < coefBLen; i++)
+            dlyX[i] = 0.0;
+        for (int j = 0; j < coefALen; j++)
+            dlyY[0] = 0.0;
+    }
+
     float filter(float input)
     {
         float acc1 = 0.0;
@@ -36,17 +44,17 @@ class SimpleIir
         dlyX[0] = input;
         for (int i = 0; i < coefBLen; i++)
             acc1 += coefsB[i] * dlyX[i];
-        for (int i = (coefBLen) - 1; i > 0; i--)
-            dlyX[i] = dlyX[i - 1];    
+        for (int i = (coefBLen)-1; i > 0; i--)
+            dlyX[i] = dlyX[i - 1];
         /* a coeficients*/
-        
+
         for (int i = 1; i < coefALen; i++)
             acc1 -= coefsA[i] * dlyY[i];
 
-        dlyY[0] = (acc1+acc2)/coefsA[0];
-        
-        for (int i = (coefALen) - 1; i > 0; i--)
-            dlyY[i] = dlyY[i - 1];  
+        dlyY[0] = (acc1 + acc2) / coefsA[0];
+
+        for (int i = (coefALen)-1; i > 0; i--)
+            dlyY[i] = dlyY[i - 1];
 
         return dlyY[0];
     }

src/main.cpp

@@ -67,9 +67,9 @@ void printNmea(const char *format, ...)
 }
 
 // 3-axis adjust value
-float offset[3];
+uint16_t offset[3];
 // Adujusting time( offset_counter / sampling rate = sec)
-int offsetCounter = bufferSize * 5;
+int offsetCounter = bufferSize * 1;
 bool isOffsetted = false;
 
 // オフセットを計算する
@@ -128,11 +128,7 @@ void loop()
 
     // オフセット計算
     if (!isOffsetted)
-    {
         calcOffset(rawData);
-        printNmea("XSOFF,0");
-        isOffsetted = true;
-    }
 
     float offsetSample[3];
     float newHPFilteredSample[3];
@@ -217,9 +213,15 @@ void loop()
     if (digitalRead(ADJUST_PIN) && isOffsetted)
     {
         isOffsetted = false;
-        offsetCounter = samplingRate * 5;
+        offsetCounter = samplingRate * 2;
         latestMaxTime = micros();
-        maxIntensity = JMA_INT_0;
         printNmea("XSOFF,1");
     }
+    else if (!isOffsetted && offsetCounter-- <= 0)
+    {
+        maxIntensity = JMA_INT_0;
+        FILTER.reset();
+        isOffsetted = true;
+        printNmea("XSOFF,0");
+    }
 }