SolarLader_Firmware/src/main.c

#include <libopencm3/stm32/rcc.h>
#include <libopencm3/stm32/adc.h>
#include <libopencm3/stm32/gpio.h>
#include <libopencm3/stm32/dma.h>
#include <libopencm3/stm32/timer.h>
#include <libopencm3/stm32/rtc.h>
#include <libopencm3/stm32/pwr.h>
#include <libopencm3/stm32/exti.h>
#include <libopencm3/cm3/nvic.h>
#include <libopencm3/cm3/systick.h>

#include <libopencmsis/core_cm3.h>

#include <fxp.h>
#include <fxp_basic.h>

#include "lcd.h"
#include "debug.h"

#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

#define MAX_SLEEP_TIME 600
#define MAX_SLEEP_TIME_LOW_VOLTAGE 120

#define ADC_VALUE_AT_ZERO_CURRENT 90

enum OperState {
	Bootstrap,
	ConvConstVoltage,
	ConvConstCurrent,
	ConvMPP,
	Idle,
};

volatile int wait_frame = 1;

#define ADC_NUM_CHANNELS 4
volatile int16_t adc_values[ADC_NUM_CHANNELS];

static void unlock_rtc_access(void)
{
	pwr_disable_backup_domain_write_protect();
	RTC_WPR = 0xCA;
	RTC_WPR = 0x53;
}

static void lock_rtc_access(void)
{
	RTC_WPR = 0xFF;
	pwr_enable_backup_domain_write_protect();
}

static void init_gpio(void)
{
	// Set up UART TX on PB6 for debugging
	gpio_mode_setup(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO6);
	gpio_set_af(GPIOB, GPIO_AF0, GPIO6);

	// GPIO for converter switch
	gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO8);
	gpio_set_af(GPIOA, GPIO_AF2, GPIO8);

	// GPIO for bootstrap pulse
	gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO9);
	gpio_set_af(GPIOA, GPIO_AF2, GPIO9);

	// GPIO for load activation
	gpio_mode_setup(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO15);
}

static void init_clock(void)
{
	/* Set STM32 to 48 MHz. */
	// Relevant for Timers
	//rcc_clock_setup_in_hse_8mhz_out_48mhz();
	rcc_clock_setup_in_hsi_out_48mhz();

	// enable GPIO clocks:
	// Port A is needed for the Display and more
	rcc_periph_clock_enable(RCC_GPIOA);

	// Port B is needed for debugging
	rcc_periph_clock_enable(RCC_GPIOB);

	// enable TIM3 for scheduling
	rcc_periph_clock_enable(RCC_TIM3);

	// enable TIM1 for PWM generation
	rcc_periph_clock_enable(RCC_TIM1);

	// enable ADC1 clock
	rcc_periph_clock_enable(RCC_ADC1);

	// enable DMA
	rcc_periph_clock_enable(RCC_DMA);
}

static void init_rtc(void)
{
	// RTC clock setup
	// see libopencm3-examples::examples/stm32/l1/stm32l-discovery/button-irq-printf-lowpower/main.c

	/* turn on power block to enable unlocking */
	rcc_periph_clock_enable(RCC_PWR);
	pwr_disable_backup_domain_write_protect();

	/* reset rtc */
	RCC_BDCR |= RCC_BDCR_BDRST;
	RCC_BDCR &= ~RCC_BDCR_BDRST;

	// use LSI for RTC
	rcc_osc_on(RCC_LSI);
	rcc_wait_for_osc_ready(RCC_LSI);

	/* Select the LSI as rtc clock */
	RCC_BDCR |= RCC_BDCR_RTCSEL_LSI;

	/* ?! Stdperiph examples don't turn this on until _afterwards_ which
	 * simply doesn't work.  It must be on at least to be able to
	 * configure it */
	RCC_BDCR |= RCC_BDCR_RTCEN;

	pwr_enable_backup_domain_write_protect();

	nvic_enable_irq(NVIC_RTC_IRQ);

	exti_set_trigger(EXTI17, EXTI_TRIGGER_RISING);
	exti_enable_request(EXTI17);
}

