//*****************************************************************************
//
// magneto.c - Functions for manipulating magnetometer readings.
//
// Copyright (c) 2013-2020 Texas Instruments Incorporated.  All rights reserved.
// Software License Agreement
// 
// Texas Instruments (TI) is supplying this software for use solely and
// exclusively on TI's microcontroller products. The software is owned by
// TI and/or its suppliers, and is protected under applicable copyright
// laws. You may not combine this software with "viral" open-source
// software in order to form a larger program.
// 
// THIS SOFTWARE IS PROVIDED "AS IS" AND WITH ALL FAULTS.
// NO WARRANTIES, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT
// NOT LIMITED TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. TI SHALL NOT, UNDER ANY
// CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR CONSEQUENTIAL
// DAMAGES, FOR ANY REASON WHATSOEVER.
// 
// This is part of revision 2.2.0.295 of the Tiva Firmware Development Package.
//
//*****************************************************************************

#include <math.h>
#include "sensorlib/magneto.h"

//*****************************************************************************
//
//! \addtogroup magneto_api
//! @{
//
//*****************************************************************************

//*****************************************************************************
//
//! Initializes the magnetometer hard- and soft-iron compensation state.
//!
//! \param psInst is a pointer to the magnetometer compensation state
//! structure.
//! \param fXOffset is the hard-iron compensation for the X axis.
//! \param fYOffset is the hard-iron compensation for the Y axis.
//! \param fZOffset is the hard-iron compensation for the Z axis.
//! \param fXYAngle is the amount to rotate around the Z axis prior to scaling
//! the Y axis reading, in radians.
//! \param fYRatio is the amount to scale the Y axis reading.
//! \param fXZAngle is the amount to rotate around the Y axis prior to scaling
//! the Z axis reading, in radians.
//! \param fZRatio is the amount to scale the Z axis reading.
//!
//! This function initializes the magnetometer compensation state structure
//! with the values that are used to perform hard- and soft-iron compensation
//! of magnetometer readings.
//!
//! \return None.
//
//*****************************************************************************
void
MagnetoCompensateInit(tMagnetoCompensation *psInst, float fXOffset,
                      float fYOffset, float fZOffset, float fXYAngle,
                      float fYRatio, float fXZAngle, float fZRatio)
{
    //
    // Save the hard- and soft-iron compensation values.
    //
    psInst->fXOffset = fXOffset;
    psInst->fYOffset = fYOffset;
    psInst->fZOffset = fZOffset;
    psInst->fXYAngle = fXYAngle;
    psInst->fYRatio = fYRatio;
    psInst->fXZAngle = fXZAngle;
    psInst->fZRatio = fZRatio;
}

