/* * Copyright (c) 2015, Texas Instruments Incorporated * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * * Neither the name of Texas Instruments Incorporated nor the names of * its contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ /* * DCM.c * * Created on: Jul 22, 2015 * Author: x0236197 */ #include #include #include #include #include #include #include "DCM.h" typedef struct DCM_Object { /* The state of the direction cosine matrix */ float dcm[3][3]; /* The time delta between updates to the DCM */ float deltaT; /* The scaling factor for DCM update based on the accelerometer reading */ float scaleA; /* The scaling factor for DCM update based on the gyroscope reading */ float scaleG; /* The scaling factor for DCM update based on the magnetometer reading */ float scaleM; /* The most recent accelerometer readings */ float pfAccel[3]; /* The most recent gyroscope readings */ float pfGyro[3]; /* The most recent magnetometer readings */ float pfMagneto[3]; } DCM_Object; static float dotProduct(float v1[3], float v2[3]); static void crossProduct(float vout[3], float v1[3], float v2[3]); static void scaleVector(float vout[3], float vin[3], float fScale); static void addVector(float vout[3], float v1[3], float v2[3]); static void normalize(float vin[3]); /* * ======== DCM_create ======== */ DCM_Handle DCM_create(float deltaT, float scaleA, float scaleG, float scaleM) { DCM_Handle handle = (DCM_Object *)malloc(sizeof(struct DCM_Object)); /* Initialize the DCM matrix to the identity matrix */ handle->dcm[0][0] = 1.0f; handle->dcm[0][1] = 0.0f; handle->dcm[0][2] = 0.0f; handle->dcm[1][0] = 0.0f; handle->dcm[1][1] = 1.0f; handle->dcm[1][2] = 0.0f; handle->dcm[2][0] = 0.0f; handle->dcm[2][1] = 0.0f; handle->dcm[2][2] = 1.0f; /* Save the time delta between DCM updates */ handle->deltaT = deltaT; /* Save the scaling factors that are applied to the accelerometer, gyroscope, and magnetometer readings */ handle->scaleA = scaleA; handle->scaleG = scaleG; handle->scaleM = scaleM; return handle; } /* * ======== DCM_delete ======== */ void DCM_delete(DCM_Handle handle) { free(handle); } /* * ======== DCM_updateAccelData ======== * Updates the accelerometer reading used by the complementary filter DCM * algorithm. * * handle is a pointer to the DCM state structure. * fAccelX is the accelerometer reading in the X body axis. * fAccelY is the accelerometer reading in the Y body axis. * fAccelZ is the accelerometer reading in the Z body axis. * * This function updates the accelerometer reading used by the complementary * filter DCM algorithm. The accelerometer readings provided to this function * are used by subsequent calls to DCM_start() and DCM_update() to * compute the attitude estimate. */ void DCM_updateAccelData(DCM_Handle handle, float fAccelX, float fAccelY, float fAccelZ) { /* The user should never pass in values that are not-a-number */ assert(!isnan(fAccelX)); assert(!isnan(fAccelY)); assert(!isnan(fAccelZ)); /* Save the new accelerometer reading */ handle->pfAccel[0] = fAccelX; handle->pfAccel[1] = fAccelY; handle->pfAccel[2] = fAccelZ; } /* * ======== DCM_updateGyroData ======== * Updates the gyroscope reading used by the complementary filter DCM * algorithm. * * handle is a pointer to the DCM state structure. * fGyroX is the gyroscope reading in the X body axis. * fGyroY is the gyroscope reading in the Y body axis. * fGyroZ is the gyroscope reading in the Z body axis. * * This function updates the gyroscope reading used by the complementary * filter DCM algorithm. The gyroscope readings provided to this function are * used by subsequent calls to DCM_update() to compute the attitude * estimate. */ void DCM_updateGyroData(DCM_Handle handle, float fGyroX, float fGyroY, float fGyroZ) { /* The user should never pass in values that are not-a-number */ assert(!isnan(fGyroX)); assert(!isnan(fGyroY)); assert(!isnan(fGyroZ)); /* Save the new gyroscope reading */ handle->pfGyro[0] = fGyroX; handle->pfGyro[1] = fGyroY; handle->pfGyro[2] = fGyroZ; } /* * ======== DCM_updateMagnetoData ======== * Updates the magnetometer reading used by the complementary filter DCM * algorithm. * * handle is a pointer to the DCM state structure. * fMagnetoX is the magnetometer reading in the X body axis. * fMagnetoY is the magnetometer reading in the Y body axis. * fMagnetoZ is the magnetometer reading in the Z body axis. * * This function updates the magnetometer reading used by the complementary * filter DCM algorithm. The magnetometer readings provided to this function * are used by subsequent calls to DCM_start() and DCM_update() to * compute the attitude estimate. */ void DCM_updateMagnetoData(DCM_Handle handle, float fMagnetoX, float fMagnetoY, float fMagnetoZ) { /* The user should never pass in values that are not-a-number */ assert(!isnan(fMagnetoX)); assert(!isnan(fMagnetoY)); assert(!isnan(fMagnetoZ)); /* Save the new magnetometer reading */ handle->pfMagneto[0] = fMagnetoX; handle->pfMagneto[1] = fMagnetoY; handle->pfMagneto[2] = fMagnetoZ; } /* * ======== cancelDrift ======== * Use orientation reference vectors to cancel out gyro drift with a PI * feedback controller. This method assumes that the device experiences * negligible acceleration. Adjustments can be made to allow this algorithm * to work with accelerating systems. Only called internally within the * DCM calculuation. */ static void cancelDrift(DCM_Handle handle) { /* two correction vectors fed into the controller */ float pfKError[3], pfIError[3]; /* calcluate the Z-axis correction based on accelerometer reading */ crossProduct(pfKError, handle->dcm[2], handle->pfAccel); /* calcluate the X-axis correction based on magnetometer reading */ crossProduct(pfIError, handle->dcm[0], handle->pfMagneto); } /* * ======== DCM_start ======== * Starts the complementary filter DCM attitude estimation from an initial * sensor reading. * * handle is a pointer to the DCM state structure. * * This function computes the initial complementary filter DCM attitude * estimation state based on the initial accelerometer and magnetometer * reading. While not necessary for the attitude estimation to converge, * using an initial state based on sensor readings results in quicker * convergence. */ void DCM_start(DCM_Handle handle) { /* The I, J and K basis vectors */ float pfI[3], pfJ[3], pfK[3]; /* The magnetometer reading forms the initial I vector, pointing north */ pfI[0] = handle->pfMagneto[0]; pfI[1] = handle->pfMagneto[1]; pfI[2] = handle->pfMagneto[2]; /* The accelerometer reading forms the initial K vector, pointing down */ pfK[0] = handle->pfAccel[0]; pfK[1] = handle->pfAccel[1]; pfK[2] = handle->pfAccel[2]; /* Compute the initial J vector, which is the cross product of the * K and I vectors */ crossProduct(pfJ, pfK, pfI); /* Recompute the I vector from the cross product of the J and K vectors. * This makes it fully orthogonal, which it wasn't before since magnetic * north points inside the Earth in many places. */ crossProduct(pfI, pfJ, pfK); /* Normalize the I, J, and K vectors */ normalize(pfI); normalize(pfJ); normalize(pfK); /* Initialize the DCM matrix from the I, J, and K vectors */ handle->dcm[0][0] = pfI[0]; handle->dcm[0][1] = pfI[1]; handle->dcm[0][2] = pfI[2]; handle->dcm[1][0] = pfJ[0]; handle->dcm[1][1] = pfJ[1]; handle->dcm[1][2] = pfJ[2]; handle->dcm[2][0] = pfK[0]; handle->dcm[2][1] = pfK[1]; handle->dcm[2][2] = pfK[2]; } /* * ======== DCM_update ======== * Updates the complementary filter DCM attitude estimation based on an * updated set of sensor readings. * * handle is a pointer to the DCM state structure. * * This function updates the complementary filter DCM attitude estimation * state based on the current sensor readings. This function must be called * at the rate specified in initialization, with new readings supplied at an * appropriate rate (for example, magnetometers typically sample at a much * slower rate than accelerometers and gyroscopes). * * Notes: split up into smaller operations */ void DCM_update(DCM_Handle handle) { /* The I, J and K basis vectors */ float pfI[3], pfJ[3], pfK[3]; /* The rotation from the previous state */ float pfDelta[3]; /* A temporary storage vector */ float pfTemp[3]; /* The orthogonality error */ float fError; bool bNAN = false; /* Form the I, J, K global unit vectors as measured from the body frame * of reference */ /* The magnetometer reading forms the new Im vector, pointing north */ pfI[0] = handle->pfMagneto[0]; pfI[1] = handle->pfMagneto[1]; pfI[2] = handle->pfMagneto[2]; /* The accelerometer reading forms the new Ka vector, pointing down */ pfK[0] = handle->pfAccel[0]; pfK[1] = handle->pfAccel[1]; pfK[2] = handle->pfAccel[2]; /* Compute the new J vector, which is the cross product of the Ka and Im * vectors */ crossProduct(pfJ, pfK, pfI); /* Recompute the Im vector from the cross product of the J and Ka vectors. * This makes it fully orthogonal, which it wasn't before since magnetic * north points inside the Earth in many places */ crossProduct(pfI, pfJ, pfK); /* Normalize the Im and Ka vectors */ normalize(pfI); normalize(pfK); /* Compute and scale the rotation as inferred from the accelerometer, * storing it in the rotation accumulator */ /* old K crossed with new K -> axis of K rotation */ crossProduct(pfTemp, handle->dcm[2], pfK); scaleVector(pfDelta, pfTemp, handle->scaleA); /* Compute and scale the rotation as measured by the gyroscope, adding * it to the rotation accumulator */ pfTemp[0] = -(handle->pfGyro[0]) * (handle->deltaT) * (handle->scaleG); pfTemp[1] = -(handle->pfGyro[1]) * (handle->deltaT) * (handle->scaleG); pfTemp[2] = -(handle->pfGyro[2]) * (handle->deltaT) * (handle->scaleG); addVector(pfDelta, pfDelta, pfTemp); /* Compute and scale the rotation as inferred from the magnetometer, * adding it to the rotation accumulator. * * Note: old I crossed with new I -> axis of I rotation */ crossProduct(pfTemp, handle->dcm[0], pfI); scaleVector(pfTemp, pfTemp, handle->scaleM); addVector(pfDelta, pfDelta, pfTemp); /* Rotate the I vector from the DCM matrix by the scaled rotation * rotation vector crossed with old I * old I += new I */ crossProduct(pfI, pfDelta, handle->dcm[0]); addVector(handle->dcm[0], handle->dcm[0], pfI); /* Rotate the K vector from the DCM matrix by the scaled rotation * rotation vector crossed with old K * old K += new K */ crossProduct(pfK, pfDelta, handle->dcm[2]); addVector(handle->dcm[2], handle->dcm[2], pfK); /* Compute the orthogonality error between the rotated I and K vectors and * adjust each by half the error, bringing them closer to orthogonality * cross couple to cancel out excess rotation: * I = I + err*K * K = K + err*I */ fError = dotProduct(handle->dcm[0], handle->dcm[2]) / -2.0f; scaleVector(pfI, handle->dcm[0], fError); scaleVector(pfK, handle->dcm[2], fError); addVector(handle->dcm[0], handle->dcm[0], pfK); addVector(handle->dcm[2], handle->dcm[2], pfI); /* Normalize the I and K vectors. * Use the Taylor expansion around 1 to simplify calculations */ scaleVector(handle->dcm[0], handle->dcm[0], 0.5f * (3.0f - dotProduct(handle->dcm[0], handle->dcm[0]))); scaleVector(handle->dcm[2], handle->dcm[2], 0.5f * (3.0f - dotProduct(handle->dcm[2], handle->dcm[2]))); /* Compute the rotated J vector from the cross product of the rotated, * corrected K and I vectors */ crossProduct(handle->dcm[1], handle->dcm[2], handle->dcm[0]); /* Determine if the newly updated DCM contains any invalid (i.e., * NaN) values */ bNAN = (isnan(handle->dcm[0][0]) || isnan(handle->dcm[0][1]) || isnan(handle->dcm[0][2]) || isnan(handle->dcm[1][0]) || isnan(handle->dcm[1][1]) || isnan(handle->dcm[1][2]) || isnan(handle->dcm[2][0]) || isnan(handle->dcm[2][1]) || isnan(handle->dcm[2][2])); /* As a debug measure, we check for NaN in the DCM. The user can trap * this event depending on their implementation of __error__. Should they * choose to disable interrupts and loop forever then they will have * preserved the stack and can analyze how they arrived at NaN */ assert(!bNAN); /* If any part of the matrix is not-a-number then reset the DCM back to * the identity matrix */ if (bNAN) { handle->dcm[0][0] = 1.0f; handle->dcm[0][1] = 0.0f; handle->dcm[0][2] = 0.0f; handle->dcm[1][0] = 0.0f; handle->dcm[1][1] = 1.0f; handle->dcm[1][2] = 0.0f; handle->dcm[2][0] = 0.0f; handle->dcm[2][1] = 0.0f; handle->dcm[2][2] = 1.0f; } } /* * ======== DCM_getMatrix ======== * Assigns the input matrix with the current DCM values. * * handle is a pointer to the DCM state structure. * dcm is a pointer to the array into which to store the DCM matrix * values. */ void DCM_getMatrix(DCM_Handle handle, float dcm[3][3]) { /* set the input matrix as the DCM */ dcm[0][0] = handle->dcm[0][0]; dcm[0][1] = handle->dcm[0][1]; dcm[0][2] = handle->dcm[0][2]; dcm[1][0] = handle->dcm[1][0]; dcm[1][1] = handle->dcm[1][1]; dcm[1][2] = handle->dcm[1][2]; dcm[2][0] = handle->dcm[2][0]; dcm[2][1] = handle->dcm[2][1]; dcm[2][2] = handle->dcm[2][2]; } /* * ======== DCM_computeEulers ======== * Computes the Euler angles from the DCM attitude estimation matrix. * * handle is a pointer to the DCM state structure. * pfRoll is a pointer to the value into which the roll is stored. * pfPitch is a pointer to the value into which the pitch is stored. * pfYaw is a pointer to the value into which the yaw is stored. * * This function computes the Euler angles that are represented by the DCM * attitude estimation matrix. If any of the Euler angles is not required, * the corresponding parameter can be \b NULL. */ void DCM_computeEulers(DCM_Handle handle, float *pfRoll, float *pfPitch, float *pfYaw) { /* Compute the roll, pitch, and yaw as required */ if (pfRoll) { *pfRoll = atan2f(handle->dcm[2][1], handle->dcm[2][2]); } if (pfPitch) { *pfPitch = -asinf(handle->dcm[2][0]); } if (pfYaw) { *pfYaw = atan2f(handle->dcm[1][0], handle->dcm[0][0]); } } /* * ======== DCM_computeQuaternion ======== * Computes the quaternion from the DCM attitude estimation matrix. * * handle is a pointer to the DCM state structure. * pfQuaternion is an array into which the quaternion is stored. * * This function computes the quaternion that is represented by the DCM * attitude estimation matrix. */ void DCM_computeQuaternion(DCM_Handle handle, float pfQuaternion[4]) { float fQs, fQx, fQy, fQz; /* Partially compute Qs, Qx, Qy, and Qz based on the DCM diagonals. The * square root, an expensive operation, is computed for only one of these * as determined later */ fQs = 1 + handle->dcm[0][0] + handle->dcm[1][1] + handle->dcm[2][2]; fQx = 1 + handle->dcm[0][0] - handle->dcm[1][1] - handle->dcm[2][2]; fQy = 1 - handle->dcm[0][0] + handle->dcm[1][1] - handle->dcm[2][2]; fQz = 1 - handle->dcm[0][0] - handle->dcm[1][1] + handle->dcm[2][2]; /* See if Qs is the largest of the diagonal values */ if ((fQs > fQx) && (fQs > fQy) && (fQs > fQz)) { /* Finish the computation of Qs */ fQs = sqrtf(fQs) / 2; /* Compute the values of the quaternion based on Qs */ pfQuaternion[0] = fQs; pfQuaternion[1] = ((handle->dcm[2][1] - handle->dcm[1][2]) / (4 * fQs)); pfQuaternion[2] = ((handle->dcm[0][2] - handle->dcm[2][0]) / (4 * fQs)); pfQuaternion[3] = ((handle->dcm[1][0] - handle->dcm[0][1]) / (4 * fQs)); } /* Qs is not the largest, so see if Qx is the largest remaining diagonal * value */ else if ((fQx > fQy) && (fQx > fQz)) { /* Finish the computation of Qx */ fQx = sqrtf(fQx) / 2; /* Compute the values of the quaternion based on Qx */ pfQuaternion[0] = ((handle->dcm[2][1] - handle->dcm[1][2]) / (4 * fQx)); pfQuaternion[1] = fQx; pfQuaternion[2] = ((handle->dcm[1][0] + handle->dcm[0][1]) / (4 * fQx)); pfQuaternion[3] = ((handle->dcm[0][2] + handle->dcm[2][0]) / (4 * fQx)); } /* Qs and Qx are not the largest, so see if Qy is the largest remaining * diagonal value */ else if (fQy > fQz) { /* Finish the computation of Qy */ fQy = sqrtf(fQy) / 2; /* Compute the values of the quaternion based on Qy */ pfQuaternion[0] = ((handle->dcm[0][2] - handle->dcm[2][0]) / (4 * fQy)); pfQuaternion[1] = ((handle->dcm[1][0] + handle->dcm[0][1]) / (4 * fQy)); pfQuaternion[2] = fQy; pfQuaternion[3] = ((handle->dcm[2][1] + handle->dcm[1][2]) / (4 * fQy)); } /* Qz is the largest diagonal value */ else { /* Finish the computation of Qz */ fQz = sqrtf(fQz) / 2; /* Compute the values of the quaternion based on Qz */ pfQuaternion[0] = ((handle->dcm[1][0] - handle->dcm[0][1]) / (4 * fQz)); pfQuaternion[1] = ((handle->dcm[0][2] + handle->dcm[2][0]) / (4 * fQz)); pfQuaternion[2] = ((handle->dcm[2][1] + handle->dcm[1][2]) / (4 * fQz)); pfQuaternion[3] = fQz; } } /* * ======== dotProduct ======== * Computes the dot product of two vectors. * * v1 is the first vector. * v2 is the second vector. * * This function computes the dot product of two 3-dimensional vector. * * Returns the dot product of the two vectors. */ static float dotProduct(float v1[3], float v2[3]) { return ((v1[0] * v2[0]) + (v1[1] * v2[1]) + (v1[2] * v2[2])); } /* * ======== crossProduct ======== * Computes the cross product of two vectors. * * vout is the output vector. * v1 is the first vector. * v2 is the second vector. * * This function computes the cross product of two 3-dimensional vectors. */ static void crossProduct(float vout[3], float v1[3], float v2[3]) { vout[0] = ((v1[1] * v2[2]) - (v1[2] * v2[1])); vout[1] = ((v1[2] * v2[0]) - (v1[0] * v2[2])); vout[2] = ((v1[0] * v2[1]) - (v1[1] * v2[0])); } /* * ======== scaleVector ======== * Scales a vector. * * vout is the output vector. * vin is the input vector. * fScale is the scale factor. * * This function scales a 3-dimensional vector by multiplying each of its * components by the scale factor. */ static void scaleVector(float vout[3], float vin[3], float fScale) { vout[0] = vin[0] * fScale; vout[1] = vin[1] * fScale; vout[2] = vin[2] * fScale; } /* * ======== addVector ======== * Adds two vectors. * * vout is the output vector. * v1 is the first vector. * v2 is the second vector. * * This function adds two 3-dimensional vectors. */ static void addVector(float vout[3], float v1[3], float v2[3]) { vout[0] = v1[0] + v2[0]; vout[1] = v1[1] + v2[1]; vout[2] = v1[2] + v2[2]; } /* * ======== normalize ======== * Normalizes a vector * * vin is the input vector. * * This function normalizes a 3-dimensional vectors by dividing its * components by its magnitude. */ static void normalize(float vin[3]) { float magnitude = sqrtf(dotProduct(vin, vin)); vin[0] = (vin[0] / magnitude); vin[1] = (vin[1] / magnitude); vin[2] = (vin[2] / magnitude); }