Added criteria voltage of 2 V means ohmic sense hit

Added criteria voltage drop of 2V over the 3 last cycles also means sense hit.

Cards/EaserCAT-7000-DIO+I2C/Firmware/src/main.cpp

@@ -42,6 +42,22 @@ void ads1014_reset(ADS1014 *ads) {
   ads->readADC_Differential_0_1();  // This is the value we are interested in
 }
 
+#include <queue>
+class OhmicSensing {
+ public:
+  void handle(uint8_t voltageState, float inVoltage, float limitVoltage,
+              uint32_t setupTime, uint8_t enabled, uint8_t &sensed);
+
+ private:
+  enum OhmicStates { OHMIC_IDLE, OHMIC_SETUP, OHMIC_PROBE };
+  OhmicStates ohmicState = OHMIC_IDLE;
+  uint32_t setupTimeSoFar = 0;
+  float_t oldVoltage = 0.0;
+  std::queue<float> voltages;
+};
+OhmicSensing Ohm1;
+OhmicSensing Ohm2;
+
 void handleVoltageReader(float scale_in, float offset, float &outVoltage,
                          int32_t &outRaw, float &oldVoltage, float &oldRaw,
                          uint8_t devType, int8_t &old_devType,
@@ -54,10 +70,7 @@ void lowpassFilter(float &oldLowPassGain,
                    uint32_t LowpassFilterPoleFrequency,
                    float LowPassFilterThresholdVoltage, float inVoltage,
                    float &outFilteredVoltage);
-void handleOhmicSensing(uint8_t &ohmicState, uint8_t voltageState,
+
-                        float inVoltage, float limitVoltage, uint32_t setupTime,
-                        uint32_t &setupTimeSoFar, uint8_t enabled,
-                        uint8_t &sensed);
 #define bitset(byte, nbit) ((byte) |= (1 << (nbit)))
 #define bitclear(byte, nbit) ((byte) &= ~(1 << (nbit)))
 #define bitflip(byte, nbit) ((byte) ^= (1 << (nbit)))
@@ -113,24 +126,14 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
                 Obj.Out_Unit2.CalculatedVoltage,
                 Obj.Out_Unit2.LowpassFilteredVoltage);
 
-#define OHMIC_IDLE 0
+  Ohm1.handle(
-#define OHMIC_SETUP 1
+      stat_1, Obj.Out_Unit1.CalculatedVoltage,
-#define OHMIC_PROBE 2
+      Obj.In_Unit1.OhmicSensingVoltageLimit, Obj.In_Unit1.OhmicSensingSetupTime,
-  static uint8_t ohmicState_1 = OHMIC_IDLE;
+      Obj.In_Unit1.EnableOhmicSensing, Obj.Out_Unit1.OhmicSensingSensed);
-  static uint8_t ohmicState_2 = OHMIC_IDLE;
+  Ohm2.handle(
-  static uint32_t setupTimeSoFar_1 = 0;
+      stat_2, Obj.Out_Unit2.CalculatedVoltage,
-  static uint32_t setupTimeSoFar_2 = 0;
+      Obj.In_Unit2.OhmicSensingVoltageLimit, Obj.In_Unit2.OhmicSensingSetupTime,
-
+      Obj.In_Unit2.EnableOhmicSensing, Obj.Out_Unit2.OhmicSensingSensed);
-  handleOhmicSensing(ohmicState_1, stat_1, Obj.Out_Unit1.CalculatedVoltage,
-                     Obj.In_Unit1.OhmicSensingVoltageLimit,
-                     Obj.In_Unit1.OhmicSensingSetupTime, setupTimeSoFar_1,
-                     Obj.In_Unit1.EnableOhmicSensing,
-                     Obj.Out_Unit1.OhmicSensingSensed);
-  handleOhmicSensing(ohmicState_2, stat_2, Obj.Out_Unit2.CalculatedVoltage,
-                     Obj.In_Unit2.OhmicSensingVoltageLimit,
-                     Obj.In_Unit2.OhmicSensingSetupTime, setupTimeSoFar_2,
-                     Obj.In_Unit2.EnableOhmicSensing,
-                     Obj.Out_Unit2.OhmicSensingSensed);
 }
 
 void ESC_interrupt_enable(uint32_t mask);
@@ -318,8 +321,7 @@ void handleVoltageReader(float scale_in, float offset, float &outVoltage,
                          MyMCP3221 *&mcp, uint8_t I2C_address,
                          uint32_t &I2C_restarts) {
   float scale = scale_in;
-  if (scale == 0.0)
+  if (scale == 0.0) scale = 1.0;
-    scale = 1.0;
   int stat = 1, data0;
 
   switch (devType) {
@@ -393,8 +395,7 @@ void handleVoltageReader(float scale_in, float offset, float &outVoltage,
     Wire2.begin();
     Wire2.setClock(I2C_BUS_SPEED);
     I2C_restarts++;
-    if (devType == ADS1014_TYPE && ads != nullptr)
+    if (devType == ADS1014_TYPE && ads != nullptr) ads1014_reset(ads);
-      ads1014_reset(ads);
     // mcp3221 has no reset, reset the I2C bus is the best we can do
   }
   readStat = stat;
@@ -425,10 +426,9 @@ void lowpassFilter(float &oldLowPassGain,
   oldLowPassFilteredVoltage = outFilteredVoltage;
 }
 
-void handleOhmicSensing(uint8_t &ohmicState, uint8_t voltageState,
+void OhmicSensing::handle(uint8_t voltageState, float inVoltage,
-                        float inVoltage, float limitVoltage, uint32_t setupTime,
+                          float limitVoltage, uint32_t setupTime,
-                        uint32_t &setupTimeSoFar, uint8_t enabled,
+                          uint8_t enabled, uint8_t &sensed) {
-                        uint8_t &sensed) {
   sensed = 0;
   if (enabled && voltageState == 0) {
     if (ohmicState == OHMIC_IDLE && inVoltage > limitVoltage) {
@@ -438,11 +438,20 @@ void handleOhmicSensing(uint8_t &ohmicState, uint8_t voltageState,
     if (ohmicState == OHMIC_SETUP) {
       if (setupTimeSoFar++ > setupTime) {
         ohmicState = OHMIC_PROBE;
+        oldVoltage = 0.0;
+        while (!voltages.empty()) voltages.pop();  // Remove history
       }
     }
-    if (ohmicState == OHMIC_PROBE && inVoltage <= limitVoltage) {
+    if (ohmicState == OHMIC_PROBE) {
+      voltages.push(inVoltage);
+#define N_VOLTAGES 3
+      while (voltages.size() > N_VOLTAGES) voltages.pop();  // Only N_VOLTAGES
+      if (inVoltage <= limitVoltage || oldVoltage - inVoltage >= 1.0 ||
+          voltages.front() - voltages.back() > 2.0) {
         sensed = 1;
       }
+      oldVoltage = inVoltage;
+    }
   } else {
     ohmicState = OHMIC_IDLE;
   }