Bachelorarbeit-Rover/lib/calibrateCompass/calibrateCompass.cpp

/**
 * @file calibrateCompass.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief
 * @version 0.1
 * @date 2023-05-23
 *
 * @copyright Copyright (c) 2023
 *
 */

#include "calibrateCompass.h"

CalibrateCompass::CalibrateCompass(QMC5883LCompass *compass)
{
    this->compass = compass;
    this->state = State::Ready;
    this->clearData();
    this->activateOnlyChilds();
}

void CalibrateCompass::runAsChild()
{
    if (this->state != State::Calibrating)
    {
        return;
    }

    bool changed = false;

    this->compass->read();
    int xAxis = this->compass->getX();
    int yAxis = this->compass->getY();
    int zAxis = this->compass->getZ();

    if (xAxis < this->data.data[0][0])
    {
        this->data.data[0][0] = xAxis;
        changed = true;
    }

    if (xAxis > this->data.data[0][1])
    {
        this->data.data[0][1] = xAxis;
        changed = true;
    }

    if (yAxis < this->data.data[1][0])
    {
        this->data.data[1][0] = yAxis;
        changed = true;
    }

    if (yAxis > this->data.data[1][1])
    {
        this->data.data[1][1] = yAxis;
        changed = true;
    }

    if (zAxis < this->data.data[2][0])
    {
        this->data.data[2][0] = zAxis;
        changed = true;
    }

    if (zAxis > this->data.data[2][1])
    {
        this->data.data[2][1] = zAxis;
        changed = true;
    }

    if (changed)
        this->lastChange = millis();

    if (millis() - this->lastChange > this->maxTimeWithoutChange)
    {
        this->state = State::Finished;
        this->checkDataValidity();
    }
}

void CalibrateCompass::run() {}

void CalibrateCompass::start()
{
    if (this->state != State::Ready)
    {
        return;
    }

    this->clearData();
    this->state = State::Calibrating;
    this->compass->clearCalibration();
    this->lastChange = millis();
}

void CalibrateCompass::useData()
{
    if (!this->dataValid)
    {
        std::cout << "CalibrateCompass::useData - Data not valid" << std::endl;
        return;
    }

    this->compass->setCalibration(this->data.data[0][0],
                                  this->data.data[0][1],
                                  this->data.data[1][0],
                                  this->data.data[1][1],
                                  this->data.data[2][0],
                                  this->data.data[2][1]);

    std::cout << "CalibrateCompass::useData " << *this << std::endl;
}

void CalibrateCompass::removeCalibration()
{
    this->compass->clearCalibration();
}

void CalibrateCompass::reset()
{
    this->clearData();
    this->state = State::Ready;
}

void CalibrateCompass::saveData()
{
    if (!this->dataValid)
    {
        return;
    }

    Preferences preferences;
    preferences.begin("compass", false);

    preferences.putInt("xLow", this->data.data[0][0]);
    preferences.putInt("xHigh", this->data.data[0][1]);
    preferences.putInt("yLow", this->data.data[1][0]);
    preferences.putInt("yHigh", this->data.data[1][1]);
    preferences.putInt("zLow", this->data.data[2][0]);
    preferences.putInt("zHigh", this->data.data[2][1]);

    preferences.end();
}

void CalibrateCompass::loadData()
{
    Preferences preferences;
    preferences.begin("compass", true);

    this->data.data[0][0] = preferences.getInt("xLow", 0);
    this->data.data[0][1] = preferences.getInt("xHigh", 0);
    this->data.data[1][0] = preferences.getInt("yLow", 0);
    this->data.data[1][1] = preferences.getInt("yHigh", 0);
    this->data.data[2][0] = preferences.getInt("zLow", 0);
    this->data.data[2][1] = preferences.getInt("zHigh", 0);

    preferences.end();
    this->checkDataValidity();
}

void CalibrateCompass::clearData()
{
    for (uint8_t i = 0; i < 3; i++)
    {
        this->data.data[i][0] = 0;
        this->data.data[i][1] = 0;
    }
    this->dataValid = false;
}

void CalibrateCompass::checkDataValidity()
{
    int sum = 0;
    for (uint8_t i = 0; i < 3; i++)
    {
        if (this->data.data[i][0] > INT16_MAX || this->data.data[i][0] < INT16_MIN || this->data.data[i][1] > INT16_MAX || this->data.data[i][1] < INT16_MIN)
        {
            this->dataValid = false;
            return;
        }
        sum += this->data.data[i][0];
        sum += this->data.data[i][1];
    }
    this->dataValid = sum;
}

std::ostream &operator<<(std::ostream &os, const CalibrateCompass &caliComp)
{
    os << "(";
    os << caliComp.data.data[0][0];
    os << ", ";
    os << caliComp.data.data[0][1];
    os << ", ";
    os << caliComp.data.data[1][0];
    os << ", ";
    os << caliComp.data.data[1][1];
    os << ", ";
    os << caliComp.data.data[2][0];
    os << ", ";
    os << caliComp.data.data[2][1];
    os << ")";
    return os;
}