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 r1, uint32_t r2) {
    this->pin = pin;
    this->r1 = r1;
    this->r2 = r2;
    this->batteryVoltageFactor = (double) (this->r1 + this->r2) / (double) this->r2;
    this->initBuffer();
    this->loopDelay = 100;
}

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

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

    return;
}

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

    this->readAdcToBuf();        
    if (this->bufferPos == 0) {
        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 == 60) {
            this->calibrationState = CalibrationState::Finished;
        }
    }
}

double Battery::getBatteryVoltage() const {
    double res = this->batteryVoltage;
    return (int)(res*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;
    this->loopDelay = 50;
    this->newRawAdcVoltages = new uint16_t[60];
}

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

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

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 > 4095) return 0;
    return  - 0.000000000000016 * pow(reading,4)
            + 0.000000000118171 * pow(reading,3)
            - 0.000000301211691 * pow(reading,2)
            + 0.001109019271794 * reading
            + 0.034143524634089;
}

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

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

    if (adcValue > this->rawAdcVoltages[this->rawAdcVoltagesCount] + 50) {
        this->batteryVoltage = -2;
        return;
    }

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

    // std::cout << "Battery::calculateBatteryVoltage() - Voltage: " << voltage << " Index: " <<(int) index << " adcValue: " << (int) adcValue <<" iD: " << indexDelta << " vD: " << valueDelta << std::endl;
}

void Battery::calculateBatteryPercent() {
    int8_t size = sizeof(this->capacityVoltages) / sizeof(*this->capacityVoltages);
    uint8_t i;
    for (i = 0; i < size; i++) {
        if (this->batteryVoltage <= this->capacityVoltages[i])
            break;
    }
    
    if (i == 0) {
        if (this->batteryVoltage > 6)
            std::cout << "Critical low battery!" << std::endl;
    } else if (i == size - 1) {

    } else {
        double diffToLowerVal = this->batteryVoltage - this->capacityVoltages[i - 1];
        double diffToHigherVal = this->capacityVoltages[i] - this->batteryVoltage;
        if (diffToLowerVal > diffToHigherVal)
            i--;
    }
    this->batteryPercent = i * (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);
}