void RTIMU::calibrateAverageCompass() { // calibrate if required if (getCompassCalibrationValid()) { 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) { // 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; }