Bachelorarbeit-Rover/lib/Battery/battery.cpp

/**
 * @file battery.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains the implementation of the class Battery
 * @version 0.1
 * @date 2022-02-05
 *
 * @copyright Copyright (c) 2022
 *
 */

#include "battery.h"

Battery::Battery(uint8_t pin, uint32_t firstResistor, uint32_t secondResistor)
    : pin{pin}, firstResistor{firstResistor}, secondResistor{secondResistor},
      batteryVoltageFactor{firstResistor + secondResistor / static_cast<double>(secondResistor)}
{
    this->initBuffer();
    Component::loopDelay = Battery::loopDelay;
}

Battery::Battery(uint8_t pin)
    : pin{pin}
{
    this->initBuffer();
    Component::loopDelay = Battery::loopDelay;
}

void Battery::run()
{
    if (this->calibrationState != CalibrationState::None)
    {
        this->runCalibration();
        return;
    }

    this->readAdcToBuf();
    this->loopCounter++;

    if (this->loopCounter >= this->calulationDelayMultiplier)
    {
        this->calculateBatteryVoltage();
        this->calculateBatteryPercent();
        this->loopCounter = 0;
        this->calculatetNewValues = true;
    }
}

void Battery::runCalibration()
{
    if (this->calibrationState != CalibrationState::Reading)
    {
        return;
    }

    this->readAdcToBuf();
    if (this->bufferPos == 0)
    {
        const uint16_t res = this->getBufAvg();
        this->newRawAdcVoltages[this->currentCalibrationVoltage] = res;
        std::cout << "Index: "
                  << (int)this->currentCalibrationVoltage
                  << " Value: "
                  << (int)res
                  << std::endl;
        this->currentCalibrationVoltage++;
        this->calibrationState = CalibrationState::Waiting;

        if (this->currentCalibrationVoltage == Battery::rawAdcVoltagesCount)
        {
            this->calibrationState = CalibrationState::Finished;
        }
    }
}

double Battery::getBatteryVoltage() const
{
    return static_cast<int>((this->batteryVoltage * 100 + 0.5)) / 100.0;
}

bool Battery::isBatteryLow(double voltage) const
{
    if (this->getBatteryVoltage() <= voltage && this->batteryVoltage > this->absurdLowVoltage)
    {
        return true;
    }
    return false;
}

bool Battery::isNewValue()
{
    if (!this->calculatetNewValues)
    {
        return false;
    }

    this->calculatetNewValues = false;
    return true;
}

void Battery::nextVoltageIsReady()
{
    if (this->calibrationState == CalibrationState::Waiting)
    {
        this->calibrationState = CalibrationState::Reading;
    }
}

void Battery::startCalibration()
{
    this->calibrationState = CalibrationState::Waiting;
    this->currentCalibrationVoltage = 0;
    this->bufferPos = 0;
    Component::loopDelay = Battery::loopDelay / 2;
    this->newRawAdcVoltages = new uint16_t[Battery::rawAdcVoltagesCount];
}

void Battery::finishCalibration()
{
    if (this->calibrationState != CalibrationState::None)
    {
        return;
    }

    delete[] this->newRawAdcVoltages;
    this->calibrationState = CalibrationState::None;
    Component::loopDelay = Battery::loopDelay;
}

double Battery::calculateInputVoltage()
{
    // Reference voltage is 3v3 so maximum reading is 3v3 = 4095 in range 0 to 4095
    double reading = this->getBufAvg();
    if (reading < 1 || reading > Battery::adcMaxValue)
    {
        return 0;
    }

    return -this->adcCurveCoeficient[0] * pow(reading, 4) + this->adcCurveCoeficient[1] * pow(reading, 3) - this->adcCurveCoeficient[2] * pow(reading, 2) + this->adcCurveCoeficient[3] * reading + this->adcCurveCoeficient[4];
}

void Battery::calculateBatteryVoltage()
{
    if (this->firstResistor && this->secondResistor)
    {
        this->batteryVoltage = this->calculateInputVoltage() * this->batteryVoltageFactor;
        return;
    }

    const uint16_t adcValue = this->getBufAvg();
    if (adcValue < this->rawAdcVoltages[0])
    {
        this->batteryVoltage = -1;
        return;
    }

    uint8_t index = 1;
    for (; index < this->rawAdcVoltagesCount; index++)
    {
        if (adcValue < this->rawAdcVoltages[index])
        {
            break;
        }
    }
    const double indexDelta = this->rawAdcVoltages[index] - this->rawAdcVoltages[index - 1];
    const double valueDelta = this->rawAdcVoltages[index] - adcValue;
    double voltage = this->startVoltage + (index - 1) * this->stepVoltage;
    voltage += valueDelta / indexDelta * this->stepVoltage;
    this->batteryVoltage = voltage;
}

void Battery::calculateBatteryPercent()
{
    int8_t size = sizeof(this->capacityVoltages) / sizeof(*this->capacityVoltages);
    uint8_t index = 0;
    for (; index < size; index++)
    {
        if (this->batteryVoltage <= this->capacityVoltages[index])
        {
            break;
        }
    }

    if (index == 0)
    {
        if (this->batteryVoltage > this->absurdLowVoltage)
        {
            std::cout << "Critical low battery!" << std::endl;
        }
    }
    else if (index == size - 1)
    {
    }
    else
    {
        const double diffToLowerVal = this->batteryVoltage - this->capacityVoltages[index - 1];
        const double diffToHigherVal = this->capacityVoltages[index] - this->batteryVoltage;
        if (diffToLowerVal > diffToHigherVal)
        {
            index--;
        }
    }
    this->batteryPercent = index * (100 / (size - 1));
}

void Battery::readAdcToBuf()
{
    this->adcBuffer[this->bufferPos] = analogRead(this->pin);
    this->bufferPos++;
    if (this->bufferPos == Battery::bufferSize)
    {
        this->bufferPos = 0;
    }

    // std::cout << "Battery::readAdcToBuf added Value: " << this->adcBuffer[this->bufferPos] << std::endl;
}

void Battery::initBuffer()
{
    for (uint8_t i = 0; i < Battery::bufferSize; i++)
    {
        this->adcBuffer[i] = 0;
    }
}

uint16_t Battery::getBufAvg() const
{
    uint32_t res = 0;
    uint8_t emptyPos = 0;
    for (uint8_t i = 0; i < Battery::bufferSize; i++)
    {
        if (this->adcBuffer[i] == 0)
        {
            emptyPos++;
        }
        res += this->adcBuffer[i];
    }
    return res / (Battery::bufferSize - emptyPos);
}