Index: firmware/App/Controllers/BalancingChamber.c
===================================================================
diff -u -ra00cc8561b1916c257c9e9dbd6934de002ecdd25 -r2c1f48366677def3a01d9a96f80cb3419a3b7674
--- firmware/App/Controllers/BalancingChamber.c	(.../BalancingChamber.c)	(revision a00cc8561b1916c257c9e9dbd6934de002ecdd25)
+++ firmware/App/Controllers/BalancingChamber.c	(.../BalancingChamber.c)	(revision 2c1f48366677def3a01d9a96f80cb3419a3b7674)
@@ -25,6 +25,7 @@
 #include "FpgaDD.h"
 #include "Heaters.h"
 #include "Level.h"
+#include "MessagePayloads.h"
 #include "MessageSupport.h"
 #include "Messaging.h"
 #include "MixingControl.h"
@@ -119,6 +120,13 @@
 static U32 bcSwitchingBasedOnClosedPeriodCounter;                                               ///< Valve-close segments remaining before first-cycle relaxations clear after Qd timing apply.
 static F32 balancingError;                                                                      ///< Balancing error that has been calculated during balancing chamber switching.
 static F32 balChamberTolSwitchTime;                                                             ///< Balancing chamber allowed tolerance time before we switch to next state.
+static U32 detectTolCount;
+static U32 maxDetectCount;
+static U32 minDetectCount;
+static BOOL isDetectionInCountWindow;
+static F32 riseDeltaPsi;
+static BOOL riseHit;
+static U32 requiredRiseCount;
 
 //TODO: remove later once level sensor working
 static U32 bicarbChamberPeriodicFillCounter;
@@ -844,8 +852,6 @@
     isBalChamberFillInProgress          = FALSE;
     isPressureStabilizedDuringFill      = FALSE;
     isPressureDroppedDuringFill         = FALSE;
-    lastPrevSpentDialPressure           = 0.0F;
-    prevSpentDialPressure               = 0.0F;
     currentBalChamberSwitchingCounter   = 0;
     currentBalChamberFillCounter        = 0;
     spentFillRiseHitCount               = 0;
@@ -866,6 +872,8 @@
 
     freshDialPressure         = getFilteredPressure( D18_PRES );
     spentDialPressure         = getFilteredPressure( D51_PRES );
+    lastPrevSpentDialPressure = spentDialPressure;
+    prevSpentDialPressure     = spentDialPressure;
 
     if ( getTestConfigStatus( TEST_CONFIG_DD_FP_ENABLE_BETA_1_0_HW ) == TRUE )
     {
@@ -1158,8 +1166,6 @@
     isPressureStabilizedDuringFill      = FALSE;
     isPressureDroppedDuringFill         = FALSE;
     balChamberSWState                   = BAL_CHAMBER_SW_STATE2;
-    lastPrevSpentDialPressure           = 0.0F;
-    prevSpentDialPressure               = 0.0F;
     currentBalChamberSwitchingCounter   = 0;
     currentBalChamberFillCounter        = 0;
     spentFillRiseHitCount               = 0;
@@ -1179,6 +1185,8 @@
     }
     freshDialPressure         = getFilteredPressure( D18_PRES );
     spentDialPressure         = getFilteredPressure( D51_PRES );
