wideband/firmware/boards/f1_dual_rev1/port.cpp

#include "port.h"


#include "wideband_config.h"

#include "hal.h"

#define ADC_CHANNEL_COUNT 10
#define ADC_SAMPLE ADC_SAMPLE_7P5

static adcsample_t adcBuffer[ADC_CHANNEL_COUNT * ADC_OVERSAMPLE];

const ADCConversionGroup convGroup =
{
    .circular = false,
    .num_channels = ADC_CHANNEL_COUNT,
    .end_cb = nullptr,
    .error_cb = nullptr,
    .cr1 = 0,
    .cr2 =
        ADC_CR2_CONT/* |
        ADC_CR2_ADON*/,   /* keep ADC enabled between convertions - for GD32 */
    .smpr1 =
        ADC_SMPR1_SMP_AN10(ADC_SAMPLE) |
        ADC_SMPR1_SMP_AN11(ADC_SAMPLE) |
        ADC_SMPR1_SMP_AN12(ADC_SAMPLE) | /* PC2 - ADC123_IN12 - L_Un_3x_sense */
        ADC_SMPR1_SMP_AN13(ADC_SAMPLE) | /* PC3 */
        ADC_SMPR1_SMP_AN14(ADC_SAMPLE) |
        ADC_SMPR1_SMP_AN15(ADC_SAMPLE),  /* PC5 */
    .smpr2 =
        ADC_SMPR2_SMP_AN0(ADC_SAMPLE) | /* PA0 */
        ADC_SMPR2_SMP_AN1(ADC_SAMPLE) | /* PA1 - ADC12_IN1 - R_Un_3x_sense */
        ADC_SMPR2_SMP_AN2(ADC_SAMPLE) | /* PA2 */
        ADC_SMPR2_SMP_AN3(ADC_SAMPLE) | /* PA3 */
        ADC_SMPR2_SMP_AN4(ADC_SAMPLE) |
        ADC_SMPR2_SMP_AN5(ADC_SAMPLE) |
        ADC_SMPR2_SMP_AN6(ADC_SAMPLE) | /* PA6 */
        ADC_SMPR2_SMP_AN7(ADC_SAMPLE) | /* PA7 */
        ADC_SMPR2_SMP_AN8(ADC_SAMPLE) | /* PB8 */
        ADC_SMPR2_SMP_AN9(ADC_SAMPLE),
    .sqr1 = ADC_SQR1_NUM_CH(ADC_CHANNEL_COUNT),
    .sqr2 =
        /* TODO: move these two channels to slow ADC! */
        ADC_SQR2_SQ7_N(15) | /* PC5 - ADC12_IN15 - L_Heater_sense */
        ADC_SQR2_SQ8_N(8)  | /* PB0 - ADC12_IN8 - R_Heater_sense */
        ADC_SQR2_SQ9_N(2)  | /* PA2 - ADC12_IN2 - R_Un_sense */
        ADC_SQR2_SQ10_N(3),  /* PA3 - ADC12_IN3 - L_Un_sense */
    .sqr3 =
        ADC_SQR3_SQ1_N(0)  | /* PA0 - ADC12_IN0 - R_Ip_sense */
        ADC_SQR3_SQ2_N(1)  | /* PA1 - ADC12_IN1 - R_Un_3x_sense */
        ADC_SQR3_SQ3_N(13) | /* PC3 - ADC123_IN13 - L_Ip_sense */
        ADC_SQR3_SQ4_N(12) | /* PC2 - ADC123_IN12 - L_Un_3x_sense */
        /* TODO: move these two channels to slow ADC! */
        ADC_SQR3_SQ5_N(6)  | /* PA6 - ADC12_IN6 - R_AUX_ADC */
        ADC_SQR3_SQ6_N(7),   /* PA7 - ADC12_IN7 - L_AUX_ADC */
};

static float AverageSamples(adcsample_t* buffer, size_t idx)
{
    uint32_t sum = 0;

    for (size_t i = 0; i < ADC_OVERSAMPLE; i++)
    {
        sum += buffer[idx];
        idx += ADC_CHANNEL_COUNT;
    }

    constexpr float scale = VCC_VOLTS / (ADC_MAX_COUNT * ADC_OVERSAMPLE);

    return (float)sum * scale;
}

static float GetMaxSample(adcsample_t* buffer, size_t idx)
{
    adcsample_t max = 0;

    for (size_t i = 0; i < ADC_OVERSAMPLE; i++)
    {
        if (buffer[idx] > max)
        {
            max = buffer[idx];
        }

        idx += ADC_CHANNEL_COUNT;
    }

    constexpr float scale = VCC_VOLTS / ADC_MAX_COUNT;

    return (float)max * scale;
}

static float l_vbatt = 0;
static float r_vbatt = 0;

