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/quaternion.c | 286 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 286 insertions(+) create mode 100644 sensorlib/quaternion.c (limited to 'sensorlib/quaternion.c') diff --git a/sensorlib/quaternion.c b/sensorlib/quaternion.c new file mode 100644 index 0000000..78296aa --- /dev/null +++ b/sensorlib/quaternion.c @@ -0,0 +1,286 @@ +//***************************************************************************** +// +// quaternion.c - Functions for performing quaternion operations. +// +// 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/quaternion.h" + +//***************************************************************************** +// +//! \addtogroup quaternion_api +//! @{ +// +//***************************************************************************** + +//***************************************************************************** +// +// If M_PI has not been defined by the system headers, define it here. +// +//***************************************************************************** +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +//***************************************************************************** +// +//! Computes a quaternion from a set of eueler angles specified in degrees +//! +//! \param pfQOut is the inverted quaternion in W,X,Y,Z form +//! \param fRollDeg is roll in degrees +//! \param fPitchDeg is pitch in degrees +//! \param fYawDeg is yaw in degrees +//! +//! This function computes a quaternion from a set of euler angles specified +//! in degrees +//! +//! \return Returns a quaternion representing the provided eulers +// +//***************************************************************************** +void +QuaternionFromEuler(float pfQOut[4], float fRollDeg, float fPitchDeg, + float fYawDeg) +{ + float fRoll, fPitch, fYaw; + float fCOSY, fCOSP, fCOSR; + float fSINY, fSINP, fSINR; + + // + // Convert roll, pitch, and yaw from degrees into radians + // + fRoll = fRollDeg * M_PI / 180.0; + fPitch = fPitchDeg * M_PI / 180.0; + fYaw = fYawDeg * M_PI / 180.0; + + // + // Pre-calculate the cosine of (yaw, pitch, roll divided by 2) + // + fCOSY = cosf(fYaw / 2.0); + fCOSP = cosf(fPitch / 2.0); + fCOSR = cosf(fRoll / 2.0); + + // + // Pre-calculate the sine of (yaw, pitch, roll divided by 2) + // + fSINY = sinf(fYaw / 2.0); + fSINP = sinf(fPitch / 2.0); + fSINR = sinf(fRoll / 2.0); + + // + // The W component + // + pfQOut[Q_W] = fCOSY * fCOSP * fCOSR - fSINY * fSINP * fSINR; + + // + // The X component + // + pfQOut[Q_X] = fSINY * fSINP * fCOSR + fCOSY * fCOSP * fSINR; + + // + // The Y component + // + pfQOut[Q_Y] = fCOSY * fSINP * fCOSR - fSINY * fCOSP * fSINR; + + // + // The Z component + // + pfQOut[Q_Z] = fSINY * fCOSP * fCOSR + fCOSY * fSINP * fSINR; +} + +//***************************************************************************** +// +//! Computes the magnitude of a quaternion. +//! +//! \param pfQIn is the source quaternion in W,X,Y,Z form +//! +//! This function computes the magnitude of a quaternion by summing the square +//! of each of the quatnerion components. +//! +//! \return Returns the scalar magnitude of the quaternion +// +//***************************************************************************** +float +QuaternionMagnitude(float pfQIn[4]) +{ + float fSumSq; + + // + // Calculate the magnitude of the quaternion by finding the sum of the + // squares of each component. + // + fSumSq = ((pfQIn[Q_W] * pfQIn[Q_W]) + (pfQIn[Q_X] * pfQIn[Q_X]) + + (pfQIn[Q_Y] * pfQIn[Q_Y]) + (pfQIn[Q_Z] * pfQIn[Q_Z])); + + return(fSumSq); +} + +//***************************************************************************** +// +//! Computes the inverse of a quaternion. +//! +//! \param pfQOut is the inverted quaternion in W,X,Y,Z form +//! \param pfQIn is the source quaternion in W,X,Y,Z form +//! +//! This function computes the inverse of a quaternion. The inverse of a +//! quaternion produces a rotation opposite to the source quaternion. This +//! can be achieved by simply changing the signs of the imaginary components +//! of a quaternion when the quatnerion is a unit quaternion. +//! +//! \return Returns the inverse of a quaternion. +// +//***************************************************************************** +void +QuaternionInverse(float pfQOut[4], float pfQIn[4]) +{ + float fMag; + + // + // Find magnitude of the quaternion. This will be used to normalize the + // source quaternion if it's not already. If it is a unit quaternion then + // the magnitude should be nearly equal to 1.0 and dividing by the + // magnitude has no mathemtical effect. + // + fMag = QuaternionMagnitude(pfQIn); + + // + // Normalize the W component + // + pfQOut[Q_W] = pfQIn[Q_W] / fMag; + + // + // Invert and normalize the X component + // + pfQOut[Q_X] = -pfQIn[Q_X] / fMag; + + // + // Invert and normalize the Y component + // + pfQOut[Q_Y] = -pfQIn[Q_Y] / fMag; + + // + // Invert and normalize the Z component + // + pfQOut[Q_Z] = -pfQIn[Q_Z] / fMag; +} + +//***************************************************************************** +// +//! Computes the product of two quaternions. +//! +//! \param pfQOut is the product of In1 X In2 +//! \param pfQIn1 is the source quaternion in W,X,Y,Z form +//! \param pfQIn2 is the source quaternion in W,X,Y,Z form +//! +//! This function computes the cross product of two quaternions. +//! +//! \return Returns the cross product of the two quaternions. +// +//***************************************************************************** +void +QuaternionMult(float pfQOut[4], float pfQIn1[4], float pfQIn2[4]) +{ + // + // Let Q1 and Q2 be two quaternions with components w,x,y,z + // Let Qp be the cross product Q1 x Q2. The components of Qp can be + // calculated as follows: + // + // Qp.w = (Q1w Q2w) - (Q1x Q2x) - (Q1y Q2y) - (Q1z Q2z) + // Qp.x = (Q1w Q2x) + (Q1x Q2w) - (Q1z Q2y) + (Q1y Q2z) + // Qp.y = (Q1y Q2w) + (Q1z Q2x) + (Q1w Q2y) - (Q1x Q2z) + // Qp.z = (Q1z Q2w) - (Q1y Q2x) + (Q1x Q2y) + (Q1w Q2z) + // + + // + // Calculate the W term + // + pfQOut[Q_W] = ((pfQIn2[Q_W] * pfQIn1[Q_W]) - (pfQIn2[Q_X] * pfQIn1[Q_X]) - + (pfQIn2[Q_Y] * pfQIn1[Q_Y]) - (pfQIn2[Q_Z] * pfQIn1[Q_Z])); + + // + // Calculate the X term + // + pfQOut[Q_X]= ((pfQIn2[Q_X] * pfQIn1[Q_W]) + (pfQIn2[Q_W] * pfQIn1[Q_X]) - + (pfQIn2[Q_Y] * pfQIn1[Q_Z]) + (pfQIn2[Q_Z] * pfQIn1[Q_Y])); + + // + // Calculate the Y term + // + pfQOut[Q_Y]= ((pfQIn2[Q_W] * pfQIn1[Q_Y]) + (pfQIn2[Q_X] * pfQIn1[Q_Z]) - + (pfQIn2[Q_Y] * pfQIn1[Q_W]) - (pfQIn2[Q_Z] * pfQIn1[Q_X])); + + // + // Calculate the Z term + // + pfQOut[Q_Z] = ((pfQIn2[Q_W] * pfQIn1[Q_Z]) - (pfQIn2[Q_X] * pfQIn1[Q_Y]) - + (pfQIn2[Q_Y] * pfQIn1[Q_X]) + (pfQIn2[Q_Z] * pfQIn1[Q_W])); +} + +//***************************************************************************** +// +//! Computes the angle between two quaternions +//! +//! \param pfQIn1 is a source quaternion in W,X,Y,Z form +//! \param pfQIn2 is a source quaternion in W,X,Y,Z form +//! +//! This function computes the angle between two quaternions. +//! +//! \return Returns the angle, in radians, between the two quaternions. +// +//***************************************************************************** +float +QuaternionAngle(float pfQIn1[4], float pfQIn2[4]) +{ + float pfQInv[4]; + float pfQProd[4]; + + // + // Let Q1 and Q2 be two quaternions having components w,x,y,z. The angle + // between the orientations represented by Q1 and Q2 can be calculated + // with: + // + // angle = arccos( (Q2 * Q1').w ) * 2.0; + // + // where Q1' is the inverse of Q1 + // + + // + // Calculate the inverse of Q1 + // + QuaternionInverse(pfQInv, pfQIn1); + + // + // Find the product of Q2 x Q1` + // + QuaternionMult(pfQProd, pfQIn2, pfQInv); + + // + // calculate the arccos of the w component of the previous product. + // + return(acosf(pfQProd[Q_W]) * 2.0); +} + +//***************************************************************************** +// +// Close the Doxygen group. +//! @} +// +//***************************************************************************** -- cgit v1.3.1