static void init_timer(void)
{
	// *** TIM1 ***
	// Configure channels 1 and 2 for PWM (-> Pins PA8, PA9)
	// Ch1 = Buck converter switch, Ch2 = bootstrap pulse
	timer_reset(TIM1);

	timer_set_mode(TIM1, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);

	// set up PWM channels
	timer_set_oc_mode(TIM1, TIM_CH_CONV, TIM_OCM_PWM1);
	timer_enable_oc_output(TIM1, TIM_CH_CONV);
	timer_enable_oc_preload(TIM1, TIM_CH_CONV);
	timer_set_oc_polarity_high(TIM1, TIM_CH_CONV);

	timer_set_oc_mode(TIM1, TIM_CH_BOOTSTRAP, TIM_OCM_PWM1);
	timer_enable_oc_output(TIM1, TIM_CH_BOOTSTRAP);
	timer_enable_oc_preload(TIM1, TIM_CH_BOOTSTRAP);
	timer_set_oc_polarity_high(TIM1, TIM_CH_BOOTSTRAP);

	timer_set_oc_value(TIM1, TIM_CH_CONV, 0); // no PWM by default
	timer_set_oc_value(TIM1, TIM_CH_BOOTSTRAP, 0); // no PWM by default

	// wanted: 50 kHz / 20 us period
	// system clock: 48 MHz
	// => 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_PERIOD - 1);

	// only generate interrupt on overflow
	timer_update_on_overflow(TIM1);

	// enable master output bit
	timer_enable_break_main_output(TIM1);

	// *** TIM3 ***
	// used for the 1-millisecond system tick
	timer_reset(TIM3);

	timer_set_mode(TIM3, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);

	// prescaler
	timer_set_prescaler(TIM3, 47); // -> 1 us counting at 48 MHz

	// auto-reload value
	timer_set_period(TIM3, 999); // -> update interrupt every 1 ms

	// enable update interrupt (triggered on timer restart)
	timer_enable_irq(TIM3, TIM_DIER_UIE);
	nvic_enable_irq(NVIC_TIM3_IRQ);

	// Start all the timers!
	timer_enable_counter(TIM3);
	timer_enable_counter(TIM1);

}

static void init_adc(void)
{
	uint8_t channels[ADC_NUM_CHANNELS] = {
		0, // VInSense
		1, // VOutSense
		2, // CurrentSense
		16 // Temperature sensor
	};

	adc_power_off(ADC1);

	// enable the temperature sensor
	adc_enable_temperature_sensor();

	// configure ADC
	//adc_enable_scan_mode(ADC1);
	adc_set_single_conversion_mode(ADC1);
	adc_set_resolution(ADC1, ADC_RESOLUTION_12BIT);
	adc_set_sample_time_on_all_channels(ADC1, ADC_SMPR_SMP_239DOT5);
	adc_disable_external_trigger_regular(ADC1);
	adc_set_right_aligned(ADC1);
	adc_set_regular_sequence(ADC1, ADC_NUM_CHANNELS, channels);

	// configure DMA for ADC

	//nvic_enable_irq(NVIC_DMA1_STREAM5_IRQ);
	dma_channel_reset(DMA1, DMA_CHANNEL1);
	dma_set_priority(DMA1, DMA_CHANNEL1, DMA_CCR_PL_LOW);
	dma_set_memory_size(DMA1, DMA_CHANNEL1, DMA_CCR_MSIZE_16BIT);
	dma_set_peripheral_size(DMA1, DMA_CHANNEL1, DMA_CCR_PSIZE_16BIT);
	dma_enable_memory_increment_mode(DMA1, DMA_CHANNEL1);
	dma_enable_circular_mode(DMA1, DMA_CHANNEL1);
	dma_set_read_from_peripheral(DMA1, DMA_CHANNEL1);
	dma_set_peripheral_address(DMA1, DMA_CHANNEL1, (uint32_t) &ADC1_DR);
	/* The array adc_values[] is filled with the waveform data to be output */
	dma_set_memory_address(DMA1, DMA_CHANNEL1, (uint32_t) adc_values);
	dma_set_number_of_data(DMA1, DMA_CHANNEL1, ADC_NUM_CHANNELS);
	//dma_enable_transfer_complete_interrupt(DMA1, DMA_CHANNEL1);
	dma_enable_channel(DMA1, DMA_CHANNEL1);

	adc_enable_dma(ADC1);

	adc_power_on(ADC1);
}