AnalogResult AnalogSample()
{
    AnalogResult res;
    /* TODO: remove Vbat measurement through heaters
     * TODO: keep heater voltage measurement for optional source for pwm calculation
     * TODO: add aux output voltage measurement for diagnostic (use slow ADC?) */
    bool l_heater = !palReadPad(L_HEATER_PORT, L_HEATER_PIN);
    bool r_heater = !palReadPad(R_HEATER_PORT, R_HEATER_PIN);

    adcConvert(&ADCD1, &convGroup, adcBuffer, ADC_OVERSAMPLE);

    bool l_heater_new = !palReadPad(L_HEATER_PORT, L_HEATER_PIN);
    bool r_heater_new = !palReadPad(R_HEATER_PORT, R_HEATER_PIN);

    if (l_heater && l_heater_new)
    {
        float vbatt_raw = GetMaxSample(adcBuffer, 6) / BATTERY_INPUT_DIVIDER;
        l_vbatt = BATTERY_FILTER_ALPHA * vbatt_raw + (1.0 - BATTERY_FILTER_ALPHA) * l_vbatt;
    }

    if (r_heater && r_heater_new)
    {
        float vbatt_raw = GetMaxSample(adcBuffer, 7) / BATTERY_INPUT_DIVIDER;
        r_vbatt = BATTERY_FILTER_ALPHA * vbatt_raw + (1.0 - BATTERY_FILTER_ALPHA) * r_vbatt;
    }

    /* Dual board has separate internal virtual ground = 3.3V / 2
     * VirtualGroundVoltageInt is used to calculate Ip current only as it
     * is used as offset for diffirential amp */
    res.VirtualGroundVoltageInt = HALF_VCC;

    for (int i = 0; i < AFR_CHANNELS; i++) {
        float NernstRaw = AverageSamples(adcBuffer, (i == 0) ? 3 : 1);
        if ((NernstRaw > 0.01) && (NernstRaw < (3.3 - 0.01))) {
            /* not clamped */
            res.ch[i].NernstVoltage = (NernstRaw - NERNST_INPUT_OFFSET) * NERNST_INPUT_GAIN;
        } else {
            /* Clamped, use ungained input */
            res.ch[i].NernstVoltage = AverageSamples(adcBuffer, (i == 0) ? 9 : 8) - HALF_VCC;
        }
    }
    /* left */
    res.ch[0].PumpCurrentVoltage = AverageSamples(adcBuffer, 2);
    res.ch[0].BatteryVoltage = l_vbatt;
    /* right */
    res.ch[1].PumpCurrentVoltage = AverageSamples(adcBuffer, 0);
    res.ch[1].BatteryVoltage = r_vbatt;

    return res;
}

/* TODO: optimize */
void SetupESRDriver(SensorType sensor)
{
    switch (sensor) {
        case SensorType::LSU42:
            /* disable bias */
            palSetPadMode(NERNST_49_BIAS_PORT, NERNST_49_BIAS_PIN,
                PAL_MODE_INPUT);
            /* disable all others ESR drivers */
            palSetPadMode(NERNST_49_ESR_DRIVER_PORT, NERNST_49_ESR_DRIVER_PIN,
                PAL_MODE_INPUT);
            palSetPadMode(NERNST_ADV_ESR_DRIVER_PORT, NERNST_ADV_ESR_DRIVER_PIN,
                PAL_MODE_INPUT);
            /* enable LSU4.2 */
            palSetPadMode(NERNST_42_ESR_DRIVER_PORT, NERNST_42_ESR_DRIVER_PIN,
                PAL_MODE_OUTPUT_PUSHPULL);
        break;
        case SensorType::LSU49:
            /* disable all others ESR drivers */
            palSetPadMode(NERNST_42_ESR_DRIVER_PORT, NERNST_42_ESR_DRIVER_PIN,
                PAL_MODE_INPUT);
            palSetPadMode(NERNST_ADV_ESR_DRIVER_PORT, NERNST_ADV_ESR_DRIVER_PIN,
                PAL_MODE_INPUT);
            /* enable LSU4.2 */
            palSetPadMode(NERNST_49_ESR_DRIVER_PORT, NERNST_49_ESR_DRIVER_PIN,
                PAL_MODE_OUTPUT_PUSHPULL);
            /* enable bias */
            palSetPadMode(NERNST_49_BIAS_PORT, NERNST_49_BIAS_PIN,
                PAL_MODE_OUTPUT_PUSHPULL);
            palSetPad(NERNST_49_BIAS_PORT, NERNST_49_BIAS_PIN);
        break;
        case SensorType::LSUADV:
            /* disable bias */
            palSetPadMode(NERNST_49_BIAS_PORT, NERNST_49_BIAS_PIN,
                PAL_MODE_INPUT);
            /* disable all others ESR drivers */
            palSetPadMode(NERNST_49_ESR_DRIVER_PORT, NERNST_49_ESR_DRIVER_PIN,
                PAL_MODE_INPUT);
            palSetPadMode(NERNST_42_ESR_DRIVER_PORT, NERNST_42_ESR_DRIVER_PIN,
                PAL_MODE_INPUT);
            /* enable LSU4.2 */
            palSetPadMode(NERNST_ADV_ESR_DRIVER_PORT, NERNST_ADV_ESR_DRIVER_PIN,
                PAL_MODE_OUTPUT_PUSHPULL);
        break;
    }
}

int GetESRSupplyR()
{
    switch (GetSensorType()) {
        case SensorType::LSU42:
            return 6800;
        case SensorType::LSU49:
            return 22000;
        case SensorType::LSUADV:
            return 47000;
    }
    return 0;
}