107 lines
2.4 KiB
C++
107 lines
2.4 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;
|
|
}
|
|
|
|
void CalibrateCompass::start() {
|
|
if (this->state != State::Ready)
|
|
return;
|
|
|
|
this->clearData();
|
|
this->state = State::Calibrating;
|
|
this->compass->setCalibration(0, 0, 0, 0, 0, 0);
|
|
this->lastChange = millis();
|
|
}
|
|
|
|
void CalibrateCompass::useData() {
|
|
if (this->state != State::Finished)
|
|
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]
|
|
)
|
|
}
|
|
|
|
void CalibrateCompass::resetData() {
|
|
this->reset();
|
|
this->compass->setCalibration(0, 0, 0, 0, 0, 0);
|
|
}
|
|
|
|
void CalibrateCompass::reset() {
|
|
this->clearData();
|
|
this->state = State::Ready;
|
|
}
|
|
|
|
void CalibrateCompass::clearData() {
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
this->data.data[i][0] = 0;
|
|
this->data.data[i][1] = 0;
|
|
}
|
|
}
|