+    lastPrevSpentDialPressure = spentDialPressure;
+    prevSpentDialPressure     = spentDialPressure;
 
     if ( getTestConfigStatus( TEST_CONFIG_DD_FP_ENABLE_BETA_1_0_HW ) == TRUE )
     {
@@ -1469,48 +1477,47 @@
     BOOL state;
 
     // Gate slope hits near expected valve-close count (current vs balChamberValveClosePeriod +/- tolerance).
-    U32 detectTolCount            = (U32)( (F32)balChamberValveClosePeriod * SPENT_FILL_DETECT_COUNT_TOL_PCT );
-    U32 maxDetectCount            = balChamberValveClosePeriod + detectTolCount;
-    U32 minDetectCount            = balChamberValveClosePeriod - detectTolCount;
-    BOOL isDetectionInCountWindow = ( ( currentBalChamberFillCounter >= minDetectCount ) &&
-                                      ( currentBalChamberFillCounter <= maxDetectCount ) ) ? TRUE : FALSE;
-    F32 riseDeltaPsi              = spentDialPressure - lastPrevSpentDialPressure;
-    BOOL riseHit                  = ( riseDeltaPsi > riseThresholdPsig ) ? TRUE : FALSE;
-    U32 requiredRiseCount;
+    detectTolCount           = (U32)( (F32)balChamberValveClosePeriod * SPENT_FILL_DETECT_COUNT_TOL_PCT );
+    maxDetectCount           = balChamberValveClosePeriod + detectTolCount;
+    minDetectCount           = balChamberValveClosePeriod - detectTolCount;
+    isDetectionInCountWindow = ( ( currentBalChamberFillCounter >= minDetectCount ) &&
+                                 ( currentBalChamberFillCounter <= maxDetectCount ) ) ? TRUE : FALSE;
+    riseDeltaPsi             = spentDialPressure - lastPrevSpentDialPressure;
+    riseHit                  = ( riseDeltaPsi > riseThresholdPsig ) ? TRUE : FALSE;
 
     // Mid band between low/high spent pressure rise: require two rise hits.
     if ( riseDeltaPsi > SPENT_FILL_SLOPE_SPENT_PRESSURE_HIGH_PSIG )
     {
         requiredRiseCount = 1U;
     }
-//    else
-//    {
-//        requiredRiseCount = 2U;
-//    }
+    else
+    {
+        requiredRiseCount = 2U;
+    }
 
     if ( ( TRUE == riseHit ) && ( TRUE == isDetectionInCountWindow ) )
     {
         spentFillRiseHitCount += 1;
         spentFillRiseMissCounter = 0;
     }
-//    else if ( spentFillRiseHitCount > 0 )
-//    {
-//        if ( ( FALSE == riseHit ) && ( TRUE == isDetectionInCountWindow ) )
-//        {
-//            spentFillRiseMissCounter += 1;
-//
-//            if ( spentFillRiseMissCounter >= 2 )
-//            {
-//                spentFillRiseHitCount    = 0;
-//                spentFillRiseMissCounter = 0;
-//            }
-//        }
-//    }
+    else if ( spentFillRiseHitCount > 0 )
+    {
+        if ( ( FALSE == riseHit ) && ( TRUE == isDetectionInCountWindow ) )
+        {
+            spentFillRiseMissCounter += 1;
 
+            if ( spentFillRiseMissCounter >= 2 )
+            {
+                spentFillRiseHitCount    = 0;
+                spentFillRiseMissCounter = 0;
+            }
+        }
+    }
+
     lastPrevSpentDialPressure = prevSpentDialPressure;
     prevSpentDialPressure     = spentDialPressure;
 
-    state = ( ( spentDialPressure >= BAL_CHAMBER_ABSOLUTE_SPENT_PRESSURE ) || ( spentFillRiseHitCount >= requiredRiseCount ) || ( currentBalChamberFillCounter >= balChamberTolSwitchTime ) ) ? TRUE : FALSE;
+    state = ( spentFillRiseHitCount >= requiredRiseCount ) ? TRUE : FALSE;
     return state;
 }
 
@@ -1550,6 +1557,9 @@
 
         if ( ( TRUE == isSpentFillCompleteDetected ) && ( isSpentFillComplete != TRUE ) )
         {
+
+            isSpentFillComplete = TRUE;
+
             // Check spent fill time against the balancing chamber closing period
             if ( balChamberValveClosePeriod > 0 )
             {
@@ -1584,11 +1594,11 @@
                 }
             }
 
-            //Update spent fill is complete
-            if ( currentBalChamberSwitchingCounter >= balChamberValveClosePeriod )
-            {
-                isSpentFillComplete = TRUE;
-            }
+//            //Update spent fill is complete
+//            if ( currentBalChamberSwitchingCounter >= balChamberValveClosePeriod )
+//            {
+//                isSpentFillComplete = TRUE;
+//            }
         }
     }
 }
@@ -1855,6 +1865,18 @@
         data.isPressureStabilizedDuringFill     = isPressureStabilizedDuringFill;
         data.balChamberSWOnlyState              = balanceChamberSwitchingOnly;
         data.isBalChamberSwitchingActive        = isBalChamberSwitchingActive;
+#ifdef __BAL_CHAMBER_DEBUG__
+        data.detectTolCount                     = detectTolCount;
+        data.maxDetectCount                     = maxDetectCount;
+        data.minDetectCount                     = minDetectCount;
+        data.isDetectionInCountWindow           = isDetectionInCountWindow;
+        data.riseDeltaPsi                       = riseDeltaPsi;
+        data.riseHit                            = riseHit;
+        data.requiredRiseCount                  = requiredRiseCount;
+        data.spentFillRiseHitCount              = spentFillRiseHitCount;
+        data.lastPrevSpentDialPressure          = lastPrevSpentDialPressure;
+        data.prevSpentDialPressure              = prevSpentDialPressure;
+#endif
 
         broadcastData( MSG_ID_DD_BAL_CHAMBER_DATA, COMM_BUFFER_OUT_CAN_DD_BROADCAST, (U08*)&data, sizeof( BAL_CHAMBER_DATA_T ) );