diff --git a/include/param_prj.h b/include/param_prj.h
index 5b32c07..fafea13 100644
--- a/include/param_prj.h
+++ b/include/param_prj.h
@@ -17,14 +17,14 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#define VER 4.95.R
+#define VER 4.96.R
 
 /* Entries must be ordered as follows:
    1. Saveable parameters (id != 0)
    2. Temporary parameters (id = 0)
    3. Display values
  */
-//Next param id (increase when adding new parameter!): 128
+//Next param id (increase when adding new parameter!): 129
 //Next value Id: 2048
 /*              category     name         unit       min     max     default id */
 
@@ -93,7 +93,8 @@
     PARAM_ENTRY(CAT_CHARGER, chargekp,    "dig",     0,      100,    80,     72  ) \
     PARAM_ENTRY(CAT_CHARGER, chargeki,    "dig",     0,      100,    10,     126 ) \
     PARAM_ENTRY(CAT_CHARGER, chargeflt,   "dig",     0,      10,     8,      73  ) \
-    PARAM_ENTRY(CAT_CHARGER, chargemax,   "%",       0,      99,     90,     79  )
+    PARAM_ENTRY(CAT_CHARGER, chargepwmin, "%",       0,      99,     0,      128 ) \
+    PARAM_ENTRY(CAT_CHARGER, chargepwmax, "%",       0,      99,     90,     79  )
 
 #define THROTTLE_PARAMETERS_COMMON \
     PARAM_ENTRY(CAT_THROTTLE,potmin,      "dig",     0,      4095,   0,      17  ) \
diff --git a/libopeninv b/libopeninv
index a3240da..1973ef6 160000
--- a/libopeninv
+++ b/libopeninv
@@ -1 +1 @@
-Subproject commit a3240daeac24399f8ad4f40ba10d56b01d674b8a
+Subproject commit 1973ef68ef5731750e1aa36997c9870adad0a29d
diff --git a/sinus.cbp b/sinus.cbp
index 3645955..3acb4d1 100644
--- a/sinus.cbp
+++ b/sinus.cbp
@@ -29,7 +29,7 @@
 					<SilentBuild command="CONTROL=SINE make &gt; $(CMD_NULL)" />
 				</MakeCommands>
 			</Target>
-			<Target title="FieldOrientedControl">
+			<Target title="FOC">
 				<Option output="stm32_foc" prefix_auto="0" extension_auto="0" />
 				<Option object_output="obj/Release/" />
 				<Option type="1" />
@@ -52,7 +52,7 @@
 		</Build>
 		<Unit filename="Makefile">
 			<Option target="SlipControl" />
-			<Option target="FieldOrientedControl" />
+			<Option target="FOC" />
 		</Unit>
 		<Unit filename="include/anain_prj.h" />
 		<Unit filename="include/digio_prj.h" />
@@ -115,7 +115,7 @@
 		<Unit filename="src/throttle.cpp" />
 		<Unit filename="stm32_sine.ld">
 			<Option target="SlipControl" />
-			<Option target="FieldOrientedControl" />
+			<Option target="FOC" />
 		</Unit>
 		<Unit filename="test/Makefile">
 			<Option target="Test" />
