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

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

    bool changed = false;

    this->compass->read();
    int x = this->compass->getX();
    int y = this->compass->getY();
    int z = this->compass->getZ();

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

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

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

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

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

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

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

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

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