C++ (Cpp) getAccelCalibrationValid Examples

Example #1

File: RTIMU.cpp Project: uutzinger/RTIMULib2-Teensy

void RTIMU::calibrateAccel()
{
    if ((getAccelCalibrationValid()==false) && (m_accelCalibrationMode==false) )
        return;
	
    // printf("%s", RTMath::displayRadians("Accel 1)", m_imuData.accel));

    if (m_imuData.accel.x() >= 0)
        m_imuData.accel.setX(m_imuData.accel.x() / m_settings->m_accelCalMax.x());
    else
        m_imuData.accel.setX(m_imuData.accel.x() / -m_settings->m_accelCalMin.x());

    if (m_imuData.accel.y() >= 0)
        m_imuData.accel.setY(m_imuData.accel.y() / m_settings->m_accelCalMax.y());
    else
        m_imuData.accel.setY(m_imuData.accel.y() / -m_settings->m_accelCalMin.y());

    if (m_imuData.accel.z() >= 0)
        m_imuData.accel.setZ(m_imuData.accel.z() / m_settings->m_accelCalMax.z());
    else
        m_imuData.accel.setZ(m_imuData.accel.z() / -m_settings->m_accelCalMin.z());

    // printf("%s", RTMath::displayRadians("Accel 2)", m_imuData.accel));

    if (m_settings->m_accelCalEllipsoidValid) {
        RTVector3 ev = m_imuData.accel;
        ev -= m_settings->m_accelCalEllipsoidOffset;

        m_imuData.accel.setX(ev.x() * m_settings->m_accelCalEllipsoidCorr[0][0] +
            ev.y() * m_settings->m_accelCalEllipsoidCorr[0][1] +
            ev.z() * m_settings->m_accelCalEllipsoidCorr[0][2]);

        m_imuData.accel.setY(ev.x() * m_settings->m_accelCalEllipsoidCorr[1][0] +
            ev.y() * m_settings->m_accelCalEllipsoidCorr[1][1] +
            ev.z() * m_settings->m_accelCalEllipsoidCorr[1][2]);

        m_imuData.accel.setZ(ev.x() * m_settings->m_accelCalEllipsoidCorr[2][0] +
            ev.y() * m_settings->m_accelCalEllipsoidCorr[2][1] +
            ev.z() * m_settings->m_accelCalEllipsoidCorr[2][2]);
    }

    // printf("%s", RTMath::displayRadians("Accel 3)", m_imuData.accel));

}

Example #2

File: RTIMU.cpp Project: carpe-noctem-cassel/cnc-msldriver

void RTIMU::calibrateAccel()
{
    if (!getAccelCalibrationValid())
        return;

    if (m_imuData.accel.x() >= 0)
        m_imuData.accel.setX(m_imuData.accel.x() / m_settings->m_accelCalMax.x());
    else
        m_imuData.accel.setX(m_imuData.accel.x() / -m_settings->m_accelCalMin.x());

    if (m_imuData.accel.y() >= 0)
        m_imuData.accel.setY(m_imuData.accel.y() / m_settings->m_accelCalMax.y());
    else
        m_imuData.accel.setY(m_imuData.accel.y() / -m_settings->m_accelCalMin.y());

    if (m_imuData.accel.z() >= 0)
        m_imuData.accel.setZ(m_imuData.accel.z() / m_settings->m_accelCalMax.z());
    else
        m_imuData.accel.setZ(m_imuData.accel.z() / -m_settings->m_accelCalMin.z());
}