diff --git a/src/main.c b/src/main.c
index 3d0edce..45b2700 100644
--- a/src/main.c
+++ b/src/main.c
@@ -6,6 +6,8 @@
 #include <libopencm3/cm3/nvic.h>
 #include <libopencm3/cm3/systick.h>
 
+#include <libopencmsis/core_cm3.h>
+
 #include <fxp.h>
 #include <fxp_basic.h>
 
@@ -339,7 +341,7 @@ int main(void)
 					timer_set_oc_value(TIM1, TIM_CH_CONV, 0);
 
 					if(time_in_state >= 5) { // bootstrap duration in ms
-						iErr = fxp_from_int(500);
+						iErr = fxp_div(IERR_LIMIT, fxp_from_int(2));
 						operState = nextState;
 					}
 					break;
@@ -383,6 +385,10 @@ int main(void)
 					if(current_avg > MAX_CURRENT) {
 						operState = ConvConstCurrent;
 					}
+
+					if(vin_avg < vout_avg) {
+						operState = Idle;
+					}
 					break;
 
 				case ConvConstCurrent:
@@ -463,8 +469,8 @@ int main(void)
 								// power test is currently idle, but power has changed
 								// too much.
 								// -> start a new one.
-								testPWM[0] = pwm + 7;
-								testPWM[1] = pwm - 6;
+								testPWM[0] = pwm + 5;
+								testPWM[1] = pwm - 4;
 								testPWMStep = 0;
 								break;
 
@@ -489,12 +495,22 @@ int main(void)
 						operState = ConvConstCurrent;
 					}
 
+					if(vin_avg < vout_avg) {
+						operState = Idle;
+					}
+
 					break;
 
 				case Idle:
 					// disable all PWMs
 					timer_set_oc_value(TIM1, TIM_CH_CONV, 0);
 					timer_set_oc_value(TIM1, TIM_CH_BOOTSTRAP, 0);
+
+					if(vin_avg > vout_avg) {
+						operState = Bootstrap;
+						nextState = ConvMPP;
+					}
+
 					break;
 
 				default:
@@ -589,7 +605,10 @@ int main(void)
 
 		timebase_ms++;
 
-		while(wait_frame);
+		while(wait_frame) {
+			__WFI();
+		}
+
 		wait_frame = 1;
 	}