diff --git a/src/charge_control.c b/src/charge_control.c
index 4c8619c..3cd5ddb 100644
--- a/src/charge_control.c
+++ b/src/charge_control.c
@@ -67,83 +67,9 @@ static void control_solar_switch(fxp_t u_bat, fxp_t corridor_high, fxp_t corrido
 }
 
 
-void charge_control_init(void)
+static void solar_fsm_update(uint64_t uptime_ms, struct MeasurementResult *meas)
 {
-	charge_state    = CHARGE_WAIT_CHARGEPUMP;
-	discharge_state = DISCHARGE_WAIT_CHARGEPUMP;
-
-	charge_state_entered    = true;
-	discharge_state_entered = true;
-
-	/* calculate thresholds */
-	u_bat_regulation_corridor = fxp_div(FXP_FROM_INT(U_BAT_REGULATION_CORRIDOR),
-	                                    FXP_FROM_INT(1000));
-
-	u_bat_initial_full = fxp_div(FXP_FROM_INT(U_BAT_INITAL_FULL), FXP_FROM_INT(1000));
-	u_bat_initial_low  = fxp_sub(u_bat_initial_full, u_bat_regulation_corridor);
-
-	u_bat_float_full = fxp_div(FXP_FROM_INT(U_BAT_FLOAT_FULL), FXP_FROM_INT(1000));
-	u_bat_float_low  = fxp_sub(u_bat_float_full, u_bat_regulation_corridor);
-
-	min_charge_pump_excess_voltage = fxp_div(FXP_FROM_INT(MIN_CHARGE_PUMP_EXCESS_VOLTAGE),
-	                                         FXP_FROM_INT(1000));
-
-	u_bat_load_on  = fxp_div(FXP_FROM_INT(U_BAT_LOAD_ON), FXP_FROM_INT(1000));
-	u_bat_load_off = fxp_div(FXP_FROM_INT(U_BAT_LOAD_OFF), FXP_FROM_INT(1000));
-
-	load_current_limit = fxp_div(FXP_FROM_INT(LOAD_CURRENT_LIMIT_MA), FXP_FROM_INT(1000));
-
-	internal_temperature_limit = fxp_div(FXP_FROM_INT(INTERNAL_TEMPERATURE_LIMIT), FXP_FROM_INT(10));
-	internal_temperature_recovery = fxp_div(FXP_FROM_INT(INTERNAL_TEMPERATURE_RECOVERY), FXP_FROM_INT(10));
-
-	sleep_solar_current = fxp_div(FXP_FROM_INT(SLEEP_SOLAR_CURRENT), FXP_FROM_INT(1000));
-	sleep_solar_excess_voltage = fxp_div(FXP_FROM_INT(SLEEP_SOLAR_EXCESS_VOLTAGE), FXP_FROM_INT(1000));
-}
-
-
-void charge_control_update(uint64_t uptime_ms, struct MeasurementResult *meas)
-{
-	/* state change tracking for efficient transistions. */
-	enum ChargeState    last_charge_state    = charge_state;
-	enum DischargeState last_discharge_state = discharge_state;
-
-	if(charge_state_entered) {
-		rs485_enqueue("STATE:CHARGE:");
-		rs485_enqueue(CHARGE_STATE_TEXT[charge_state]);
-		rs485_enqueue("\n");
-		charge_state_entered_timestamp = uptime_ms;
-	}
-
-	if(discharge_state_entered) {
-		rs485_enqueue("STATE:DISCHG:");
-		rs485_enqueue(DISCHARGE_STATE_TEXT[discharge_state]);
-		rs485_enqueue("\n");
-		discharge_state_entered_timestamp = uptime_ms;
-	}
-
-	uint64_t charge_time_in_state    = uptime_ms - charge_state_entered_timestamp;
-	uint64_t discharge_time_in_state = uptime_ms - discharge_state_entered_timestamp;
-
-	/* calculate charge pump excess voltage above battery voltage. */
-	fxp_t charge_pump_voltage_delta = fxp_sub(meas->u_sw, meas->u_bat);
-
-	/* generalized charge pump control */
-	if(charge_state_entered || discharge_state_entered) {
-		if(charge_state == CHARGE_WAIT_CHARGEPUMP
-				|| discharge_state == DISCHARGE_WAIT_CHARGEPUMP) {
-			// either charge or discharge control is waiting for the charge
-			// pump, so power it up!
-			charge_pump_start();
-		} else if((charge_state == CHARGE_SLEEP)
-				&& (discharge_state == DISCHARGE_VOLTAGE_LOW)) {
-			// no power from the solar panel and the battery voltage is too
-			// low, so both switches are off and we can safely stop the charge
-			// pump
-			charge_pump_stop();
-		}
-	}
-
-	/* Charging FSM */
+	uint64_t charge_time_in_state = uptime_ms - charge_state_entered_timestamp;
 
 	switch(charge_state) {
 		case CHARGE_WAIT_CHARGEPUMP:
@@ -152,7 +78,9 @@ void charge_control_update(uint64_t uptime_ms, struct MeasurementResult *meas)
 				power_switch_solar_off();
 			}
 
-			if(charge_pump_voltage_delta > min_charge_pump_excess_voltage) {
+			// calculate charge pump output excess voltage over battery voltage
+			// and compare to the threshold
+			if(fxp_sub(meas->u_sw, meas->u_bat) > min_charge_pump_excess_voltage) {
 				charge_state = CHARGE_INITIAL;
 			}
 			break;
@@ -240,8 +168,12 @@ void charge_control_update(uint64_t uptime_ms, struct MeasurementResult *meas)
 			// unknown state
 			break;
 	}
+}
 