static void deepsleep(uint32_t duration_secs)
{
	uint32_t tmp = 0;

	// unlock RTC registers
	unlock_rtc_access();

	// enter initialization mode
	RTC_ISR |= RTC_ISR_INIT;

	// wait until initialization mode has been entered
	while((RTC_ISR & RTC_ISR_INITF) != RTC_ISR_INITF) {
		// do nothing
	}

	RTC_TR = 0; // 00:00:00
	RTC_DR = // friday, 01.01.16
		(1 << RTC_DR_YT_SHIFT) |
		(6 << RTC_DR_YU_SHIFT) |
		(5 << RTC_DR_WDU_SHIFT) |
		(0 << RTC_DR_MT_SHIFT) |
		(1 << RTC_DR_MU_SHIFT) |
		(0 << RTC_DR_DT_SHIFT) |
		(1 << RTC_DR_DU_SHIFT);

	// disable Alarm A
	RTC_CR &= ~RTC_CR_ALRAE;

	// wait until register is writeable
	while((RTC_ISR & RTC_ISR_ALRAWF) != RTC_ISR_ALRAWF) {
		// do nothing
	}

	tmp |= (duration_secs % 10) << RTC_ALRMXR_SU_SHIFT;
	duration_secs /= 10;

	tmp |= (duration_secs % 6) << RTC_ALRMXR_ST_SHIFT;
	duration_secs /= 6;

	tmp |= (duration_secs % 10) << RTC_ALRMXR_MNU_SHIFT;
	duration_secs /= 10;

	tmp |= (duration_secs % 6) << RTC_ALRMXR_MNT_SHIFT;
	duration_secs /= 6;

	tmp |= (duration_secs % 10) << RTC_ALRMXR_HU_SHIFT;
	duration_secs /= 10;

	tmp |= (duration_secs % 2) << RTC_ALRMXR_HT_SHIFT;
	// FIXME: >1d is not supported

	tmp |= RTC_ALRMXR_MSK4; // ignore day/date

	// set alarm register
	RTC_ALRMAR = tmp;

	// clear Alarm A flag
	RTC_ISR &= ~RTC_ISR_ALRAF;

	// enable RTC alarm interrupt for wakeup
	RTC_CR |= RTC_CR_ALRAE | RTC_CR_ALRAIE;

	// leave initialization mode
	RTC_ISR &= ~RTC_ISR_INIT;

	// lock registers again (using invalid key)
	lock_rtc_access();

	// enter deep sleep mode
	SCB_SCR |= SCB_SCR_SLEEPDEEP;
	PWR_CR |= PWR_CR_LPDS; // voltage regulator low-power mode
	pwr_set_stop_mode();
	__WFI();
	SCB_SCR &= ~SCB_SCR_SLEEPDEEP; // no deepsleep except in this function
	init_clock();
}

#if 0
/* Set up timer to fire freq times per second */
static void init_systick(int freq)
{
	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB);
	/* clear counter so it starts right away */
	STK_CVR = 0;

	systick_set_reload(rcc_ahb_frequency / freq);
	systick_counter_enable();
	systick_interrupt_enable();
}
#endif

/* Temperature sensor calibration value address */
#define TEMP110_CAL_ADDR ((uint16_t*) ((uint32_t) 0x1FFFF7C2))
#define TEMP30_CAL_ADDR ((uint16_t*) ((uint32_t) 0x1FFFF7B8))
#define VDD_CALIB ((uint16_t) (330)) /* calibration voltage = 3,30V - DO NOT CHANGE */
#define VDD_APPLI ((uint16_t) (330)) /* actual supply voltage */

/* function for temperature conversion */
static fxp_t calc_temperature(uint16_t adc_val)
{
	fxp_t temperature = fxp_from_int(
			((int32_t)adc_val * VDD_APPLI / VDD_CALIB) -
			 (int32_t)*TEMP30_CAL_ADDR);

	temperature = fxp_mult(temperature, fxp_from_int(110 - 30));
	temperature = fxp_div(temperature, fxp_from_int(*TEMP110_CAL_ADDR - *TEMP30_CAL_ADDR));

	return fxp_add(temperature, fxp_from_int(30));
}

struct PowerState {
	fxp_t vin, vin_avg;
	fxp_t vout, vout_avg;
	fxp_t current, current_avg;

	fxp_t temp, temp_avg; // junction temperature

	fxp_t power_avg;
};

