diff --git a/fxplib b/fxplib
index 448c350..035e7ec 160000
--- a/fxplib
+++ b/fxplib
@@ -1 +1 @@
-Subproject commit 448c3504f65a37275b29661320ecf4f3ca6a924d
+Subproject commit 035e7ecda497b408a5791ba20a0906b913dfef45
diff --git a/src/main.c b/src/main.c
index b539caf..d446f80 100644
--- a/src/main.c
+++ b/src/main.c
@@ -12,14 +12,15 @@
 #include "lcd.h"
 #include "debug.h"
 
-#define CONV_PWM_MAX 960
+#define CONV_PWM_PERIOD 960
+#define CONV_PWM_MAX (98*CONV_PWM_PERIOD/100)
 
 #define TIM_CH_CONV      TIM_OC1
 #define TIM_CH_BOOTSTRAP TIM_OC2
 
 enum OperState {
 	Bootstrap,
-	Convert,
+	ConvConstVoltage,
 	Idle,
 };
 
@@ -99,13 +100,13 @@ static void init_timer(void)
 
 	// wanted: 50 kHz / 20 us period
 	// system clock: 48 MHz
-	// => 960 clock cycles / period = CONV_PWM_MAX
+	// => 960 clock cycles / period = CONV_PWM_PERIOD
 
 	// prescaler
 	timer_set_prescaler(TIM1, 0); // Timer runs at system clock
 
 	// auto-reload value
-	timer_set_period(TIM1, CONV_PWM_MAX - 1);
+	timer_set_period(TIM1, CONV_PWM_PERIOD - 1);
 
 	// only generate interrupt on overflow
 	timer_update_on_overflow(TIM1);
@@ -207,6 +208,17 @@ int main(void)
 
 	fxp_t vin, vout, current;
 
+	fxp_t pErr, iErr = 0;
+	fxp_t controlAction;
+
+	fxp_t PGAIN = fxp_from_float( 20.000f);
+	fxp_t IGAIN = fxp_from_float(  1.000f);
+	fxp_t IERR_LIMIT = fxp_from_int(10000);
+
+	fxp_t CURRENT_THRESHOLD = fxp_from_float(0.05f);
+
+	fxp_t maxVoltage = fxp_from_float(11.000f);
+
 	// Calculated values
 	//fxp_t VIN_SCALE = fxp_from_float(3.3f * (100 + 12.4f) / 12.4f / 4095.0f);
 	//fxp_t VOUT_SCALE = fxp_from_float(3.3f * (100 + 12.0f) / 12.0f / 4095.0f);
@@ -254,16 +266,6 @@ int main(void)
 		// scale current measurement
 		curVoltage = VOLTAGE_MEAS_MAX * adcval / 4096.0f;
 
-		// calculate error values
-		pErr = targetVoltage - curVoltage;
-		iErr += pErr;
-
-		// limit integral error range
-		if     (iErr >  IERR_LIMIT) iErr =  IERR_LIMIT;
-		else if(iErr < -IERR_LIMIT) iErr = -IERR_LIMIT;
-
-		// calculate the controller output ("action")
-		controlAction = pErr * PGAIN + iErr * IGAIN;
 
 		// calculate resulting PWM value
 		if(controlAction > MAX_DUTY_CYCLE) {
@@ -297,7 +299,7 @@ int main(void)
 				lcd_send_string(msg);
 				lcd_send_string("V ");
 
-				lcd_set_cursor_pos(0, 8);
+				lcd_set_cursor_pos(0, 10);
 
 				scaled_val = fxp_mult(current, fxp_from_int(1000)); // A -> mA
 				fxp_format(scaled_val, number, 0);
@@ -313,8 +315,9 @@ int main(void)
 
 				fxp_format_int((int32_t)cpuload, number);
 				fxp_right_align(number, msg, 3, '0');
-				lcd_send_string("L:0.");
+				lcd_send_string("L:");
 				lcd_send_string(msg);
+				lcd_send_string("m");
 			}
 		}
 
@@ -332,19 +335,48 @@ int main(void)
 		if(timebase_ms >= 1000) {
 			switch(operState) {
 				case Bootstrap:
-					// enable bootstrap pulse for 10 ns / 10 % duty cycle
+					// enable bootstrap pulse with very low duty cycle
 					// disable converter
 					timer_set_oc_value(TIM1, TIM_CH_BOOTSTRAP, 48);
 					timer_set_oc_value(TIM1, TIM_CH_CONV, 0);
 
-					if(time_in_state > 10) { // bootstrap for 10 ms
-						operState = Convert;
+					if(time_in_state >= 5) { // bootstrap duration in ms
+						iErr = fxp_from_int(500);
+						operState = ConvConstVoltage;
 					}
 					break;
 
-				case Convert:
-					timer_set_oc_value(TIM1, TIM_CH_CONV, 800);
+				case ConvConstVoltage:
+					// bootstrap off
 					timer_set_oc_value(TIM1, TIM_CH_BOOTSTRAP, 0);
+
+					// calculate error values
+					pErr = fxp_sub(maxVoltage, vout);
+					iErr = fxp_add(iErr, pErr);
+
+					// limit integral error range
+					if     (iErr >  IERR_LIMIT) iErr =  IERR_LIMIT;
+					else if(iErr < -IERR_LIMIT) iErr = -IERR_LIMIT;
+
+					// calculate the controller output ("action")
+					controlAction = fxp_add(
+							fxp_mult(pErr, PGAIN),
+							fxp_mult(iErr, IGAIN));
+
+					int32_t pwm = fxp_to_int(controlAction);
+
+					if(pwm > 5000 && current < CURRENT_THRESHOLD) {
+						operState = Bootstrap;
+						break;
+					}
+
+					if(pwm >= CONV_PWM_MAX) {
+						timer_set_oc_value(TIM1, TIM_CH_CONV, CONV_PWM_MAX);
+					} else if(pwm > 0) {
+						timer_set_oc_value(TIM1, TIM_CH_CONV, pwm);
+					} else {
+						timer_set_oc_value(TIM1, TIM_CH_CONV, 0);
+					}
 					break;
 
 				case Idle:
@@ -387,6 +419,22 @@ int main(void)
 			sentSomething = 1;
 		}
 
+		if((timebase_ms % 500) == 100) {
+			debug_send_string("pErr=");
+			fxp_format(pErr, msg, 3);
+			debug_send_string(msg);
+
+			debug_send_string("; iErr=");
+			fxp_format(iErr, msg, 3);
+			debug_send_string(msg);
+
+			debug_send_string("; action=");
+			fxp_format(controlAction, msg, 1);
+			debug_send_string(msg);
+
+			sentSomething = 1;
+		}
+
 		if(sentSomething) {
 			debug_send_string("\r\n");
 			sentSomething = 0;