diff --git a/src/main.c b/src/main.c
index fc39d0b..3d0edce 100644
--- a/src/main.c
+++ b/src/main.c
@@ -22,6 +22,7 @@ enum OperState {
 	Bootstrap,
 	ConvConstVoltage,
 	ConvConstCurrent,
+	ConvMPP,
 	Idle,
 };
 
@@ -216,6 +217,11 @@ int main(void)
 	fxp_t pErr, iErr = 0;
 	fxp_t controlAction;
 
+	fxp_t power_avg, refPower;
+	fxp_t testPower[2];
+	int32_t testPWM[2];
+	int     testPWMStep;
+
 	fxp_t PGAIN_CV = fxp_from_float( 50.000f);
 	fxp_t IGAIN_CV = fxp_from_float(  0.300f);
 	fxp_t IERR_LIMIT = fxp_from_int(4000);
@@ -228,8 +234,11 @@ int main(void)
 	fxp_t AVG_FACT = fxp_from_float(0.10f);
 	fxp_t AVG_FACT_INV = fxp_sub(fxp_from_int(1), AVG_FACT);
 
-	fxp_t maxVoltage = fxp_from_float(12.000f);
-	fxp_t maxCurrent = fxp_from_float( 5.000f);
+	fxp_t MAX_VOLTAGE = fxp_from_float(12.000f);
+	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_CURRENT_THR = fxp_sub(MAX_CURRENT, fxp_from_float(0.500f));
 
 	// Calculated values
 	//fxp_t VIN_SCALE = fxp_from_float(3.3f * (100 + 12.4f) / 12.4f / 4095.0f);
@@ -300,7 +309,6 @@ int main(void)
 				fxp_right_align(number, msg, 3, '0');
 				lcd_send_string("L:");
 				lcd_send_string(msg);
-				lcd_send_string("m");
 			}
 		}
 
@@ -319,6 +327,8 @@ int main(void)
 		vout_avg    = fxp_add(fxp_mult(vout, AVG_FACT),    fxp_mult(vout_avg, AVG_FACT_INV));
 		current_avg = fxp_add(fxp_mult(current, AVG_FACT), fxp_mult(current_avg, AVG_FACT_INV));
 