static void report_status(struct PowerState *power_state,
		int32_t pwm, enum OperState operState)
{
	char number[FXP_STR_MAXLEN];

	debug_send_string("DATA:");

	fxp_format(power_state->vin_avg, number, 3);
	debug_send_string(number);
	debug_send_string(":");

	fxp_format(power_state->vout_avg, number, 3);
	debug_send_string(number);
	debug_send_string(":");

	fxp_format(power_state->current_avg, number, 3);
	debug_send_string(number);
	debug_send_string(":");

	fxp_format_int(pwm, number);
	debug_send_string(number);
	debug_send_string(":");

	fxp_format_int((int32_t)operState, number);
	debug_send_string(number);
	debug_send_string(":");

	fxp_format(power_state->temp_avg, number, 1);
	debug_send_string(number);
	debug_send_string("\r\n");
}

fxp_t MPP_MAX_POWER_CHANGE_FACTOR;

struct MPPState {
	int32_t testIdx; /* -1, if test requested, 0..MPP_TEST_STEPS-1 if running, >=MPP_TEST_STEPS if finished */

	uint32_t nextTestStepTime;

	fxp_t refPower;
	int32_t refPWM;

	fxp_t maxPower;
	int32_t maxPWM;

	fxp_t powerAccu;
	int32_t powerSamples;

	int32_t mppMaxPWM;
};

#define MPP_TEST_IGNORE_DURATION 5 /* ms */
#define MPP_TEST_ACCU_DURATION 20 /* ms */

#define MPP_TEST_DURATION (MPP_TEST_IGNORE_DURATION + MPP_TEST_ACCU_DURATION)

#define MPP_TEST_STEPS 6
const int32_t mpp_pwm_offsets[MPP_TEST_STEPS] = {-3, 4, -16, 15};

static void mpp_run(
		uint32_t time_in_state, struct MPPState *mpp_state,
		struct PowerState *power_state, int32_t *pwm)
{
	fxp_t power_change;
	fxp_t power_change_factor;

	if(mpp_state->testIdx == -1) {
		/* initiate new test */
		mpp_state->refPower = power_state->power_avg;
		mpp_state->nextTestStepTime = time_in_state + MPP_TEST_DURATION; /* time to give averagers to settle */
		mpp_state->testIdx = 0;
		mpp_state->refPWM = *pwm;

		mpp_state->maxPower = 0;
		mpp_state->maxPWM = *pwm;

		mpp_state->powerAccu = 0;
		mpp_state->powerSamples = 0;

		*pwm = *pwm + mpp_pwm_offsets[0];
	} else if(mpp_state->testIdx < MPP_TEST_STEPS) {
		/* test running */

		/* accumulation */
		if(time_in_state > (mpp_state->nextTestStepTime - MPP_TEST_ACCU_DURATION)) {
			mpp_state->powerAccu = fxp_add(mpp_state->powerAccu,
					fxp_mult(power_state->vout, power_state->current));
			mpp_state->powerSamples++;
		}

		if(time_in_state > mpp_state->nextTestStepTime) {
			/* averaging */
			mpp_state->powerAccu = fxp_div(mpp_state->powerAccu, fxp_from_int(mpp_state->powerSamples));

#ifdef DEBUG
			char msg[16];
			debug_send_string("Accu: ");
			fxp_format(mpp_state->powerAccu, msg, 3);
			debug_send_string(msg);
			debug_send_string(" ");
			fxp_format_int(mpp_state->powerSamples, msg);
			debug_send_string(msg);
			debug_send_string(" PWM: ");
			fxp_format_int(*pwm, msg);
			debug_send_string(msg);
			debug_send_string(" Idx: ");
			fxp_format_int(mpp_state->testIdx, msg);
			debug_send_string(msg);
			debug_send_string("\r\n");
#endif

			if(mpp_state->powerAccu > mpp_state->maxPower) {
				mpp_state->maxPower = mpp_state->powerAccu;
				mpp_state->maxPWM = *pwm;
			}

			mpp_state->testIdx++;

			if(mpp_state->testIdx < MPP_TEST_STEPS) {
				*pwm = mpp_state->refPWM + mpp_pwm_offsets[mpp_state->testIdx];

				mpp_state->nextTestStepTime = time_in_state + MPP_TEST_DURATION;

				mpp_state->powerAccu = 0;
				mpp_state->powerSamples = 0;

			}
		}

	} else if(mpp_state->testIdx == MPP_TEST_STEPS) {
		/* finalize test */
		if(mpp_state->maxPower > mpp_state->refPower) {
			mpp_state->refPower = mpp_state->maxPower;
			*pwm = mpp_state->maxPWM;

			/* test again after a short time */
			mpp_state->testIdx++;
			mpp_state->nextTestStepTime = time_in_state + 250;
		} else {
			/* We were already at the maximum power point */
			*pwm = mpp_state->refPWM;

			/* test again after some time */
			mpp_state->testIdx++;
			mpp_state->nextTestStepTime = time_in_state + 20000;
		}
	} else {
		/* no test active, just holding PWM */

		/* initiate new test after defined time */
		if(time_in_state > mpp_state->nextTestStepTime) {
			mpp_state->testIdx = -1;
		}

		/* initiate new test if power changes too much */
		power_change = fxp_abs(fxp_sub(mpp_state->refPower, power_state->power_avg));
		power_change_factor = fxp_div(power_change, mpp_state->refPower);

		if(power_change_factor > MPP_MAX_POWER_CHANGE_FACTOR) {
			mpp_state->testIdx = -1;
		}
	}
}

