156 lines
3.9 KiB
C++
156 lines
3.9 KiB
C++
/**
|
|
* @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->newData = true;
|
|
}
|
|
}
|
|
|
|
void CalibrateCompass::start() {
|
|
if (this->state != State::Ready)
|
|
return;
|
|
|
|
this->clearData();
|
|
this->state = State::Calibrating;
|
|
this->compass->removeCalibration();
|
|
this->lastChange = millis();
|
|
}
|
|
|
|
void CalibrateCompass::useData() {
|
|
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::resetData() {
|
|
this->reset();
|
|
this->compass->removeCalibration();
|
|
}
|
|
|
|
void CalibrateCompass::reset() {
|
|
this->clearData();
|
|
this->state = State::Ready;
|
|
}
|
|
|
|
void CalibrateCompass::saveData() {
|
|
if (!this->newData)
|
|
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();
|
|
}
|
|
|
|
void CalibrateCompass::clearData() {
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
this->data.data[i][0] = 0;
|
|
this->data.data[i][1] = 0;
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|