+		power_avg = fxp_mult(vout_avg, current_avg);
+
 		// Main FSM
 		if(timebase_ms >= 1000) {
 			switch(operState) {
@@ -339,7 +349,7 @@ int main(void)
 					timer_set_oc_value(TIM1, TIM_CH_BOOTSTRAP, 0);
 
 					// calculate error values
-					pErr = fxp_sub(maxVoltage, vout_avg);
+					pErr = fxp_sub(MAX_VOLTAGE, vout_avg);
 					iErr = fxp_add(iErr, pErr);
 
 					// limit integral error range
@@ -366,7 +376,11 @@ int main(void)
 						nextState = ConvConstVoltage;
 					}
 
-					if(current_avg > maxCurrent) {
+					if(time_in_state > 1000 && vout_avg < MPP_VOLTAGE_THR) {
+						operState = ConvMPP;
+					}
+
+					if(current_avg > MAX_CURRENT) {
 						operState = ConvConstCurrent;
 					}
 					break;
@@ -376,7 +390,7 @@ int main(void)
 					timer_set_oc_value(TIM1, TIM_CH_BOOTSTRAP, 0);
 
 					// calculate error values
-					pErr = fxp_sub(maxCurrent, current_avg);
+					pErr = fxp_sub(MAX_CURRENT, current_avg);
 					iErr = fxp_add(iErr, pErr);
 
 					// limit integral error range
@@ -403,11 +417,80 @@ int main(void)
 						nextState = ConvConstCurrent;
 					}
 
-					if(vout_avg > maxVoltage) {
+					if(time_in_state > 1000 && current_avg < MPP_CURRENT_THR) {
+						operState = ConvMPP;
+					}
+
+					if(vout_avg > MAX_VOLTAGE) {
 						operState = ConvConstVoltage;
 					}
 					break;
 
+				case ConvMPP:
+					// bootstrap off
+					timer_set_oc_value(TIM1, TIM_CH_BOOTSTRAP, 0);
+
+					if((time_in_state % 50) == 0) {
+						switch(testPWMStep) {
+							case 0:
+							case 2:
+								pwm = testPWM[testPWMStep/2];
+								timer_set_oc_value(TIM1, TIM_CH_CONV, pwm);
+								break;
+
+							case 1:
+							case 3:
+								testPower[testPWMStep/2] = power_avg;
+								break;
+
+							case 4:
+								if(testPower[1] > testPower[0]) {
+									refPower = testPower[1];
+									pwm      = testPWM[1];
+								} else {
+									refPower = testPower[0];
+									pwm      = testPWM[0];
+								}
+
+								if(pwm < (CONV_PWM_PERIOD/2) || pwm > CONV_PWM_MAX) {
+									pwm = 3 * CONV_PWM_PERIOD / 4;
+								}
+
+								timer_set_oc_value(TIM1, TIM_CH_CONV, pwm);
+								break;
+
+							case 5:
+								// power test is currently idle, but power has changed
+								// too much.
+								// -> start a new one.
+								testPWM[0] = pwm + 7;
+								testPWM[1] = pwm - 6;
+								testPWMStep = 0;
+								break;
+
+							default:
+								testPWMStep = 4;
+								break;
+						}
+
+						testPWMStep++;
+					}
+
+					if(time_in_state > 1000 && current < CURRENT_THRESHOLD) {
+						operState = Bootstrap;
+						nextState = ConvMPP;
+					}
+
+					if(vout_avg > MAX_VOLTAGE) {
+						operState = ConvConstVoltage;
+					}
+
+					if(current_avg > MAX_CURRENT) {
+						operState = ConvConstCurrent;
+					}
+
+					break;
+
 				case Idle:
 					// disable all PWMs
 					timer_set_oc_value(TIM1, TIM_CH_CONV, 0);
@@ -426,22 +509,25 @@ int main(void)
 				time_in_state = 0;
 				lastState = operState;
 
-				lcd_set_cursor_pos(0, 7);
+				lcd_set_cursor_pos(0, 6);
 				switch(operState) {
 					case Idle:
-						lcd_send_string("ID");
+						lcd_send_string("IDL");
 						break;
 					case Bootstrap:
-						lcd_send_string("BS");
+						lcd_send_string("BTS");
 						break;
 					case ConvConstCurrent:
-						lcd_send_string("CC");
+						lcd_send_string("CC ");
 						break;
 					case ConvConstVoltage:
-						lcd_send_string("CV");
+						lcd_send_string("CV ");
+						break;
+					case ConvMPP:
+						lcd_send_string("MPP");
 						break;
 					default:
-						lcd_send_string("??");
+						lcd_send_string("???");
 						break;
 				}
 			} else {
@@ -458,12 +544,6 @@ int main(void)
 				debug_send_string(msg);
 			}
 
-			debug_send_string("\r\n");
-
-			debug_send_string("State: ");
-			fxp_format_int((int)operState, msg);
-			debug_send_string(msg);
-
 			sentSomething = 1;
 		}
 
@@ -483,6 +563,22 @@ int main(void)
 			sentSomething = 1;
 		}
 
+		if((timebase_ms % 500) == 200) {
+			debug_send_string("power_avg=");
+			fxp_format(power_avg, msg, 2);
+			debug_send_string(msg);
+
+			debug_send_string("; ref=");
+			fxp_format(refPower, msg, 2);
+			debug_send_string(msg);
+
+			debug_send_string("; pwm=");
+			fxp_format_int(pwm, msg);
+			debug_send_string(msg);
+
+			sentSomething = 1;
+		}
+
 		if(sentSomething) {
 			debug_send_string("\r\n");
 			sentSomething = 0;