thirdparty/RTIMULib/RTIMULib/RTIMUMagCal.cpp

////////////////////////////////////////////////////////////////////////////
//
//  This file is part of RTIMULib
//
//  Copyright (c) 2014-2015, richards-tech, LLC
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy of
//  this software and associated documentation files (the "Software"), to deal in
//  the Software without restriction, including without limitation the rights to use,
//  copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
//  Software, and to permit persons to whom the Software is furnished to do so,
//  subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in all
//  copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
//  INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
//  PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
//  HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
//  OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
//  SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.


#include "RTIMUMagCal.h"

RTIMUMagCal::RTIMUMagCal(RTIMUSettings *settings)
{
    m_settings = settings;
}

RTIMUMagCal::~RTIMUMagCal()
{

}

void RTIMUMagCal::magCalInit()
{
    magCalReset();
}

void RTIMUMagCal::magCalReset()
{
    m_magMin = RTVector3(RTIMUCALDEFS_DEFAULT_MIN, RTIMUCALDEFS_DEFAULT_MIN, RTIMUCALDEFS_DEFAULT_MIN);
    m_magMax = RTVector3(RTIMUCALDEFS_DEFAULT_MAX, RTIMUCALDEFS_DEFAULT_MAX, RTIMUCALDEFS_DEFAULT_MAX);

    m_magCalCount = 0;
    for (int i = 0; i < RTIMUCALDEFS_OCTANT_COUNT; i++)
        m_octantCounts[i] = 0;
    m_magCalInIndex = m_magCalOutIndex = 0;

    //  throw away first few samples so we don't see any old calibrated samples
    m_startCount = 100;
}

void RTIMUMagCal::newMinMaxData(const RTVector3& data)
{
    if (m_startCount > 0) {
        m_startCount--;
        return;
    }

    for (int i = 0; i < 3; i++) {
        if (m_magMin.data(i) > data.data(i)) {
            m_magMin.setData(i, data.data(i));
        }

        if (m_magMax.data(i) < data.data(i)) {
            m_magMax.setData(i, data.data(i));
        }
    }
}

bool RTIMUMagCal::magCalValid()
{
    bool valid = true;

     for (int i = 0; i < 3; i++) {
        if (m_magMax.data(i) < m_magMin.data(i))
            valid = false;
    }
    return valid;

}

void RTIMUMagCal::magCalSaveMinMax()
{
    m_settings->m_compassCalValid = true;
    m_settings->m_compassCalMin = m_magMin;
    m_settings->m_compassCalMax = m_magMax;
    m_settings->m_compassCalEllipsoidValid = false;
    m_settings->saveSettings();

    //  need to invalidate ellipsoid data in order to use new min/max data

    m_magCalCount = 0;
    for (int i = 0; i < RTIMUCALDEFS_OCTANT_COUNT; i++)
        m_octantCounts[i] = 0;
    m_magCalInIndex = m_magCalOutIndex = 0;

    // and set up for min/max calibration

    setMinMaxCal();
}

void RTIMUMagCal::newEllipsoidData(const RTVector3& data)
{
    RTVector3 calData;

    //  do min/max calibration first

    for (int i = 0; i < 3; i++)
        calData.setData(i, (data.data(i) - m_minMaxOffset.data(i)) * m_minMaxScale.data(i));

    //  now see if it's already there - we want them all unique and slightly separate (using a fuzzy compare)

    for (int index = m_magCalOutIndex, i = 0; i < m_magCalCount; i++) {
        if ((abs(calData.x() - m_magCalSamples[index].x()) < RTIMUCALDEFS_ELLIPSOID_MIN_SPACING) &&
            (abs(calData.y() - m_magCalSamples[index].y()) < RTIMUCALDEFS_ELLIPSOID_MIN_SPACING) &&
            (abs(calData.z() - m_magCalSamples[index].z()) < RTIMUCALDEFS_ELLIPSOID_MIN_SPACING)) {
                return;                                         // too close to another sample
        }
        if (++index == RTIMUCALDEFS_MAX_MAG_SAMPLES)
            index = 0;
    }


    m_octantCounts[findOctant(calData)]++;

    m_magCalSamples[m_magCalInIndex++] = calData;
    if (m_magCalInIndex == RTIMUCALDEFS_MAX_MAG_SAMPLES)
        m_magCalInIndex = 0;

    if (++m_magCalCount == RTIMUCALDEFS_MAX_MAG_SAMPLES) {
        // buffer is full - pull oldest
        removeMagCalData();
    }
}

