void RTIMU::calibrateAverageCompass()
{
// see if need to do runtime mag calibration (i.e. no stored calibration data)
if (!m_compassCalibrationMode && !m_settings->m_compassCalValid) {
// try runtime calibration
bool changed = false;
// see if there is a new max or min
if (m_runtimeMagCalMax[0] < m_imuData.compass.x()) {
m_runtimeMagCalMax[0] = m_imuData.compass.x();
changed = true;
}
if (m_runtimeMagCalMax[1] < m_imuData.compass.y()) {
m_runtimeMagCalMax[1] = m_imuData.compass.y();
changed = true;
}
if (m_runtimeMagCalMax[2] < m_imuData.compass.z()) {
m_runtimeMagCalMax[2] = m_imuData.compass.z();
changed = true;
}
if (m_runtimeMagCalMin[0] > m_imuData.compass.x()) {
m_runtimeMagCalMin[0] = m_imuData.compass.x();
changed = true;
}
if (m_runtimeMagCalMin[1] > m_imuData.compass.y()) {
m_runtimeMagCalMin[1] = m_imuData.compass.y();
changed = true;
}
if (m_runtimeMagCalMin[2] > m_imuData.compass.z()) {
m_runtimeMagCalMin[2] = m_imuData.compass.z();
changed = true;
}
// now see if ranges are sufficient
if (changed) {
float delta;
if (!m_runtimeMagCalValid) {
m_runtimeMagCalValid = true;
for (int i = 0; i < 3; i++)
{
delta = m_runtimeMagCalMax[i] - m_runtimeMagCalMin[i];
if ((delta < RTIMU_RUNTIME_MAGCAL_RANGE) || (m_runtimeMagCalMin[i] > 0) || (m_runtimeMagCalMax[i] < 0))
{
m_runtimeMagCalValid = false;
break;
}
}
}
// find biggest range and scale to that
if (m_runtimeMagCalValid) {
float magMaxDelta = -1;
for (int i = 0; i < 3; i++) {
if ((m_runtimeMagCalMax[i] - m_runtimeMagCalMin[i]) > magMaxDelta)
{
magMaxDelta = m_runtimeMagCalMax[i] - m_runtimeMagCalMin[i];
}
}
// adjust for + and - range
magMaxDelta /= 2.0;
for (int i = 0; i < 3; i++)
{
delta = (m_runtimeMagCalMax[i] - m_runtimeMagCalMin[i]) / 2.0;
m_compassCalScale[i] = magMaxDelta / delta;
m_compassCalOffset[i] = (m_runtimeMagCalMax[i] + m_runtimeMagCalMin[i]) / 2.0;
}
}
}
}
if (getCompassCalibrationValid() || getRuntimeCompassCalibrationValid()) {
m_imuData.compass.setX((m_imuData.compass.x() - m_compassCalOffset[0]) * m_compassCalScale[0]);
m_imuData.compass.setY((m_imuData.compass.y() - m_compassCalOffset[1]) * m_compassCalScale[1]);
m_imuData.compass.setZ((m_imuData.compass.z() - m_compassCalOffset[2]) * m_compassCalScale[2]);
if (m_settings->m_compassCalEllipsoidValid) {
RTVector3 ev = m_imuData.compass;
ev -= m_settings->m_compassCalEllipsoidOffset;
m_imuData.compass.setX(ev.x() * m_settings->m_compassCalEllipsoidCorr[0][0] +
ev.y() * m_settings->m_compassCalEllipsoidCorr[0][1] +
ev.z() * m_settings->m_compassCalEllipsoidCorr[0][2]);
m_imuData.compass.setY(ev.x() * m_settings->m_compassCalEllipsoidCorr[1][0] +
ev.y() * m_settings->m_compassCalEllipsoidCorr[1][1] +
ev.z() * m_settings->m_compassCalEllipsoidCorr[1][2]);
m_imuData.compass.setZ(ev.x() * m_settings->m_compassCalEllipsoidCorr[2][0] +
ev.y() * m_settings->m_compassCalEllipsoidCorr[2][1] +
ev.z() * m_settings->m_compassCalEllipsoidCorr[2][2]);
}
}
// update running average
m_compassAverage.setX(m_imuData.compass.x() * COMPASS_ALPHA + m_compassAverage.x() * (1.0 - COMPASS_ALPHA));
m_compassAverage.setY(m_imuData.compass.y() * COMPASS_ALPHA + m_compassAverage.y() * (1.0 - COMPASS_ALPHA));
m_compassAverage.setZ(m_imuData.compass.z() * COMPASS_ALPHA + m_compassAverage.z() * (1.0 - COMPASS_ALPHA));
m_imuData.compass = m_compassAverage;
}
void RTIMU::calibrateAverageCompass()
{
// see if need to do runtime mag calibration (i.e. no stored calibration data)
if ((!m_compassCalibrationMode && !m_settings->m_compassCalValid) || (m_compassRunTimeCalibrationEnable)) {
// try runtime calibration
bool changed = false;
// see if there is a new max or min
if (m_runtimeMagCalMax.x() < m_imuData.compass.x()) {
m_runtimeMagCalMax.setX( m_imuData.compass.x());
changed = true;
}
if (m_runtimeMagCalMax.y() < m_imuData.compass.y()) {
m_runtimeMagCalMax.setY( m_imuData.compass.y());
changed = true;
}
if (m_runtimeMagCalMax.z() < m_imuData.compass.z()) {
m_runtimeMagCalMax.setZ( m_imuData.compass.z());
changed = true;
}
if (m_runtimeMagCalMin.x() > m_imuData.compass.x()) {
m_runtimeMagCalMin.setX( m_imuData.compass.x());
changed = true;
}
if (m_runtimeMagCalMin.y() > m_imuData.compass.y()) {
m_runtimeMagCalMin.setY( m_imuData.compass.y());
changed = true;
}
if (m_runtimeMagCalMin.z() > m_imuData.compass.z()) {
m_runtimeMagCalMin.setZ( m_imuData.compass.z());
changed = true;
}
// now see if ranges are sufficient
if (changed) {
float delta;
if (!m_runtimeMagCalValid) {
m_runtimeMagCalValid = true;
for (int i = 0; i < 3; i++)
{
delta = m_runtimeMagCalMax.data(i) - m_runtimeMagCalMin.data(i);
if ((delta < RTIMU_RUNTIME_MAGCAL_RANGE) || (m_runtimeMagCalMin.data(i) > 0) || (m_runtimeMagCalMax.data(i) < 0))
{
m_runtimeMagCalValid = false;
break;
}
}
}
// find biggest range and scale to that
if (m_runtimeMagCalValid) {
float magMaxDelta = -1;
for (int i = 0; i < 3; i++) {
if ((m_runtimeMagCalMax.data(i) - m_runtimeMagCalMin.data(i)) > magMaxDelta)
{
magMaxDelta = m_runtimeMagCalMax.data(i) - m_runtimeMagCalMin.data(i);
}
}
// adjust for + and - range
magMaxDelta /= 2.0;
for (int i = 0; i < 3; i++)
{
delta = (m_runtimeMagCalMax.data(i) - m_runtimeMagCalMin.data(i)) / 2.0;
m_compassCalScale[i] = magMaxDelta / delta;
m_compassCalOffset[i] = (m_runtimeMagCalMax.data(i) + m_runtimeMagCalMin.data(i)) / 2.0;
}
m_settings->m_accelCalMax = m_runtimeMagCalMax;
m_settings->m_accelCalMin = m_runtimeMagCalMin;
}
}
}
// calibrate if required
if (getCompassCalibrationValid() || getRuntimeCompassCalibrationValid()) {
m_imuData.compass.setX((m_imuData.compass.x() - m_compassCalOffset[0]) * m_compassCalScale[0]);
m_imuData.compass.setY((m_imuData.compass.y() - m_compassCalOffset[1]) * m_compassCalScale[1]);
m_imuData.compass.setZ((m_imuData.compass.z() - m_compassCalOffset[2]) * m_compassCalScale[2]);
if (m_settings->m_compassCalEllipsoidValid) {
RTVector3 ev = m_imuData.compass;
ev -= m_settings->m_compassCalEllipsoidOffset;
m_imuData.compass.setX(ev.x() * m_settings->m_compassCalEllipsoidCorr[0][0] +
ev.y() * m_settings->m_compassCalEllipsoidCorr[0][1] +
ev.z() * m_settings->m_compassCalEllipsoidCorr[0][2]);
m_imuData.compass.setY(ev.x() * m_settings->m_compassCalEllipsoidCorr[1][0] +
ev.y() * m_settings->m_compassCalEllipsoidCorr[1][1] +
ev.z() * m_settings->m_compassCalEllipsoidCorr[1][2]);
m_imuData.compass.setZ(ev.x() * m_settings->m_compassCalEllipsoidCorr[2][0] +
ev.y() * m_settings->m_compassCalEllipsoidCorr[2][1] +
ev.z() * m_settings->m_compassCalEllipsoidCorr[2][2]);
}
}
// update running average
m_compassAverageX->addValue(m_imuData.compass.x());
m_compassAverageY->addValue(m_imuData.compass.y());
m_compassAverageZ->addValue(m_imuData.compass.z());
//Serial.println(m_compassAverageX->getAverage() - m_imuData.compass.x());
//Serial.println(m_compassAverageY->getAverage() - m_imuData.compass.y());
//Serial.println(m_compassAverageZ->getAverage() - m_imuData.compass.z());
m_imuData.compass.setX(m_compassAverageX->getAverage());
m_imuData.compass.setY(m_compassAverageY->getAverage());
m_imuData.compass.setZ(m_compassAverageZ->getAverage());
//m_compassAverage.setX(m_imuData.compass.x() * COMPASS_ALPHA + m_compassAverage.x() * (1.0 - COMPASS_ALPHA));
//m_compassAverage.setY(m_imuData.compass.y() * COMPASS_ALPHA + m_compassAverage.y() * (1.0 - COMPASS_ALPHA));
//m_compassAverage.setZ(m_imuData.compass.z() * COMPASS_ALPHA + m_compassAverage.z() * (1.0 - COMPASS_ALPHA));
//
//m_imuData.compass = m_compassAverage;
}
