LNSC-2420-Firmware/src/measurement.c

#include <libopencm3/stm32/adc.h>
#include <libopencm3/stm32/dma.h>

#include <fxp.h>

#include "libopencm3/stm32/f0/adc.h"
#include "pinout.h"

#include "measurement.h"
#include "calibration.h"
#include "config.h"

#define ADC_NUM_CHANNELS 6
static volatile int16_t adc_values[ADC_NUM_CHANNELS];

static fxp_t calibration_factors[ADC_NUM_CHANNELS-1]; // all except temperature; filled in measurement_init()

fxp_t avg_alpha_i_solar;
fxp_t avg_alpha_i_load;
fxp_t avg_alpha_u_bat;
fxp_t avg_alpha_u_sw;
fxp_t avg_alpha_u_solar;
fxp_t avg_alpha_temp;

fxp_t avg_alpha_i_solar_inv;
fxp_t avg_alpha_i_load_inv;
fxp_t avg_alpha_u_bat_inv;
fxp_t avg_alpha_u_sw_inv;
fxp_t avg_alpha_u_solar_inv;
fxp_t avg_alpha_temp_inv;

/* 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 ((int32_t) (330)) /* calibration voltage = 3,30V - DO NOT CHANGE */
#define VDD_APPLI ((int32_t) (330)) /* actual supply voltage */

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

	fxp_t temperature = FXP_FROM_INT(temperature_raw);

	fxp_t scale_dividend = FXP_FROM_INT(110 - 30);
	fxp_t scale_divisor = FXP_FROM_INT((int32_t)(*TEMP110_CAL_ADDR - *TEMP30_CAL_ADDR));

	fxp_t scale = fxp_div(scale_dividend, scale_divisor);

	return fxp_add(fxp_mult(temperature, scale), FXP_FROM_INT(30));
}


static fxp_t adc_val_to_pin_voltage(uint16_t adc_val)
{
	return fxp_div(
	           fxp_mult(FXP_FROM_INT(adc_val), fxp_div(FXP_FROM_INT(33), FXP_FROM_INT(10))),
	           FXP_FROM_INT(4096));
}


static fxp_t convert_voltage_divider(uint16_t adc_val, fxp_t r1, fxp_t r2, fxp_t cal)
{
	fxp_t pin_voltage = adc_val_to_pin_voltage(adc_val);

	fxp_t meas_voltage = fxp_mult(pin_voltage, fxp_div(fxp_add(r1, r2), r2));

	return fxp_mult(meas_voltage, cal);
}


static fxp_t convert_ina139(uint16_t adc_val, fxp_t rshunt, fxp_t vgain, fxp_t cal)
{
	fxp_t pin_voltage = adc_val_to_pin_voltage(adc_val);

	fxp_t shunt_voltage = fxp_div(pin_voltage, vgain);
	fxp_t current = fxp_div(shunt_voltage, rshunt);

	return fxp_mult(current, cal);
}


void measurement_init(void)
{
	uint8_t channels[ADC_NUM_CHANNELS] = {
		ANALOG_INPUT_U_BAT,   // U_Bat
		ANALOG_INPUT_U_SOLAR, // U_Solar
		ANALOG_INPUT_U_SW,    // U_SW
		ANALOG_INPUT_I_SOLAR, // I_Solar
		ANALOG_INPUT_I_LOAD,  // I_Load
		ADC_CHANNEL_TEMP      // Temperature sensor
	};

	// Convert calibration factors to fixed-point numbers for direct use
	calibration_factors[ANALOG_INPUT_U_BAT] =
		fxp_div(FXP_FROM_INT(CAL_FACTOR_U_BAT), FXP_FROM_INT(1000));
	calibration_factors[ANALOG_INPUT_U_SOLAR] =
		fxp_div(FXP_FROM_INT(CAL_FACTOR_U_SOLAR), FXP_FROM_INT(1000));
	calibration_factors[ANALOG_INPUT_U_SW] =
		fxp_div(FXP_FROM_INT(CAL_FACTOR_U_SW), FXP_FROM_INT(1000));
	calibration_factors[ANALOG_INPUT_I_SOLAR] =
		fxp_div(FXP_FROM_INT(CAL_FACTOR_I_SOLAR), FXP_FROM_INT(1000));
	calibration_factors[ANALOG_INPUT_I_LOAD] =
		fxp_div(FXP_FROM_INT(CAL_FACTOR_I_LOAD), FXP_FROM_INT(1000));

	// Convert and precalculate coefficients for exponential averaging
	avg_alpha_i_solar = fxp_div(FXP_FROM_INT(AVG_ALPHA_I_SOLAR), FXP_FROM_INT(1000));
	avg_alpha_i_load  = fxp_div(FXP_FROM_INT(AVG_ALPHA_I_LOAD), FXP_FROM_INT(1000));
	avg_alpha_u_bat   = fxp_div(FXP_FROM_INT(AVG_ALPHA_U_BAT), FXP_FROM_INT(1000));
	avg_alpha_u_sw    = fxp_div(FXP_FROM_INT(AVG_ALPHA_U_SW), FXP_FROM_INT(1000));
	avg_alpha_u_solar = fxp_div(FXP_FROM_INT(AVG_ALPHA_U_SOLAR), FXP_FROM_INT(1000));
	avg_alpha_temp    = fxp_div(FXP_FROM_INT(AVG_ALPHA_TEMP), FXP_FROM_INT(1000));

	// Inverse (1 - alpha) exponential averaging coefficients
	avg_alpha_i_solar_inv = fxp_sub(FXP_FROM_INT(1), avg_alpha_i_solar);
	avg_alpha_i_load_inv  = fxp_sub(FXP_FROM_INT(1), avg_alpha_i_load);
	avg_alpha_u_bat_inv   = fxp_sub(FXP_FROM_INT(1), avg_alpha_u_bat);
	avg_alpha_u_sw_inv    = fxp_sub(FXP_FROM_INT(1), avg_alpha_u_sw);
	avg_alpha_u_solar_inv = fxp_sub(FXP_FROM_INT(1), avg_alpha_u_solar);
	avg_alpha_temp_inv    = fxp_sub(FXP_FROM_INT(1), avg_alpha_temp);

	// Prepare the ADC
	adc_power_off(ADC1);

	// enable the temperature sensor
	adc_enable_temperature_sensor();

	// configure ADC
	//adc_enable_scan_mode(ADC1);
	adc_set_clk_source(ADC1, ADC_CLKSOURCE_PCLK_DIV4); // -> 12 MHz @ 48 MHz ABP
	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);

	adc_calibrate(ADC1);

	// 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);
}