diff --git a/src/pwmgeneration-foc.cpp b/src/pwmgeneration-foc.cpp
index 71c8c5f..b86a65b 100644
--- a/src/pwmgeneration-foc.cpp
+++ b/src/pwmgeneration-foc.cpp
@@ -230,45 +230,43 @@ void PwmGeneration::PwmInit()
 s32fp PwmGeneration::ProcessCurrents(s32fp& id, s32fp& iq)
 {
    static int il1Avg = 0, il2Avg = 0;
-   const int offsetSamples = 16;
+   const int offsetSamples = 64;
 
    if (initwait > 0)
    {
       initwait--;
+   }
 
-      if (initwait <= offsetSamples)
-      {
-         il1Avg += AnaIn::il1.Get();
-         il2Avg += AnaIn::il2.Get();
-      }
-      else
-      {
-         il1Avg = il2Avg = 0;
-      }
+   s32fp il1 = GetCurrent(AnaIn::il1, ilofs[0], Param::Get(Param::il1gain));
+   s32fp il2 = GetCurrent(AnaIn::il2, ilofs[1], Param::Get(Param::il2gain));
 
-      if (initwait == 1)
-      {
-         SetCurrentOffset(il1Avg / offsetSamples, il2Avg / offsetSamples);
-      }
+   //250ms after motor standstill
+   if (idleCounter >= (pwmfrq / 4) && idleCounter < ((pwmfrq / 4) + offsetSamples))
+   {
+      il1Avg += AnaIn::il1.Get();
+      il2Avg += AnaIn::il2.Get();
+   }
+   else if (idleCounter == ((pwmfrq / 4) + offsetSamples))
+   {
+      SetCurrentOffset(il1Avg / offsetSamples, il2Avg / offsetSamples);
    }
    else
    {
-      s32fp il1 = GetCurrent(AnaIn::il1, ilofs[0], Param::Get(Param::il1gain));
-      s32fp il2 = GetCurrent(AnaIn::il2, ilofs[1], Param::Get(Param::il2gain));
-
-      if ((Param::GetInt(Param::pinswap) & SWAP_CURRENTS) > 0)
-         FOC::ParkClarke(il2, il1, angle);
-      else
-         FOC::ParkClarke(il1, il2, angle);
-      id = FOC::id;
-      iq = FOC::iq;
-
-      Param::SetFlt(Param::id, FOC::id);
-      Param::SetFlt(Param::iq, FOC::iq);
-      Param::SetFlt(Param::il1, il1);
-      Param::SetFlt(Param::il2, il2);
+      il1Avg = il2Avg = 0;
    }
 
+   if ((Param::GetInt(Param::pinswap) & SWAP_CURRENTS) > 0)
+      FOC::ParkClarke(il2, il1, angle);
+   else
+      FOC::ParkClarke(il1, il2, angle);
+   id = FOC::id;
+   iq = FOC::iq;
+
+   Param::SetFlt(Param::id, FOC::id);
+   Param::SetFlt(Param::iq, FOC::iq);
+   Param::SetFlt(Param::il1, il1);
+   Param::SetFlt(Param::il2, il2);
+
    return 0;
 }
 
diff --git a/src/pwmgeneration.cpp b/src/pwmgeneration.cpp
index 22685d5..aaca014 100644
--- a/src/pwmgeneration.cpp
+++ b/src/pwmgeneration.cpp
@@ -98,11 +98,14 @@ int PwmGeneration::GetCpuLoad()
 
 static void ConfigureChargeController()
 {
+   int pwmin = FP_TOINT((Param::Get(Param::chargepwmin) * (1 << pwmdigits)) / 100);
+   int pwmax = FP_TOINT((Param::Get(Param::chargepwmax) * (1 << pwmdigits)) / 100);
+
    chargeController.SetCallingFrequency(rcc_apb2_frequency / FRQ_DIVIDER);
-   chargeController.SetMinMaxY(0, FP_TOINT((Param::Get(Param::chargemax) * (1 << pwmdigits)) / 100));
+   chargeController.SetMinMaxY(pwmin, pwmax);
    chargeController.SetGains(Param::GetInt(Param::chargekp), Param::GetInt(Param::chargeki));
    chargeController.SetRef(0);
-   chargeController.ResetIntegrator();
+   chargeController.PreloadIntegrator(pwmin);
 }
 
 void PwmGeneration::SetOpmode(int _opmode)
diff --git a/src/stm32_sine.cpp b/src/stm32_sine.cpp
index 6c3c308..873fc9e 100644
--- a/src/stm32_sine.cpp
+++ b/src/stm32_sine.cpp
@@ -605,7 +605,7 @@ static void Ms10Task(void)
    {
       PwmGeneration::SetTorquePercent(torquePercent);
    }
-   else if (MOD_BOOST == opmode || MOD_BUCK == opmode)
+   else if ((MOD_BOOST == opmode || MOD_BUCK == opmode) && initWait == -1)
    {
       s32fp chargeCur = Param::Get(Param::chargecur);
       s32fp tempDerate = FP_FROMINT(100);
@@ -646,7 +646,16 @@ static void Ms10Task(void)
       {
          //In buck mode we precharge to a different voltage
          if ((chargemode == MOD_BUCK && udc >= Param::Get(Param::udcswbuck)) || chargemode == MOD_BOOST)
+         {
             newMode = chargemode;
+
+            //Prius needs to run PWM before closing the contactor
+            if (hwRev == HW_PRIUS)
+            {
+               PwmGeneration::SetChargeCurrent(0);
+               PwmGeneration::SetOpmode(newMode);
+            }
+         }
       }
       else if (Param::GetBool(Param::din_start) ||
               (Param::GetInt(Param::tripmode) == TRIP_AUTORESUME && PwmGeneration::Tripped()))