-	/* Load control FSM */
+
+static void load_fsm_update(uint64_t uptime_ms, struct MeasurementResult *meas)
+{
+	uint64_t discharge_time_in_state = uptime_ms - discharge_state_entered_timestamp;
 
 	switch(discharge_state) {
 		case DISCHARGE_WAIT_CHARGEPUMP:
@@ -250,7 +182,9 @@ void charge_control_update(uint64_t uptime_ms, struct MeasurementResult *meas)
 				power_switch_load_off();
 			}
 
-			if(charge_pump_voltage_delta > min_charge_pump_excess_voltage) {
+			// calculate charge pump output excess voltage over battery voltage
+			// and compare to the threshold
+			if(fxp_sub(meas->u_sw, meas->u_bat) > min_charge_pump_excess_voltage) {
 				discharge_state = DISCHARGE_VOLTAGE_LOW;
 			}
 			break;
@@ -298,6 +232,84 @@ void charge_control_update(uint64_t uptime_ms, struct MeasurementResult *meas)
 			// unknown state
 			break;
 	}
+}
+
+
+void charge_control_init(void)
+{
+	charge_state    = CHARGE_WAIT_CHARGEPUMP;
+	discharge_state = DISCHARGE_WAIT_CHARGEPUMP;
+
+	charge_state_entered    = true;
+	discharge_state_entered = true;
+
+	/* calculate thresholds */
+	u_bat_regulation_corridor = fxp_div(FXP_FROM_INT(U_BAT_REGULATION_CORRIDOR),
+	                                    FXP_FROM_INT(1000));
+
+	u_bat_initial_full = fxp_div(FXP_FROM_INT(U_BAT_INITAL_FULL), FXP_FROM_INT(1000));
+	u_bat_initial_low  = fxp_sub(u_bat_initial_full, u_bat_regulation_corridor);
+
+	u_bat_float_full = fxp_div(FXP_FROM_INT(U_BAT_FLOAT_FULL), FXP_FROM_INT(1000));
+	u_bat_float_low  = fxp_sub(u_bat_float_full, u_bat_regulation_corridor);
+
+	min_charge_pump_excess_voltage = fxp_div(FXP_FROM_INT(MIN_CHARGE_PUMP_EXCESS_VOLTAGE),
+	                                         FXP_FROM_INT(1000));
+
+	u_bat_load_on  = fxp_div(FXP_FROM_INT(U_BAT_LOAD_ON), FXP_FROM_INT(1000));
+	u_bat_load_off = fxp_div(FXP_FROM_INT(U_BAT_LOAD_OFF), FXP_FROM_INT(1000));
+
+	load_current_limit = fxp_div(FXP_FROM_INT(LOAD_CURRENT_LIMIT_MA), FXP_FROM_INT(1000));
+
+	internal_temperature_limit = fxp_div(FXP_FROM_INT(INTERNAL_TEMPERATURE_LIMIT), FXP_FROM_INT(10));
+	internal_temperature_recovery = fxp_div(FXP_FROM_INT(INTERNAL_TEMPERATURE_RECOVERY), FXP_FROM_INT(10));
+
+	sleep_solar_current = fxp_div(FXP_FROM_INT(SLEEP_SOLAR_CURRENT), FXP_FROM_INT(1000));
+	sleep_solar_excess_voltage = fxp_div(FXP_FROM_INT(SLEEP_SOLAR_EXCESS_VOLTAGE), FXP_FROM_INT(1000));
+}
+
+
+void charge_control_update(uint64_t uptime_ms, struct MeasurementResult *meas)
+{
+	/* state change tracking for efficient transistions. */
+	enum ChargeState    last_charge_state    = charge_state;
+	enum DischargeState last_discharge_state = discharge_state;
+
+	if(charge_state_entered) {
+		rs485_enqueue("STATE:CHARGE:");
+		rs485_enqueue(CHARGE_STATE_TEXT[charge_state]);
+		rs485_enqueue("\n");
+		charge_state_entered_timestamp = uptime_ms;
+	}
+
+	if(discharge_state_entered) {
+		rs485_enqueue("STATE:DISCHG:");
+		rs485_enqueue(DISCHARGE_STATE_TEXT[discharge_state]);
+		rs485_enqueue("\n");
+		discharge_state_entered_timestamp = uptime_ms;
+	}
+
+	/* calculate charge pump excess voltage above battery voltage. */
+	fxp_t charge_pump_voltage_delta = fxp_sub(meas->u_sw, meas->u_bat);
+
+	/* generalized charge pump control */
+	if(charge_state_entered || discharge_state_entered) {
+		if(charge_state == CHARGE_WAIT_CHARGEPUMP
+				|| discharge_state == DISCHARGE_WAIT_CHARGEPUMP) {
+			// either charge or discharge control is waiting for the charge
+			// pump, so power it up!
+			charge_pump_start();
+		} else if((charge_state == CHARGE_SLEEP)
+				&& (discharge_state == DISCHARGE_VOLTAGE_LOW)) {
+			// no power from the solar panel and the battery voltage is too
+			// low, so both switches are off and we can safely stop the charge
+			// pump
+			charge_pump_stop();
+		}
+	}
+
+	solar_fsm_update(uptime_ms, meas);
+	load_fsm_update(uptime_ms, meas);
 
 	charge_state_entered    = charge_state    != last_charge_state;
 	discharge_state_entered = discharge_state != last_discharge_state;