Fault handling

ryodine · May 13, 2020 · a280737 · a280737
1 parent ed8008d
commit a280737
Show file tree

Hide file tree

Showing 3 changed files with 254 additions and 16 deletions.
diff --git a/FaultHandling.cpp b/FaultHandling.cpp
@@ -0,0 +1,140 @@
+#include "FaultHandling.h"
+#include "Arduino.h"
+
+Fault::Handler* Fault::Handler::inst = 0;
+
+Fault::Handler* Fault::Handler::instance()
+{
+  if (inst == 0) {
+    inst = new Handler();
+  }
+  return inst;
+}
+
+Fault::Handler::Handler()
+{
+  for (int i = 0; i < ALL_OK; i++) {
+    faults[i] = false;
+  }
+}
+
+void Fault::Handler::setFaultCode(Type fault)
+{
+  if (!this->faults[fault]) {
+    this->faults[fault] = true;
+    this->minorFaultState=0;
+    this->lastTs = millis();
+  }
+}
+
+void Fault::Handler::unlatchFaultCode(Type fault)
+{
+  this->faults[fault] = false;
+}
+
+bool Fault::Handler::hasFault(Type fault)
+{
+  return this->faults[fault];
+
+}
+
+bool Fault::Handler::hasFaultOfType(Type start, Type end)
+{
+  for (int i = start; i < end; i++) {
+    if (hasFault((Type) i)) {
+      return true;
+    }
+  }
+  return false;
+}
+
+int Fault::Handler::numFaults()
+{
+  int count = 0;
+  for (int i = 0; i < ALL_OK; i++) {
+    if (faults[i] == true) {
+      count++;
+    }
+  }
+  return count;
+}
+
+int Fault::Handler::nextFault(Type start)
+{
+  for (int i = start; i < ALL_OK; i++) {
+    if (faults[i] == true) {
+      return i;
+    }
+  }
+  return -1;
+}
+
+
+void Fault::Handler::printFaultReport()
+{
+  Serial.println("FAULT REPORT:");
+  for (int i = ZERO+1; i < FATAL_END_SENTINEL; i++) {
+    Serial.print("Fatal Fault #");
+    Serial.print(i);
+    Serial.print(": ");
+    Serial.println(faults[i]);
+  }
+  for (int i = FATAL_END_SENTINEL+1; i < RETRYABLE_END_SENTINEL; i++) {
+    Serial.print("Recoverable Fault #");
+    Serial.print(i);
+    Serial.print(": ");
+    Serial.println(faults[i]);
+  }
+}
+
+void Fault::Handler::faultFlasherPeriodic(const int LEDPIN)
+{
+  if (!hasFault()) {
+    digitalWrite(LEDPIN, HIGH);
+    return;
+  }
+
+  if (hasMajorFault()) {
+    int fault = nextFault(ZERO);
+    while (true) {
+      for (int i = 0; i < 5; i++) {
+        digitalWrite(LEDPIN, LOW);
+        delay(100);
+        digitalWrite(LEDPIN, HIGH);
+        delay(200);
+        digitalWrite(LEDPIN, LOW);
+        delay(100);
+      }
+      for (int i = 0; i < fault; i++) {
+        digitalWrite(LEDPIN, LOW);
+        delay(100);
+        digitalWrite(LEDPIN, HIGH);
+        delay(1000);
+        digitalWrite(LEDPIN, LOW);
+        delay(100);
+      }
+      printFaultReport();
+    }
+  }
+
+  if (hasMinorFault()) {
+    int fault = nextFault(FATAL_END_SENTINEL);
+    unsigned long millisDiff = millis() - lastTs;
+    if (millisDiff > 100 || millisDiff < 0) {
+      minorFaultState++;
+      lastTs = millis();
+    }
+    int fs = minorFaultState % (fault*6+4);
+    if (fs < 4) {
+      digitalWrite(LEDPIN, fs%2 == 0);
+    }
+
+    if (fs >= 4) {
+      digitalWrite(LEDPIN, fs%6 <= 4);
+    }
+
+    if (fs == (fault*6+3)) {
+      printFaultReport();
+    }
+  }
+}
diff --git a/FaultHandling.h b/FaultHandling.h
@@ -0,0 +1,41 @@
+#ifndef POOL_FAULT_HANDLING_H
+#define POOL_FAULT_HANDLING_H
+
+namespace Fault {
+  enum Type {
+    ZERO,
+    ACCEL_INIT,
+    FRAM_INIT,
+    FATAL_END_SENTINEL,
+    ACCEL_NOT_READY,
+    ACCEL_IMPLAUSIBLE_READING,
+    ACCEL_PARITY_FAILURE,
+    RETRYABLE_END_SENTINEL,
+    ALL_OK
+  };
+
+  class Handler {
+    public:
+      static Handler* instance();
+      void setFaultCode(Type fault);
+      bool hasMinorFault() { return hasFaultOfType(FATAL_END_SENTINEL, ALL_OK); };
+      bool hasMajorFault() { return hasFaultOfType(ZERO, FATAL_END_SENTINEL); };
+      bool hasFault() { return hasFaultOfType(ZERO, ALL_OK); };
+      void faultFlasherPeriodic(const int LEDPIN);
+      void unlatchFaultCode(Type fault);
+    private:
+      bool faults[ALL_OK];
+      static Handler* inst;
+      int minorFaultState = 0;
+      unsigned long lastTs;
+
+      bool hasFault(Type fault);
+      bool hasFaultOfType(Type start, Type end);
+      int numFaults();
+      int nextFault(Type start);
+      void printFaultReport();
+      Handler();
+  };
+};
+
+#endif
diff --git a/POOL_PLC.ino b/POOL_PLC.ino
@@ -7,6 +7,7 @@
 #include "AccelerometerFiltering.h"
 #include "AccelerometerModel.h"
 #include "ADXL355.h"