static void load_on(void)
{
	gpio_set(GPIOA, GPIO15);
}

static void load_off(void)
{
	gpio_clear(GPIOA, GPIO15);
}

int main(void)
{
	//uint32_t cpuload = 0;
	uint64_t timebase_ms = 0;
	uint32_t time_in_state = 0;
	char msg[128];
	char number[FXP_STR_MAXLEN];
	uint8_t sentSomething = 0;
	uint8_t loadInitialized = 0;

	int32_t pwm = 0;

	enum OperState operState = Bootstrap;
	enum OperState nextState = ConvConstVoltage;
	enum OperState lastState = operState;

	struct PowerState power_state;
	struct MPPState mpp_state;

	fxp_t pErr = 0, iErr = 0;
	fxp_t controlAction = 0;

	uint32_t sleep_time = 10;
	uint64_t force_display_update_time = 1000;

	fxp_t PGAIN_CV = fxp_from_float( 2000.000f);
	fxp_t IGAIN_CV = fxp_from_float(    1.000f);
	fxp_t IERR_LIMIT = fxp_from_int(1000);

	fxp_t PGAIN_CC = fxp_from_float(500.000f);
	fxp_t IGAIN_CC = fxp_from_float(  1.000f);

	fxp_t CURRENT_THRESHOLD = fxp_from_float(0.001f);

	fxp_t AVG_FACT = fxp_from_float(0.01f);
	fxp_t AVG_FACT_INV = fxp_sub(fxp_from_int(1), AVG_FACT);

	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(CONST_VOLTAGE, fxp_from_float(0.500f));
	fxp_t MPP_CURRENT_THR = fxp_sub(MAX_CURRENT, fxp_from_float(0.500f));

	// input voltage must exceed output voltage by this value to leave idle mode
	fxp_t WAKEUP_OFFSET_VOLTAGE = fxp_from_float(1.0f);

	// switch off load below LOAD_OFF_THRESHOLD to protect the battery; when the
	// battery recovers above LOAD_ON_THRESHOLD the load is switched on again.
	//
	// If the battery voltage is below LOAD_LOW_VOLTAGE_THRESHOLD, the battery
	// voltage is monitored more closely during idle mode.
	fxp_t LOAD_ON_THRESHOLD = fxp_from_float(13.000f);
	fxp_t LOAD_OFF_THRESHOLD = fxp_from_float(11.200f);
	fxp_t LOAD_LOW_VOLTAGE_THRESHOLD = fxp_from_float(11.600f);

	// Calculated values
	//fxp_t VIN_SCALE = fxp_from_float(3.3f * (100 + 10.0f) / 10.0f / 4095.0f);
	//fxp_t VOUT_SCALE = fxp_from_float(3.3f * (100 + 12.0f) / 12.0f / 4095.0f);

	// Calibrated from measurements
	fxp_t VIN_SCALE = fxp_from_float(36.41f / 4096.0f);
	fxp_t VOUT_SCALE = fxp_from_float(30.87f / 4096.0f);

	// current = adc • 0.00166 + -0.0725 = adc • m + t
	fxp_t ADC2CURRENT_M = fxp_from_float( 0.00166f);
	fxp_t ADC2CURRENT_T = fxp_from_float(-0.07250f);

	/* if power changes by more than this factor, MPP is tested again */
	MPP_MAX_POWER_CHANGE_FACTOR = fxp_from_float(0.2f);

	/* initalize power_state */
	power_state.vin_avg = 0;
	power_state.vout_avg = 0;
	power_state.current_avg = 0;
	power_state.power_avg = 0;
	power_state.temp_avg = fxp_from_int(-999);

	mpp_state.mppMaxPWM = CONV_PWM_MAX;

	/* initialize mpp_state */
	mpp_state.maxPWM = CONV_PWM_MAX;
	mpp_state.refPWM = CONV_PWM_MAX;

	init_clock();
	init_rtc();
	init_gpio();
	init_adc();
	init_timer();

	lcd_init();
	debug_init();

	debug_send_string("Init complete\r\n");

	//init_systick(1000);

	// triggered every 1 ms
	while (1) {
		if(timebase_ms == 900) {
			adc_power_off(ADC1);
			adc_calibrate(ADC1);
			adc_power_on(ADC1);
		}

		// let the ADC+DMA do its work
		adc_start_conversion_regular(ADC1);

		// *** Do some calculations while ADC converts ***

		if(lcd_setup()) {
			lcd_process();

			if(timebase_ms == force_display_update_time) {
				lcd_set_cursor_pos(1, 0);

				fxp_format(power_state.vin_avg, number, 1);
				fxp_right_align(number, msg, 4, ' ');
				lcd_send_string("I:");
				lcd_send_string(msg);
				lcd_send_string("V ");

				fxp_format(power_state.vout_avg, number, 2);
				fxp_right_align(number, msg, 5, ' ');
				lcd_send_string("O:");
				lcd_send_string(msg);
				lcd_send_string("V ");

				lcd_set_cursor_pos(0, 0);

				fxp_format(power_state.temp_avg, number, 1);
				fxp_right_align(number, msg, 4, ' ');
				lcd_send_string(msg);
				lcd_send_string("C");

				lcd_set_cursor_pos(0, 10);

				fxp_format(power_state.power_avg, number, 2);
				fxp_right_align(number, msg, 5, ' ');
				lcd_send_string(msg);
				lcd_send_string("W");

				force_display_update_time += 500;
			}
		}


		// wait for DMA transfer to complete
		while(!dma_get_interrupt_flag(DMA1, DMA_CHANNEL1, DMA_TCIF) && wait_frame);
		dma_clear_interrupt_flags(DMA1, DMA_CHANNEL1, DMA_TCIF);

		if(timebase_ms % 250 == 0) {
			debug_send_string("ADC: ");
			for(int i = 0; i < ADC_NUM_CHANNELS; i++) {
				fxp_format_int(adc_values[i], msg);
				debug_send_string(msg);
				debug_send_string(" ");
			}
			debug_send_string("\r\n");
		}

		// convert read values
		power_state.vin     = fxp_mult(fxp_from_int(adc_values[0]), VIN_SCALE);
		power_state.vout    = fxp_mult(fxp_from_int(adc_values[1]), VOUT_SCALE);

		if(adc_values[2] <= ADC_VALUE_AT_ZERO_CURRENT+3) {
			power_state.current = 0;
		} else {
			// current = adc • m + t
			power_state.current = fxp_add(fxp_mult(fxp_from_int(adc_values[2]), ADC2CURRENT_M), ADC2CURRENT_T);
		}

		power_state.temp = calc_temperature(adc_values[3]);

		power_state.vin_avg     = fxp_add(fxp_mult(power_state.vin, AVG_FACT),     fxp_mult(power_state.vin_avg, AVG_FACT_INV));
		power_state.vout_avg    = fxp_add(fxp_mult(power_state.vout, AVG_FACT),    fxp_mult(power_state.vout_avg, AVG_FACT_INV));
		power_state.current_avg = fxp_add(fxp_mult(power_state.current, AVG_FACT), fxp_mult(power_state.current_avg, AVG_FACT_INV));
		power_state.temp_avg    = fxp_add(fxp_mult(power_state.temp, AVG_FACT),    fxp_mult(power_state.temp_avg, AVG_FACT_INV));

		power_state.power_avg = fxp_mult(power_state.vout_avg, power_state.current_avg);

		// load management
		if(timebase_ms >= 100) {
			if(!loadInitialized && power_state.vout_avg > LOAD_OFF_THRESHOLD) {
				load_on();
				loadInitialized = 1;
			}

			if(loadInitialized) {
				if(power_state.vout_avg < LOAD_OFF_THRESHOLD) {
					load_off();
				} else if(power_state.vout_avg > LOAD_ON_THRESHOLD) {
					load_on();
				} /* else current state is kept */
			}
		}

		// Main FSM
		if(timebase_ms >= 1000) {
			switch(operState) {
				case Bootstrap:
					// disable converter
					// enable bootstrap pulse with very low duty cycle
					timer_set_oc_value(TIM1, TIM_CH_CONV, 0);
					timer_set_oc_value(TIM1, TIM_CH_BOOTSTRAP, 24);

					if(time_in_state >= 10) { // bootstrap duration in ms
						//iErr = 0;
						operState = nextState;
					}
					break;

				case ConvConstVoltage:
					// bootstrap off
					timer_set_oc_value(TIM1, TIM_CH_BOOTSTRAP, 0);

					// calculate error values
					pErr = fxp_sub(CONST_VOLTAGE, power_state.vout_avg);
					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_CV),
							fxp_mult(iErr, IGAIN_CV));

					pwm = fxp_to_int(controlAction);

					if(pwm > CONV_PWM_MAX) {
						pwm = CONV_PWM_MAX;
					} else if(pwm < 0) {
						pwm = 0;
					}

					timer_set_oc_value(TIM1, TIM_CH_CONV, pwm);

#ifdef DEBUG
					if((time_in_state % 100) == 0) {
						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(" controlAction: ");
						fxp_format(controlAction, msg, 3);
						debug_send_string(msg);
						sentSomething = 1;
					}
#endif

					if(time_in_state > 5000 && fxp_to_int(controlAction) > CONV_PWM_MAX && power_state.current_avg < CURRENT_THRESHOLD) {
						operState = Bootstrap;
						nextState = ConvConstVoltage;
					}

					if(time_in_state > 1000 && power_state.vout_avg < MPP_VOLTAGE_THR) {
						operState = ConvMPP;
					}

					if(power_state.current_avg > MAX_CURRENT) {
						operState = ConvConstCurrent;
					}

					if(power_state.vin_avg < power_state.vout_avg) {
						operState = Idle;
					}
					break;

				case ConvConstCurrent:
					if(time_in_state == 0) {
						iErr = 0;
					}

					// bootstrap off
					timer_set_oc_value(TIM1, TIM_CH_BOOTSTRAP, 0);

					// calculate error values
					pErr = fxp_sub(MAX_CURRENT, power_state.current_avg);
					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_CC),
							fxp_mult(iErr, IGAIN_CC));

					pwm = fxp_to_int(controlAction);

					if(pwm > CONV_PWM_MAX) {
						pwm = CONV_PWM_MAX;
					} else if(pwm < 0) {
						pwm = 0;
					}

					timer_set_oc_value(TIM1, TIM_CH_CONV, pwm);

					if(time_in_state > 5000 && pwm > CONV_PWM_MAX && power_state.current_avg < CURRENT_THRESHOLD) {
						operState = Bootstrap;
						nextState = ConvConstCurrent;
					}

					if(time_in_state > 1000 && power_state.current_avg < MPP_CURRENT_THR) {
						operState = ConvMPP;
					}

					if(power_state.vout_avg > MAX_VOLTAGE) {
						operState = ConvConstVoltage;
					}
					break;

				case ConvMPP:
					// bootstrap off
					timer_set_oc_value(TIM1, TIM_CH_BOOTSTRAP, 0);

					mpp_run(time_in_state, &mpp_state, &power_state, &pwm);

					if(pwm > mpp_state.mppMaxPWM) {
						pwm = mpp_state.mppMaxPWM;
					} else if(pwm < CONV_PWM_PERIOD/100) {
						pwm = CONV_PWM_PERIOD/100;
					}

					timer_set_oc_value(TIM1, TIM_CH_CONV, pwm);

					if((mpp_state.mppMaxPWM < CONV_PWM_MAX) &&
							time_in_state > 5000 && ((time_in_state % 3000) == 10)) {
						mpp_state.mppMaxPWM++;
					}

					if(time_in_state > 5000 && power_state.current_avg < CURRENT_THRESHOLD) {
						operState = Bootstrap;
						nextState = ConvMPP;
						mpp_state.testIdx = -1;

						if(mpp_state.mppMaxPWM > CONV_PWM_MAX/50) {
							mpp_state.mppMaxPWM -= 10;
						} else {
							// limit has gone too low -> retry at maximum
							mpp_state.mppMaxPWM = CONV_PWM_MAX;
						}

						pwm = mpp_state.mppMaxPWM;
					}

					if(power_state.vout_avg > MAX_VOLTAGE) {
						operState = ConvConstVoltage;
					}

					if(power_state.current_avg > MAX_CURRENT) {
						operState = ConvConstCurrent;
					}

					if(power_state.vin_avg < power_state.vout_avg) {
						operState = Idle;
						mpp_state.mppMaxPWM = CONV_PWM_MAX;
					}

