Status checking and more filters

ryodine · May 5, 2020 · ed8008d · ed8008d
1 parent 7ea6df4
commit ed8008d
Show file tree

Hide file tree

Showing 3 changed files with 46 additions and 25 deletions.
diff --git a/ADXL355.cpp b/ADXL355.cpp
@@ -57,7 +57,7 @@ boolean ADXL355::begin(byte range, byte filter=ADXL355_FILTER_OFF)
 }
 
 void ADXL355::takeSample()
-{
+{ 
   byte bytebuf[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
   spi_multibyte_read(bytebuf, 9, ADXL355__REG_ACCELEROMETER_DATA_BEGIN);
 
@@ -88,3 +88,8 @@ ADXL355Measurement ADXL355::getSample()
 
   return measure;
 }
+
+byte ADXL355::getStatus()
+{
+  return spi_readbyte(ADXL255__REG_STATUS);
+}
diff --git a/ADXL355.h b/ADXL355.h
@@ -6,6 +6,7 @@
 // READ registers
 #define ADXL355__REG_XDATA3 0x08
 #define ADXL355__REG_ACCELEROMETER_DATA_BEGIN ADXL355__REG_XDATA3
+#define ADXL255__REG_STATUS 0x04
 
 // WRITE Registers
 #define ADXL355__REG_POWER_CONTROL 0x2D
@@ -23,7 +24,15 @@
 #define ADXL355_RANGE_4G 0x02
 #define ADXL355_RANGE_8G 0x03
 
-#define ADXL355_FILTER_LPF_LOW_FREQ 0b00001010
+#define ADXL355_STATUS_DATA_READY 0b00000001
+#define ADXL355_STATUS_FIFO_FULL 0b00000010
+#define ADXL355_STATUS_FIFO_OVR 0b00000100
+#define ADXL355_STATUS_ACTIVITY 0b00001000
+#define ADXL355_STATUS_NVM_BUSY 0b00010000
+
+#define ADXL355_FILTER_LPF_4HZ_ODR 0b00001010
+#define ADXL355_FILTER_LPF_8HZ_ODR 0b00001001
+#define ADXL355_FILTER_LPF_16HZ_ODR 0b00001000
 #define ADXL355_FILTER_OFF 0x000000
 
 typedef struct{
@@ -48,6 +57,8 @@ class ADXL355 {
       : chipselect(cs), settings(SPISettings(speed, MSBFIRST, SPI_MODE0)) {};
     boolean begin(byte range, byte filter=ADXL355_FILTER_OFF);
     void takeSample();
+    byte getStatus();
+    bool dataReady() { return getStatus() & ADXL355_STATUS_DATA_READY; };
     ADXL355Measurement getSample();
 };
 

diff --git a/POOL_PLC.ino b/POOL_PLC.ino
@@ -18,7 +18,7 @@
 #define OUT_BR CONTROLLINO_D19
 
 ADXL355 accel(CONTROLLINO_D4);
-AccelerometerFilterMovingAverage ewmaFilter(0, 0.25);
+AccelerometerFilterMovingAverage ewmaFilter(0, 0.15);
 AccelerometerModel accelModel;
 HighestCornerAlgo cornerAlgo(2.5 / 180.0 * PI, 5.0 / 180.0 * PI);
 
@@ -35,7 +35,8 @@ void setup() {
 
   SPI.begin();
   Serial.begin(9600);
-  while (!accel.begin(ADXL355_RANGE_2G, ADXL355_FILTER_LPF_LOW_FREQ)) {
+  // ADXL355_FILTER_LPF_LOW_FREQ
+  while (!accel.begin(ADXL355_RANGE_2G, ADXL355_FILTER_LPF_16HZ_ODR)) {
     digitalWrite(STATUS_FLASH_PIN, LOW);
     delay(100);
     digitalWrite(STATUS_FLASH_PIN, HIGH);
@@ -52,28 +53,32 @@ void setup() {
 }
 
 void loop() {
-  // put your main code here, to run repeatedly:
-  accel.takeSample();
-
-  ADXL355Measurement measure = accel.getSample();
-  ewmaFilter.addData(measure);
-  measure = ewmaFilter.getAverage();
-
-  Eigen::Vector2d angles = accelModel.calculate(Eigen::Vector3d(measure.x, measure.y, measure.z));
-  Serial.print(angles[0] * 180.0 / PI);
-  Serial.print("\t");
-  Serial.print(angles[1] * 180.0 / PI);
-  Serial.println();
-
-  cornerAlgo.update(angles[0], angles[1]);
-
-  digitalWrite(OUT_TR, cornerAlgo.getCorner(0));
-  digitalWrite(OUT_TL, cornerAlgo.getCorner(1));
-  digitalWrite(OUT_BL, cornerAlgo.getCorner(2));
-  digitalWrite(OUT_BR, cornerAlgo.getCorner(3));
+  ADXL355Measurement measure;
+  if (accel.dataReady()) {
+    accel.takeSample();
+
+    measure = accel.getSample();
+    ewmaFilter.addData(measure);
+    measure = ewmaFilter.getAverage();
+
+    Eigen::Vector2d angles = accelModel.calculate(Eigen::Vector3d(measure.x, measure.y, measure.z));
+    Serial.print(angles[0] * 180.0 / PI);
+    Serial.print("\t");
+    Serial.print(angles[1] * 180.0 / PI);
+    Serial.println();
+
+    cornerAlgo.update(angles[0], angles[1]);
+
+    digitalWrite(OUT_TR, cornerAlgo.getCorner(0));
+    digitalWrite(OUT_TL, cornerAlgo.getCorner(1));
+    digitalWrite(OUT_BL, cornerAlgo.getCorner(2));
+    digitalWrite(OUT_BR, cornerAlgo.getCorner(3));
+  }
 
   if (!digitalRead(ZERO_BUTTON)) {
-    //accelModel.setMeasurementAsZero(Eigen::Vector3d(measure.x, measure.y, measure.z));
+    accelModel.setMeasurementAsZero(Eigen::Vector3d(measure.x, measure.y, measure.z));
   }
-  delay(50);
+
+  // avoid thrashing the sensor too much
+  delay(10);
 }