Index: firmware/App/Modes/SelfTests.c
===================================================================
diff -u -r0339d8357ff90e6540ec4420b536336e8c2bc6ea -rfc8ce9d479040ba4472b152955745760460e47db
--- firmware/App/Modes/SelfTests.c	(.../SelfTests.c)	(revision 0339d8357ff90e6540ec4420b536336e8c2bc6ea)
+++ firmware/App/Modes/SelfTests.c	(.../SelfTests.c)	(revision fc8ce9d479040ba4472b152955745760460e47db)
@@ -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,19 +1629,28 @@
         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 ) ) )
     {
         // Re-send command if DG rejects
         isValvesSettingSent = FALSE;
 
-        if ( FALSE == cmdResp.rejected )
+        if ( DG_CMD_REQUEST_REJECT_REASON_NONE == cmdResp.rejectCode )
         {
             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();  // TODO rename this variable
+            state           = WET_SELF_TESTS_FIRST_DISPLACEMENT_STATE;
         }
     }
 
@@ -1666,15 +1675,28 @@
 {
     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 ) );
+        // This is not a volume for now and it is a flow rate.
+        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 ) / ( WET_SELF_TEST_DISPLACEMENT_TIME_MS ) );
+
+            settleStartTime = getMSTimerCount();
+            state           = WET_SELF_TESTS_FIRST_DISPLACEMENT_VERIFY_STATE;
+        }
     }
 
     if ( TRUE == doesAlarmStatusIndicateStop() )
@@ -1698,10 +1720,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 +1780,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,18 +1789,29 @@
         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 ) ) )
     {
         // Re-send command if DG rejects
         isValvesSettingSent = FALSE;
 
-        if ( FALSE == cmdResp.rejected )
+        if ( DG_CMD_REQUEST_REJECT_REASON_NONE == cmdResp.rejectCode )
         {
             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,16 +1837,27 @@
 {
     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 ( TRUE == didTimeout( displacementStartTime, WET_SELF_TEST_DISPLACEMENT_TIME_MS ) )
+    if ( 0 == settleStartTime )
     {
-        signalDialInPumpHardStop();
-        // Convert flow into volume for period of general task interval
-        fmdIntegratedVolume = ( ( fmdIntegratedVolume * TASK_GENERAL_INTERVAL ) / ( SEC_PER_MIN * MS_PER_SECOND ) );
+        fmdIntegratedVolume += getMeasuredDialInFlowRate();
 
-        settleStartTime = getMSTimerCount();
-        state = WET_SELF_TESTS_SECOND_DISPLACEMENT_VERIFY_STATE;
+        if ( TRUE == didTimeout( displacementStartTime, WET_SELF_TEST_DISPLACEMENT_TIME_MS ) )
+        {
+            signalDialInPumpHardStop();
+            fmdIntegratedVolume = ( ( fmdIntegratedVolume * TASK_GENERAL_INTERVAL ) / ( WET_SELF_TEST_DISPLACEMENT_TIME_MS ) );
+
+            settleStartTime = getMSTimerCount();
+            state           = WET_SELF_TESTS_SECOND_DISPLACEMENT_VERIFY_STATE;
+        }
     }
 
     if ( TRUE == doesAlarmStatusIndicateStop() )
@@ -1837,13 +1881,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_