void measurement_start(void)
{
	// start the ADC conversion sequency. The result will be transferred to RAM
	// by the DMA.
	adc_start_conversion_regular(ADC1);
}


void measurement_wait_for_completion(void)
{
	// wait for DMA transfer to complete
	while(!dma_get_interrupt_flag(DMA1, DMA_CHANNEL1, DMA_TCIF));
	dma_clear_interrupt_flags(DMA1, DMA_CHANNEL1, DMA_TCIF);
}


void measurement_finalize(struct MeasurementResult *result)
{
	result->u_bat = convert_voltage_divider(adc_values[ANALOG_INPUT_U_BAT],
	                                        FXP_FROM_INT(220),
	                                        FXP_FROM_INT(22),
	                                        calibration_factors[ANALOG_INPUT_U_BAT]);

	result->u_solar = convert_voltage_divider(adc_values[ANALOG_INPUT_U_SOLAR],
	                                          FXP_FROM_INT(330),
	                                          FXP_FROM_INT(22),
	                                          calibration_factors[ANALOG_INPUT_U_SOLAR]);

	result->u_sw = convert_voltage_divider(adc_values[ANALOG_INPUT_U_SW],
	                                       FXP_FROM_INT(1000),
	                                       FXP_FROM_INT(47),
	                                       calibration_factors[ANALOG_INPUT_U_SW]);

	result->i_solar = convert_ina139(adc_values[ANALOG_INPUT_I_SOLAR],
	                                 fxp_div(FXP_FROM_INT(2), FXP_FROM_INT(1000)),
	                                 FXP_FROM_INT(56),
	                                 calibration_factors[ANALOG_INPUT_I_SOLAR]);

	result->i_load = convert_ina139(adc_values[ANALOG_INPUT_I_LOAD],
	                                fxp_div(FXP_FROM_INT(5), FXP_FROM_INT(1000)),
	                                FXP_FROM_INT(56),
	                                calibration_factors[ANALOG_INPUT_I_LOAD]);

	result->temperature = calc_temperature(adc_values[5]);

	/* calculate exponentially averaged values */
	result->avg_u_bat =
		fxp_add(
				fxp_mult(avg_alpha_u_bat, result->u_bat),
				fxp_mult(avg_alpha_u_bat_inv, result->avg_u_bat));

	result->avg_u_solar =
		fxp_add(
				fxp_mult(avg_alpha_u_solar, result->u_solar),
				fxp_mult(avg_alpha_u_solar_inv, result->avg_u_solar));

	result->avg_u_sw =
		fxp_add(
				fxp_mult(avg_alpha_u_sw, result->u_sw),
				fxp_mult(avg_alpha_u_sw_inv, result->avg_u_sw));

	result->avg_i_solar =
		fxp_add(
				fxp_mult(avg_alpha_i_solar, result->i_solar),
				fxp_mult(avg_alpha_i_solar_inv, result->avg_i_solar));

	result->avg_i_load =
		fxp_add(
				fxp_mult(avg_alpha_i_load, result->i_load),
				fxp_mult(avg_alpha_i_load_inv, result->avg_i_load));

	result->avg_temperature =
		fxp_add(
				fxp_mult(avg_alpha_temp, result->temperature),
				fxp_mult(avg_alpha_temp_inv, result->avg_temperature));
}