Index: firmware/App/Modes/SelfTests.c
===================================================================
diff -u -r5ce2656c662525e10acd1e55f16f7aca5ce9b4d6 -rabcf0f081f9c0bf3f3e0746d896e5b7c81f27a31
--- firmware/App/Modes/SelfTests.c	(.../SelfTests.c)	(revision 5ce2656c662525e10acd1e55f16f7aca5ce9b4d6)
+++ firmware/App/Modes/SelfTests.c	(.../SelfTests.c)	(revision abcf0f081f9c0bf3f3e0746d896e5b7c81f27a31)
@@ -1620,7 +1620,7 @@
     WET_SELF_TESTS_STATE_T state = WET_SELF_TESTS_FIRST_DISPLACEMENT_SETUP_STATE;
     DG_CMD_RESPONSE_T cmdResp;
 
-    if ( ( FALSE == isValvesSettingSent ) && ( TRUE == didTimeout( settleStartTime, RESERVOIR_SETTLE_TIME_MS ) ) )
+    /*if ( ( FALSE == isValvesSettingSent ) && ( TRUE == didTimeout( settleStartTime, RESERVOIR_SETTLE_TIME_MS ) ) )
     {
         reservoirVolume[ DG_RESERVOIR_1 ] = getReservoirWeightLargeFilter( DG_RESERVOIR_1 );
         reservoirVolume[ DG_RESERVOIR_2 ] = getReservoirWeightLargeFilter( DG_RESERVOIR_2 );
@@ -1629,6 +1629,15 @@
         setValvePosition( VDO, VALVE_POSITION_C_CLOSE );
         cmdChangeDGValveSetting( DG_VALVE_SETTING_R1_TO_R2 );
         isValvesSettingSent = TRUE;
+    }*/ // TODO remove this code if the new wet self test works better
+
+    if ( FALSE == isValvesSettingSent )
+    {
+        setValvePosition( VDI, VALVE_POSITION_C_CLOSE );
+        setValvePosition( VDO, VALVE_POSITION_C_CLOSE );
+        cmdChangeDGValveSetting( DG_VALVE_SETTING_R1_TO_R2 );
+        isValvesSettingSent = TRUE;
+        settleStartTime     = 0;
     }
 
     if ( ( TRUE == isValvesSettingSent ) && ( TRUE == getDGCommandResponse( DG_CMD_VALVE_SETTING, &cmdResp ) ) )
@@ -1639,9 +1648,9 @@
         if ( FALSE == cmdResp.rejected )
         {
             setDialInPumpTargetFlowRate( DIP_FLOW_RATE_FIRST_DISPLACEMENT_ML_MIN, MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_OPEN_LOOP );
-            displacementStartTime = getMSTimerCount();
-            fmdIntegratedVolume = 0.0;
-            state = WET_SELF_TESTS_FIRST_DISPLACEMENT_STATE;
+
+            settleStartTime = getMSTimerCount();
+            state           = WET_SELF_TESTS_FIRST_DISPLACEMENT_STATE;
         }
     }
 
