diff --git a/Makefile b/Makefile
index e51c95f..4319f2a 100644
--- a/Makefile
+++ b/Makefile
@@ -52,7 +52,7 @@ LDFLAGS+=-Wl,--start-group -lc -lgcc -lnosys -Wl,--end-group
 CFLAGS_debug=-O0 -ggdb #-DDEBUG
 LDFLAGS_debug=
 
-CFLAGS_release=-O3 -ggdb
+CFLAGS_release=-Os -ggdb
 LDFLAGS_release=
 
 # target configuration
diff --git a/src/main.c b/src/main.c
index a280549..c15a0ef 100644
--- a/src/main.c
+++ b/src/main.c
@@ -207,7 +207,7 @@ int main(void)
 	char number[FXP_STR_MAXLEN];
 	uint8_t sentSomething = 0;
 
-	int32_t pwm;
+	int32_t pwm = 0;
 
 	enum OperState operState = Bootstrap;
 	enum OperState nextState = ConvConstVoltage;
@@ -216,13 +216,13 @@ int main(void)
 	fxp_t vin, vout, current;
 	fxp_t vin_avg = 0, vout_avg = 0, current_avg = 0;
 
-	fxp_t pErr, iErr = 0;
-	fxp_t controlAction;
+	fxp_t pErr = 0, iErr = 0;
+	fxp_t controlAction = 0;
 
-	fxp_t power_avg, refPower;
+	fxp_t power_avg, refPower = 0;
 	fxp_t testPower[2];
 	int32_t testPWM[2];
-	int     testPWMStep;
+	int     testPWMStep = 5;
 
 	fxp_t PGAIN_CV = fxp_from_float( 50.000f);
 	fxp_t IGAIN_CV = fxp_from_float(  0.300f);
@@ -231,15 +231,16 @@ int main(void)
 	fxp_t PGAIN_CC = fxp_from_float( 50.000f);
 	fxp_t IGAIN_CC = fxp_from_float(  0.300f);
 
-	fxp_t CURRENT_THRESHOLD = fxp_from_float(0.05f);
+	fxp_t CURRENT_THRESHOLD = fxp_from_float(0.01f);
 
 	fxp_t AVG_FACT = fxp_from_float(0.10f);
 	fxp_t AVG_FACT_INV = fxp_sub(fxp_from_int(1), AVG_FACT);
 
-	fxp_t MAX_VOLTAGE = fxp_from_float(13.800f);
+	fxp_t MAX_VOLTAGE = fxp_from_float(14.400f);
+	fxp_t CONST_VOLTAGE = fxp_from_float(13.800f);
 	fxp_t MAX_CURRENT = fxp_from_float( 5.000f);
 
-	fxp_t MPP_VOLTAGE_THR = fxp_sub(MAX_VOLTAGE, fxp_from_float(0.500f));
+	fxp_t MPP_VOLTAGE_THR = fxp_sub(CONST_VOLTAGE, fxp_from_float(0.500f));
 	fxp_t MPP_CURRENT_THR = fxp_sub(MAX_CURRENT, fxp_from_float(0.500f));
 
 	// Calculated values
@@ -353,7 +354,7 @@ int main(void)
 					timer_set_oc_value(TIM1, TIM_CH_BOOTSTRAP, 0);
 
 					// calculate error values
-					pErr = fxp_sub(MAX_VOLTAGE, vout_avg);
+					pErr = fxp_sub(CONST_VOLTAGE, vout_avg);
 					iErr = fxp_add(iErr, pErr);
 
 					// limit integral error range
@@ -375,7 +376,7 @@ int main(void)
 						timer_set_oc_value(TIM1, TIM_CH_CONV, 0);
 					}
 
-					if(time_in_state > 1000 && pwm > CONV_PWM_MAX && current < CURRENT_THRESHOLD) {
+					if(time_in_state > 5000 && pwm > CONV_PWM_MAX && current < CURRENT_THRESHOLD) {
 						operState = Bootstrap;
 						nextState = ConvConstVoltage;
 					}
@@ -420,7 +421,7 @@ int main(void)
 						timer_set_oc_value(TIM1, TIM_CH_CONV, 0);
 					}
 
-					if(time_in_state > 1000 && pwm > CONV_PWM_MAX && current < CURRENT_THRESHOLD) {
+					if(time_in_state > 5000 && pwm > CONV_PWM_MAX && current < CURRENT_THRESHOLD) {
 						operState = Bootstrap;
 						nextState = ConvConstCurrent;
 					}
@@ -471,8 +472,8 @@ int main(void)
 								// power test is currently idle, but power has changed
 								// too much.
 								// -> start a new one.
-								testPWM[0] = pwm + 5;
-								testPWM[1] = pwm - 4;
+								testPWM[0] = pwm + 4;
+								testPWM[1] = pwm - 3;
 								testPWMStep = 0;
 								break;
 
@@ -484,7 +485,7 @@ int main(void)
 						testPWMStep++;
 					}
 
-					if(time_in_state > 1000 && current < CURRENT_THRESHOLD) {
+					if(time_in_state > 5000 && current < CURRENT_THRESHOLD) {
 						operState = Bootstrap;
 						nextState = ConvMPP;
 					}
@@ -508,7 +509,7 @@ int main(void)
 					timer_set_oc_value(TIM1, TIM_CH_CONV, 0);
 					timer_set_oc_value(TIM1, TIM_CH_BOOTSTRAP, 0);
 
-					if(vin_avg > vout_avg) {
+					if(time_in_state > 1000 && vin_avg > vout_avg) {
 						operState = Bootstrap;
 						nextState = ConvMPP;
 					}