From 4085ae3ddfbbf10c8ccbd3dccd43452c40a1fe40 Mon Sep 17 00:00:00 2001 From: Yuval Adam Date: Fri, 13 Mar 2015 12:24:52 +0200 Subject: Add bootloader, nfclib and sensorlib --- sensorlib/magneto.c | 248 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 248 insertions(+) create mode 100644 sensorlib/magneto.c (limited to 'sensorlib/magneto.c') diff --git a/sensorlib/magneto.c b/sensorlib/magneto.c new file mode 100644 index 0000000..b7f7595 --- /dev/null +++ b/sensorlib/magneto.c @@ -0,0 +1,248 @@ +//***************************************************************************** +// +// magneto.c - Functions for manipulating magnetometer readings. +// +// Copyright (c) 2013-2014 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.1.0.12573 of the Tiva Firmware Development Package. +// +//***************************************************************************** + +#include +#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. +//! @} +// +//***************************************************************************** -- cgit v1.3.1