#ifdef DEBUG
					if((time_in_state % 100) == 0) {
						debug_send_string("PWM: ");
						fxp_format_int(pwm, msg);
						debug_send_string(msg);
						debug_send_string(" Limit: ");
						fxp_format_int(mpp_state.mppMaxPWM, msg);
						debug_send_string(msg);
						sentSomething = 1;
					}
#endif

					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(time_in_state > 1000 && power_state.vin_avg > fxp_add(power_state.vout_avg, WAKEUP_OFFSET_VOLTAGE)) {
						sleep_time = 10;
						operState = Bootstrap;
						nextState = ConvMPP;
					}

					if(time_in_state > 10000) {
						// not enough power for too long -> put system to deep sleep

						lcd_set_cursor_pos(0, 0);
						lcd_send_string("Sleep(");
						fxp_format_int(sleep_time, msg);
						lcd_send_string(msg);
						lcd_send_string(")        ");
						while(lcd_process() == 0); // send everything immediately

#ifndef DEBUG
						deepsleep(sleep_time);

						// Woke up again.
						lcd_set_cursor_pos(0, 0);
						lcd_send_string("           ");
						time_in_state = 9900; // run the voltage test again
#else
						time_in_state = 0;
#endif

						sleep_time *= 2;
						if(power_state.vout_avg > LOAD_LOW_VOLTAGE_THRESHOLD) {
							if(sleep_time > MAX_SLEEP_TIME) {
								sleep_time = MAX_SLEEP_TIME;
							}
						} else {
							if(sleep_time > MAX_SLEEP_TIME_LOW_VOLTAGE) {
								sleep_time = MAX_SLEEP_TIME_LOW_VOLTAGE;
							}
						}

						force_display_update_time = timebase_ms + 10;
					}

					break;

				default:
					debug_send_string("Invalid state detected!");
					sentSomething = 1;

					operState = Idle;
					break;
			}

			if(operState != lastState) {
				time_in_state = 0;
				lastState = operState;

				lcd_set_cursor_pos(0, 6);
				switch(operState) {
					case Idle:
						lcd_send_string("IDL");
						break;
					case Bootstrap:
						lcd_send_string("BTS");
						break;
					case ConvConstCurrent:
						lcd_send_string("CC ");
						break;
					case ConvConstVoltage:
						lcd_send_string("CV ");
						break;
					case ConvMPP:
						lcd_send_string("MPP");
						break;
					default:
						lcd_send_string("???");
						break;
				}
			} else {
				time_in_state++;
			}
		}

		if(sentSomething) {
			debug_send_string("\r\n");
			sentSomething = 0;
		}

		if((timebase_ms % 1000) == 490) {
			report_status(&power_state, pwm, operState);
		}

		/*
		if((timebase_ms % 1000) == 10) {
			cpuload /= 1000;

			// use CPU load values here

			cpuload = 0;
		}

		// cpu load = timer1 value after main loop operations
		cpuload += timer_get_counter(TIM3);
		*/

		timebase_ms++;

		while(wait_frame) {
			__WFI();
		}

		wait_frame = 1;
	}

	return 0;
}

/* Called when systick fires */
void sys_tick_handler(void)
{
	wait_frame = 0;
}

void tim3_isr(void)
{
	// check for update interrupt
	if(timer_interrupt_source(TIM3, TIM_SR_UIF)) {
		wait_frame = 0;
		timer_clear_flag(TIM3, TIM_SR_UIF);
	}
}

void rtc_isr(void)
{
	exti_reset_request(EXTI17);
}

void hard_fault_handler(void)
{
	while (1);
}