bool RTIMUMagCal::magCalEllipsoidValid()
{
    bool valid = true;

    for (int i = 0; i < RTIMUCALDEFS_OCTANT_COUNT; i++) {
        if (m_octantCounts[i] < RTIMUCALDEFS_OCTANT_MIN_SAMPLES)
            valid = false;
    }
    return valid;
}

RTVector3 RTIMUMagCal::removeMagCalData()
{
    RTVector3 ret;

    if (m_magCalCount == 0)
        return ret;

    ret = m_magCalSamples[m_magCalOutIndex++];
    if (m_magCalOutIndex == RTIMUCALDEFS_MAX_MAG_SAMPLES)
        m_magCalOutIndex = 0;
    m_magCalCount--;
    m_octantCounts[findOctant(ret)]--;
    return ret;
}

bool RTIMUMagCal::magCalSaveRaw(const char *ellipsoidFitPath)
{
    FILE *file;
    char *rawFile;

    if (ellipsoidFitPath != NULL) {
        // need to deal with ellipsoid fit processing
        rawFile = (char *)malloc(strlen(RTIMUCALDEFS_MAG_RAW_FILE) + strlen(ellipsoidFitPath) + 2);
        sprintf(rawFile, "%s/%s", ellipsoidFitPath, RTIMUCALDEFS_MAG_RAW_FILE);
        if ((file = fopen(rawFile, "w")) == NULL) {
            HAL_ERROR("Failed to open ellipsoid fit raw data file\n");
            return false;
        }
        while (m_magCalCount > 0) {
            RTVector3 sample = removeMagCalData();
            fprintf(file, "%f %f %f\n", sample.x(), sample.y(), sample.z());
        }
        fclose(file);
    }
    return true;
}

bool RTIMUMagCal::magCalSaveCorr(const char *ellipsoidFitPath)
{
    FILE *file;
    char *corrFile;
    float a[3];
    float b[9];

    if (ellipsoidFitPath != NULL) {
        corrFile = (char *)malloc(strlen(RTIMUCALDEFS_MAG_CORR_FILE) + strlen(ellipsoidFitPath) + 2);
        sprintf(corrFile, "%s/%s", ellipsoidFitPath, RTIMUCALDEFS_MAG_CORR_FILE);
        if ((file = fopen(corrFile, "r")) == NULL) {
            HAL_ERROR("Failed to open ellipsoid fit correction data file\n");
            return false;
        }
        if (fscanf(file, "%f %f %f %f %f %f %f %f %f %f %f %f",
            a + 0, a + 1, a + 2, b + 0, b + 1, b + 2, b + 3, b + 4, b + 5, b + 6, b + 7, b + 8) != 12) {
            HAL_ERROR("Ellipsoid corrcetion file didn't have 12 floats\n");
            fclose(file);
            return false;
        }
        fclose(file);
        m_settings->m_compassCalEllipsoidValid = true;
        m_settings->m_compassCalEllipsoidOffset = RTVector3(a[0], a[1], a[2]);
        memcpy(m_settings->m_compassCalEllipsoidCorr, b, 9 * sizeof(float));
        m_settings->saveSettings();
        return true;
    }
    return false;
}


void RTIMUMagCal::magCalOctantCounts(int *counts)
{
    memcpy(counts, m_octantCounts, RTIMUCALDEFS_OCTANT_COUNT * sizeof(int));
}

int RTIMUMagCal::findOctant(const RTVector3& data)
{
    int val = 0;

    if (data.x() >= 0)
        val = 1;
    if (data.y() >= 0)
        val |= 2;
    if (data.z() >= 0)
        val |= 4;

    return val;
}

void RTIMUMagCal::setMinMaxCal()
{
    float maxDelta = -1;
    float delta;

    //  find biggest range

    for (int i = 0; i < 3; i++) {
        if ((m_magMax.data(i) - m_magMin.data(i)) > maxDelta)
            maxDelta = m_magMax.data(i) - m_magMin.data(i);
    }
    if (maxDelta < 0) {
        HAL_ERROR("Error in min/max calibration data\n");
        return;
    }
    maxDelta /= 2.0f;                                       // this is the max +/- range

    for (int i = 0; i < 3; i++) {
        delta = (m_magMax.data(i) -m_magMin.data(i)) / 2.0f;
        m_minMaxScale.setData(i, maxDelta / delta);            // makes everything the same range
        m_minMaxOffset.setData(i, (m_magMax.data(i) + m_magMin.data(i)) / 2.0f);
    }
}