//*****************************************************************************
//
//! Performs hard- and soft-iron compensation on magnetometer readings.
//!
//! \param psInst is a pointer to the magnetometer compensation state
//! structure.
//! \param pfMagnetoX is a pointer to the magnetometer X-axis reading.
//! \param pfMagnetoY is a pointer to the magnetometer Y-axis reading.
//! \param pfMagnetoZ is a pointer to the magnetometer Z-axis reading.
//!
//! This function performs hard- and soft-iron compensation on the given
//! magnetometer reading.  Hard-iron distortions cause a fixed offset in the
//! reading, regardless of orientation.  Hard-iron compensation is performed by
//! negating this fixed offset.
//!
//! Soft-iron distortion is more complicated, causing an offset that varies as
//! the sensor rotates, which results in the sensor returning an ellipse as it
//! rotates instead of a circle.  Performing soft-iron compensation requires
//! rotating the sensor reading such that the major axis of the ellipse is
//! aligned with one of the magnetometer axes, scaling one of the axes, then
//! rotating the scaled sensor reading back.  This operation is performed two
//! times; once to scale the Y axis to the same scale as the X axis, and once
//! again to scale the Z axis to the same scale as the X axis.
//!
//! Hard-iron compensation is performed prior to soft-iron compensation.
//!
//! \return None.
//
//*****************************************************************************
void
MagnetoCompensate(tMagnetoCompensation *psInst, float *pfMagnetoX,
                  float *pfMagnetoY, float *pfMagnetoZ)
{
    float fSin, fCos, fX, fY, fZ, fTemp;

    //
    // Get the magnetometer values.
    //
    fX = *pfMagnetoX;
    fY = *pfMagnetoY;
    fZ = *pfMagnetoZ;

    //
    // Perform hard-iron distortion compensation.
    //
    fX += psInst->fXOffset;
    fY += psInst->fYOffset;
    fZ += psInst->fZOffset;

    //
    // Perform soft-iron distortion compensation on the X-Y plane.  Start by
    // computing the sine and cosine of the rotation angle (which will be used
    // multiple times below).
    //
    fSin = sinf(psInst->fXYAngle);
    fCos = cosf(psInst->fXYAngle);

    //
    // Rotate the magnetometer reading around the Z axis.
    //
    fTemp = (fCos * fX) - (fSin * fY);
    fY = (fCos * fY) + (fSin * fX);
    fX = fTemp;

    //
    // Scale the Y-axis reading so that it has the same range as the X-axis
    // reading.
    //
    fY *= psInst->fYRatio;

    //
    // Rotate the magnetometer reading around the Z axis again, this time in
    // the opposite direction.
    //
    fTemp = (fCos * fX) + (fSin * fY);
    fY = (fCos * fY) - (fSin * fX);
    fX = fTemp;

    //
    // Perform soft-iron distortion compensation on the X-Z plane.  Start by
    // computing the sine and cosine of the rotation angle (which will be used
    // multiple times below).
    //
    fSin = sinf(psInst->fXZAngle);
    fCos = cosf(psInst->fXZAngle);

    //
    // Rotate the magnetometer reading around the Y axis.
    //
    fTemp = (fCos * fZ) - (fSin * fX);
    fX = (fCos * fX) + (fSin * fZ);
    fZ = fTemp;

    //
    // Scale the Z-axis reading so that it has the same range as the X-axis
    // reading.
    //
    fZ *= psInst->fZRatio;

    //
    // Rotate the magnetometer reading around the Y axis again, this time in
    // the opposite direction.
    //
    fTemp = (fCos * fZ) + (fSin * fX);
    fX = (fCos * fX) - (fSin * fZ);
    fZ = fTemp;

    //
    // Return the compensated magnetometer values.
    //
    *pfMagnetoX = fX;
    *pfMagnetoY = fY;
    *pfMagnetoZ = fZ;
}

//*****************************************************************************
//
//! Computes the compass heading from magnetometer data and roll/pitch.
//!
//! \param fMagnetoX is the X component of the magnetometer reading.
//! \param fMagnetoY is the Y component of the magnetometer reading.
//! \param fMagnetoZ is the Z component of the magnetometer reading.
//! \param fRoll is the roll angle, in radians.
//! \param fPitch is the pitch angle, in radians.
//!
//! This function computes the compass heading by performing tilt compensation
//! on the magnetometer reading.
//!
//! \return Returns the compass heading, in radians.
//
//*****************************************************************************
float
MagnetoHeadingCompute(float fMagnetoX, float fMagnetoY, float fMagnetoZ,
                      float fRoll, float fPitch)
{
    float fSinRoll, fCosRoll, fSinPitch, fCosPitch, fX, fY, fHeading;

    //
    // Compute the sine and cosine of the roll and pitch angles.
    //
    fSinRoll = sinf(fRoll);
    fCosRoll = cosf(fRoll);
    fSinPitch = sinf(fPitch);
    fCosPitch = cosf(fPitch);

    //
    // Rotate the magnetometer data such that it is level with the ground,
    // based on the provided roll and pitch.
    //
    fX = ((fMagnetoX * fCosPitch) + (fMagnetoY * fSinRoll * fSinPitch) +
          (fMagnetoZ * fCosRoll * fSinPitch));
    fY = (fMagnetoY * fCosRoll) - (fMagnetoZ * fSinRoll);

    //
    // Compute the compass heading and make it positive.
    //
    fHeading = atan2f(-fY, fX);
    if(fHeading < 0)
    {
        fHeading += 2 * 3.141592;
    }

    //
    // Return the computed compass heading.
    //
    return(fHeading);
}

//*****************************************************************************
//
// Close the Doxygen group.
//! @}
//
//*****************************************************************************