@@ -1666,15 +1675,27 @@
 {
     WET_SELF_TESTS_STATE_T state = WET_SELF_TESTS_FIRST_DISPLACEMENT_STATE;
 
-    fmdIntegratedVolume += getMeasuredDialInFlowRate();
+    if ( ( TRUE == didTimeout( settleStartTime, 5000 ) ) && ( settleStartTime != 0 ) ) // TODO #define
+    {
+        reservoirVolume[ DG_RESERVOIR_1 ] = getReservoirWeight( DG_RESERVOIR_1 );
+        reservoirVolume[ DG_RESERVOIR_2 ] = getReservoirWeight( DG_RESERVOIR_2 );
+        displacementStartTime = getMSTimerCount();
+        fmdIntegratedVolume   = 0.0F;
+        settleStartTime       = 0;
+    }
 
-    if ( TRUE == didTimeout( displacementStartTime, WET_SELF_TEST_DISPLACEMENT_TIME_MS ) )
+    if ( 0 == settleStartTime )
     {
-        signalDialInPumpHardStop();
-        fmdIntegratedVolume = ( ( fmdIntegratedVolume * TASK_GENERAL_INTERVAL ) / ( SEC_PER_MIN * MS_PER_SECOND ) );
+        fmdIntegratedVolume += getMeasuredDialInFlowRate();
 
-        settleStartTime = getMSTimerCount();
-        state = WET_SELF_TESTS_FIRST_DISPLACEMENT_VERIFY_STATE;
+        if ( TRUE == didTimeout( displacementStartTime, WET_SELF_TEST_DISPLACEMENT_TIME_MS ) )
+        {
+            signalDialInPumpHardStop();
+            fmdIntegratedVolume = ( ( fmdIntegratedVolume * TASK_GENERAL_INTERVAL ) / ( SEC_PER_MIN * MS_PER_SECOND ) );
+
+            settleStartTime = getMSTimerCount();
+            state           = WET_SELF_TESTS_FIRST_DISPLACEMENT_VERIFY_STATE;
+        }
     }
 
     if ( TRUE == doesAlarmStatusIndicateStop() )
@@ -1698,10 +1719,10 @@
 {
     WET_SELF_TESTS_STATE_T state = WET_SELF_TESTS_FIRST_DISPLACEMENT_VERIFY_STATE;
 
-    if ( TRUE == didTimeout( settleStartTime, RESERVOIR_SETTLE_TIME_MS ) )
+    //if ( TRUE == didTimeout( settleStartTime, RESERVOIR_SETTLE_TIME_MS ) )
     {
-        F32 resOneDiffAfterDisplacement     = reservoirVolume[ DG_RESERVOIR_1 ] - getReservoirWeightLargeFilter( DG_RESERVOIR_1 );
-        F32 resTwoDiffAfterDisplacement     = getReservoirWeightLargeFilter( DG_RESERVOIR_2 ) - reservoirVolume[ DG_RESERVOIR_2 ];
+        F32 resOneDiffAfterDisplacement     = reservoirVolume[ DG_RESERVOIR_1 ] - getReservoirWeight( DG_RESERVOIR_1 ); //getReservoirWeightLargeFilter( DG_RESERVOIR_1 );
+        F32 resTwoDiffAfterDisplacement     = /*getReservoirWeightLargeFilter( DG_RESERVOIR_2 )*/ getReservoirWeight( DG_RESERVOIR_2 ) - reservoirVolume[ DG_RESERVOIR_2 ];
         F32 averageDisp                     = ( resOneDiffAfterDisplacement + resTwoDiffAfterDisplacement ) / 2.0F;
         F32 integratedVolumeDiff            = fabs( fmdIntegratedVolume - averageDisp );
         F32 integratedVolumeToTargetPercent = fabs( 1.0F - ( fmdIntegratedVolume / averageDisp ) );
@@ -1758,7 +1779,7 @@
     WET_SELF_TESTS_STATE_T state = WET_SELF_TESTS_SECOND_DISPLACEMENT_SETUP_STATE;
     DG_CMD_RESPONSE_T cmdResp;
 
-    if ( FALSE == isValvesSettingSent )
+    /*if ( FALSE == isValvesSettingSent )
     {
         reservoirVolume[ DG_RESERVOIR_1 ] = getReservoirWeightLargeFilter( DG_RESERVOIR_1 );
         reservoirVolume[ DG_RESERVOIR_2 ] = getReservoirWeightLargeFilter( DG_RESERVOIR_2 );
@@ -1767,6 +1788,15 @@
         setValvePosition( VDO, VALVE_POSITION_C_CLOSE );
         cmdChangeDGValveSetting( DG_VALVE_SETTING_R1_TO_R2 );
         isValvesSettingSent = TRUE;
+    }*/
+
+    if ( FALSE == isValvesSettingSent )
+    {
+        setValvePosition( VDI, VALVE_POSITION_C_CLOSE );
+        setValvePosition( VDO, VALVE_POSITION_C_CLOSE );
+        cmdChangeDGValveSetting( DG_VALVE_SETTING_R1_TO_R2 );
+        isValvesSettingSent = TRUE;
+        settleStartTime     = 0;
     }
 
     if ( ( TRUE == isValvesSettingSent ) && ( TRUE == getDGCommandResponse( DG_CMD_VALVE_SETTING, &cmdResp ) ) )
@@ -1777,8 +1807,10 @@
         if ( FALSE == cmdResp.rejected )
         {
             setDialInPumpTargetFlowRate( DIP_FLOW_RATE_SECOND_DISPLACEMENT_ML_MIN, MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_OPEN_LOOP );
-            displacementStartTime = getMSTimerCount();
-            fmdIntegratedVolume = 0.0;
+
+            settleStartTime = getMSTimerCount();
+            //displacementStartTime = getMSTimerCount();
+            //fmdIntegratedVolume = 0.0;
             state = WET_SELF_TESTS_SECOND_DISPLACEMENT_STATE;
         }
     }
@@ -1804,8 +1836,31 @@
 {
     WET_SELF_TESTS_STATE_T state = WET_SELF_TESTS_SECOND_DISPLACEMENT_STATE;
 
-    fmdIntegratedVolume += getMeasuredDialInFlowRate();
+    if ( ( TRUE == didTimeout( settleStartTime, 5000 ) ) && ( settleStartTime != 0 ) ) // TODO #define
+    {
+        reservoirVolume[ DG_RESERVOIR_1 ] = getReservoirWeight( DG_RESERVOIR_1 );
+        reservoirVolume[ DG_RESERVOIR_2 ] = getReservoirWeight( DG_RESERVOIR_2 );
+        displacementStartTime = getMSTimerCount();
+        fmdIntegratedVolume   = 0.0F;
+        settleStartTime       = 0;
+    }
 
+    if ( 0 == settleStartTime )
+    {
+        fmdIntegratedVolume += getMeasuredDialInFlowRate();
+
+        if ( TRUE == didTimeout( displacementStartTime, WET_SELF_TEST_DISPLACEMENT_TIME_MS ) )
+        {
+            signalDialInPumpHardStop();
+            fmdIntegratedVolume = ( ( fmdIntegratedVolume * TASK_GENERAL_INTERVAL ) / ( SEC_PER_MIN * MS_PER_SECOND ) );
+
+            settleStartTime = getMSTimerCount();
+            state           = WET_SELF_TESTS_SECOND_DISPLACEMENT_VERIFY_STATE;
+        }
+    }
+
+    /*fmdIntegratedVolume += getMeasuredDialInFlowRate();
+
     if ( TRUE == didTimeout( displacementStartTime, WET_SELF_TEST_DISPLACEMENT_TIME_MS ) )
     {
         signalDialInPumpHardStop();
@@ -1814,7 +1869,7 @@
 
         settleStartTime = getMSTimerCount();
         state = WET_SELF_TESTS_SECOND_DISPLACEMENT_VERIFY_STATE;
-    }
+    }*/
 
     if ( TRUE == doesAlarmStatusIndicateStop() )
     {
@@ -1837,13 +1892,13 @@
 {
     WET_SELF_TESTS_STATE_T state = WET_SELF_TESTS_SECOND_DISPLACEMENT_VERIFY_STATE;
 
-    if ( TRUE == didTimeout( settleStartTime, RESERVOIR_SETTLE_TIME_MS ) )
+    //if ( TRUE == didTimeout( settleStartTime, RESERVOIR_SETTLE_TIME_MS ) )
     {
-        F32 resOneDiffAfterDisplacement     = reservoirVolume[ DG_RESERVOIR_1 ] - getReservoirWeightLargeFilter( DG_RESERVOIR_1 );
-        F32 resTwoDiffAfterDisplacement     = getReservoirWeightLargeFilter( DG_RESERVOIR_2 ) - reservoirVolume[ DG_RESERVOIR_2 ];
-        F32 avgDisp                         = ( resOneDiffAfterDisplacement + resTwoDiffAfterDisplacement ) / 2.0;
+        F32 resOneDiffAfterDisplacement     = reservoirVolume[ DG_RESERVOIR_1 ] - getReservoirWeight( DG_RESERVOIR_1 ); //getReservoirWeightLargeFilter( DG_RESERVOIR_1 );
+        F32 resTwoDiffAfterDisplacement     = /*getReservoirWeightLargeFilter( DG_RESERVOIR_2 )*/getReservoirWeight( DG_RESERVOIR_2 ) - reservoirVolume[ DG_RESERVOIR_2 ];
+        F32 avgDisp                         = ( resOneDiffAfterDisplacement + resTwoDiffAfterDisplacement ) / 2.0F;
         F32 integratedVolumeDiff            = fabs( fmdIntegratedVolume - avgDisp );
-        F32 integratedVolumeToTargetPercent = fabs( 1.0 - ( fmdIntegratedVolume / avgDisp ) );
+        F32 integratedVolumeToTargetPercent = fabs( 1.0F - ( fmdIntegratedVolume / avgDisp ) );
         F32 integrateVolumeToleranceG       = WET_SELF_TEST_INTEGRATED_VOLUME_TOLERANCE;
 
 #ifndef _RELEASE_