+#include "FaultHandling.h"
 #include "HighestCornerAlgorithm.h"
 
 #define STATUS_FLASH_PIN CONTROLLINO_D0
@@ -17,12 +18,19 @@
 #define OUT_BL CONTROLLINO_D18
 #define OUT_BR CONTROLLINO_D19
 
+Fault::Handler* faultHandler;
 ADXL355 accel(CONTROLLINO_D4);
-AccelerometerFilterMovingAverage ewmaFilter(0, 0.15);
+AccelerometerFilterMovingAverage ewmaFilter(0, 0.05);
 AccelerometerModel accelModel;
 HighestCornerAlgo cornerAlgo(2.5 / 180.0 * PI, 5.0 / 180.0 * PI);
 
+// Accelerometer implausibility check constants and counters
+const int max_accel_retries = 10;
+const double accel_max_gravity_deviation_fraction = 0.5;
+int accelBusyCounter = 0;
+
 void setup() {
+  faultHandler = Fault::Handler::instance();
   // put your setup code here, to run once:
   pinMode(STATUS_FLASH_PIN, OUTPUT);
   pinMode(OUT_TR, OUTPUT);
@@ -36,49 +44,98 @@ void setup() {
   SPI.begin();
   Serial.begin(9600);
   // 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);
-    delay(100);
-    Serial.println("ADXL355 Failed to initialize");
+  if (!accel.begin(ADXL355_RANGE_2G, ADXL355_FILTER_LPF_16HZ_ODR)) {
+    faultHandler->setFaultCode(Fault::ACCEL_INIT);
   }
-  Serial.println("ADXL355 Initialized");
 
   delay(200);
 
   //Allows a preconfigured offset (useful for YAW)
   //All rotations are in Roll-Pitch-Yaw (Left to right).
   accelModel.setBaseFrameAnglesRadians(Eigen::Vector3d(0,0,0));
+
 }
 
 void loop() {
+
+  // This just runs the periodic error flasher code
+  faultHandler->faultFlasherPeriodic(STATUS_FLASH_PIN);
+
   ADXL355Measurement measure;
+
+  // Check if there is data ready on the accelerometer
   if (accel.dataReady()) {
+
+    // If the accelerometer has data ready, then we can safely unlatch & reset the no data ready fault
+    faultHandler->unlatchFaultCode(Fault::ACCEL_NOT_READY);
+    accelBusyCounter = 0;
+
+    // Sample the accelerometer
     accel.takeSample();
-
     measure = accel.getSample();
+
+    // Magnitude plausibility check
+    magnitudePlausibilityCheck(measure);
+
+    // Filter the data with an Expoentially Weighted Moving Average
     ewmaFilter.addData(measure);
     measure = ewmaFilter.getAverage();
-
+
+    // Calculate the angles from the data using the mathematical model
     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();
-
+
+    // Pass the calculated angles into the highest corner finding algorithm
     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));
+
+    // Control the solenoids, if no faults
+    if (!faultHandler->hasFault()) {
+      digitalWrite(OUT_TR, cornerAlgo.getCorner(0));
+      digitalWrite(OUT_TL, cornerAlgo.getCorner(1));
+      digitalWrite(OUT_BL, cornerAlgo.getCorner(2));
+      digitalWrite(OUT_BR, cornerAlgo.getCorner(3));
+    }
+
+  } else {
+    // If there isn't data ready for max_accel_retries tries, then register an temporary fault.
+    if (accelBusyCounter++ > max_accel_retries) {
+      faultHandler->setFaultCode(Fault::ACCEL_NOT_READY);
+    }
   }
 
+  // if there are faults, do not control the solenoids.
+  if (faultHandler->hasFault()) {
+    digitalWrite(OUT_TR, LOW);
+    digitalWrite(OUT_TL, LOW);
+    digitalWrite(OUT_BL, LOW);
+    digitalWrite(OUT_BR, LOW);
+  }
+
+  // Check if the user wanted to zero the accelerometers
   if (!digitalRead(ZERO_BUTTON)) {
     accelModel.setMeasurementAsZero(Eigen::Vector3d(measure.x, measure.y, measure.z));
   }
 
   // avoid thrashing the sensor too much
   delay(10);
 }
+
+/**
+ * Checks that the accelerometer is outputing values that are plausible,
+ * by checking that the magnitude of the force vector is close to gravity.
+ * Sets the fault ACCEL_IMPLAUSIBLE_READING if there is an error
+ * 
+ * @param measure the measurement from the accelerometer
+ */
+void magnitudePlausibilityCheck(ADXL355Measurement measure)
+{
+  double magnitude = sqrt(pow(measure.x/16.0, 2) + pow(measure.y/16.0, 2) + pow(measure.z/16.0, 2));
+  if (magnitude > 1.0+accel_max_gravity_deviation_fraction || magnitude < 1.0-accel_max_gravity_deviation_fraction) {
+    faultHandler->setFaultCode(Fault::ACCEL_IMPLAUSIBLE_READING);
+  } else {
+    faultHandler->unlatchFaultCode(Fault::ACCEL_IMPLAUSIBLE_READING);
+  }
+}