(Mostly) remove Hungarian notation

fusion.c

@@ -1,7 +1,7 @@
 // Copyright (c) 2014, Freescale Semiconductor, Inc.
 // All rights reserved.
 // vim: set ts=4:
-// 
+//
 // 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
@@ -12,7 +12,7 @@
 //     * Neither the name of Freescale Semiconductor, Inc. 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
@@ -61,10 +61,6 @@
 #define ONEOVER3840 0.0002604166667F	// 1 / 3840
 
 
-#define SENSORFS 100
-#define OVERSAMPLE_RATIO 4
-
-
 
 static void fqAeq1(Quaternion_t *pqA);
 static void feCompassNED(float fR[][3], float *pfDelta, const float fBc[], const float fGp[]);
@@ -86,68 +82,69 @@ static float fatan_15deg(float x);
 
 
 // function initializes the 9DOF Kalman filter
-void fInit_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV, int16_t iSensorFS, int16_t iOverSampleRatio)
+void fInit_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV)
 {
 	int8_t i, j;				// loop counters
 
 	// reset the flag denoting that a first 9DOF orientation lock has been achieved
-	SV->iFirstOrientationLock = 0;
+	SV->FirstOrientationLock = 0;
 
 	// compute and store useful product terms to save floating point calculations later
-	SV->fFastdeltat = 1.0F / (float) iSensorFS;
+	SV->Fastdeltat = 1.0F / (float)SENSORFS;
-	SV->fdeltat = (float) iOverSampleRatio * SV->fFastdeltat;
+	SV->deltat = (float)OVERSAMPLE_RATIO * SV->Fastdeltat;
-	SV->fdeltatsq = SV->fdeltat * SV->fdeltat;
+	SV->deltatsq = SV->deltat * SV->deltat;
-	SV->fcasq = FCA_9DOF_GBY_KALMAN * FCA_9DOF_GBY_KALMAN;
+	SV->casq = FCA_9DOF_GBY_KALMAN * FCA_9DOF_GBY_KALMAN;
-	SV->fcdsq = FCD_9DOF_GBY_KALMAN * FCD_9DOF_GBY_KALMAN;
+	SV->cdsq = FCD_9DOF_GBY_KALMAN * FCD_9DOF_GBY_KALMAN;
-	SV->fQwbplusQvG = FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN;
+	SV->QwbplusQvG = FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN;
 
 	// initialize the fixed entries in the measurement matrix C
 	for (i = 0; i < 6; i++) {
 		for (j = 0; j < 12; j++) {
-			SV->fC6x12[i][j]= 0.0F;
+			SV->C6x12[i][j]= 0.0F;
 		}
 	}
-	SV->fC6x12[0][6] = SV->fC6x12[1][7] = SV->fC6x12[2][8] = 1.0F; 
+	SV->C6x12[0][6] = SV->C6x12[1][7] = SV->C6x12[2][8] = 1.0F;
-	SV->fC6x12[3][9] = SV->fC6x12[4][10] = SV->fC6x12[5][11] = -1.0F; 
+	SV->C6x12[3][9] = SV->C6x12[4][10] = SV->C6x12[5][11] = -1.0F;
 
 	// zero a posteriori orientation, error vector xe+ (thetae+, be+, de+, ae+) and b+ and inertial
-	f3x3matrixAeqI(SV->fRPl);
+	f3x3matrixAeqI(SV->RPl);
-	fqAeq1(&(SV->fqPl));
+	fqAeq1(&(SV->qPl));
 	for (i = X; i <= Z; i++) {
-		SV->fThErrPl[i] = SV->fbErrPl[i] = SV->faErrSePl[i] = SV->fdErrSePl[i] = SV->fbPl[i] = 0.0F;
+		SV->ThErrPl[i] = SV->bErrPl[i] = SV->aErrSePl[i] = SV->dErrSePl[i] = SV->bPl[i] = 0.0F;
 	}
 
 	// initialize the reference geomagnetic vector (uT, global frame)
-	SV->fDeltaPl = 0.0F;
+	SV->DeltaPl = 0.0F;
 	// initialize NED geomagnetic vector to zero degrees inclination
-	SV->fmGl[X] = DEFAULTB;
+	SV->mGl[X] = DEFAULTB;
-	SV->fmGl[Y] = 0.0F;
+	SV->mGl[Y] = 0.0F;
-	SV->fmGl[Z] = 0.0F;
+	SV->mGl[Z] = 0.0F;
 
 	// initialize noise variances for Qv and Qw matrix updates
-	SV->fQvAA = FQVA_9DOF_GBY_KALMAN + FQWA_9DOF_GBY_KALMAN + FDEGTORAD * FDEGTORAD * SV->fdeltatsq * (FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN);
+	SV->QvAA = FQVA_9DOF_GBY_KALMAN + FQWA_9DOF_GBY_KALMAN + FDEGTORAD * FDEGTORAD * SV->deltatsq * (FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN);
-	SV->fQvMM = FQVM_9DOF_GBY_KALMAN + FQWD_9DOF_GBY_KALMAN + FDEGTORAD * FDEGTORAD * SV->fdeltatsq * DEFAULTB * DEFAULTB * (FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN);
+	SV->QvMM = FQVM_9DOF_GBY_KALMAN + FQWD_9DOF_GBY_KALMAN + FDEGTORAD * FDEGTORAD * SV->deltatsq * DEFAULTB * DEFAULTB * (FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN);
 
 	// initialize the 12x12 noise covariance matrix Qw of the a priori error vector xe-
-	// Qw is then recursively updated as P+ = (1 - K * C) * P- = (1 - K * C) * Qw  and Qw updated from P+
+	// Qw is then recursively updated as P+ = (1 - K * C) * P- = (1 - K * C) * Qw  and Qw
+	// updated from P+
 	// zero the matrix Qw
 	for (i = 0; i < 12; i++) {
 		for (j = 0; j < 12; j++) {
-			SV->fQw12x12[i][j] = 0.0F;
+			SV->Qw12x12[i][j] = 0.0F;
 		}
 	}
 	// loop over non-zero values
 	for (i = 0; i < 3; i++) {
 		// theta_e * theta_e terms
-		SV->fQw12x12[i][i] = FQWINITTHTH_9DOF_GBY_KALMAN;
+		SV->Qw12x12[i][i] = FQWINITTHTH_9DOF_GBY_KALMAN;
 		// b_e * b_e terms
-		SV->fQw12x12[i + 3][i + 3] = FQWINITBB_9DOF_GBY_KALMAN;
+		SV->Qw12x12[i + 3][i + 3] = FQWINITBB_9DOF_GBY_KALMAN;
 		// th_e * b_e terms
-		SV->fQw12x12[i][i + 3] = SV->fQw12x12[i + 3][i] = FQWINITTHB_9DOF_GBY_KALMAN;
+		SV->Qw12x12[i][i + 3] = SV->Qw12x12[i + 3][i] = FQWINITTHB_9DOF_GBY_KALMAN;
 		// a_e * a_e terms
-		SV->fQw12x12[i + 6][i + 6] = FQWINITAA_9DOF_GBY_KALMAN;
+		SV->Qw12x12[i + 6][i + 6] = FQWINITAA_9DOF_GBY_KALMAN;
 		// d_e * d_e terms
-		SV->fQw12x12[i + 9][i + 9] = FQWINITDD_9DOF_GBY_KALMAN;
+		SV->Qw12x12[i + 9][i + 9] = FQWINITDD_9DOF_GBY_KALMAN;
 	}
 
 	// clear the reset flag
@@ -160,8 +157,8 @@ void fInit_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV, int16_t iSensorFS, int16_t
 // 9DOF orientation function implemented using a 12 element Kalman filter
 void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 		const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro,
-		const MagCalibration_t *MagCal, int16_t iOverSampleRatio)
+		const MagCalibration_t *MagCal)
-{	
+{
 	// local scalars and arrays
 	float fopp, fadj, fhyp;						// opposite, adjacent and hypoteneuse
 	float fsindelta, fcosdelta;					// sin and cos of inclination angle delta
@@ -193,7 +190,7 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 
 	// do a reset and return if requested
 	if (SV->resetflag) {
-		fInit_9DOF_GBY_KALMAN(SV, SENSORFS, OVERSAMPLE_RATIO);
+		fInit_9DOF_GBY_KALMAN(SV);
 		return;
 	}
 
@@ -201,16 +198,16 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 	// initial orientation lock to accelerometer and magnetometer eCompass orientation
 	// *********************************************************************************
 
-	// do a once-only orientation lock after the first valid magnetic calibration 
+	// do a once-only orientation lock after the first valid magnetic calibration
-	if (MagCal->iValidMagCal && !SV->iFirstOrientationLock) {
+	if (MagCal->ValidMagCal && !SV->FirstOrientationLock) {
 		// get the 6DOF orientation matrix and initial inclination angle
-		feCompassNED(SV->fRPl, &(SV->fDeltaPl), Mag->fBcFast, Accel->fGpFast);
+		feCompassNED(SV->RPl, &(SV->DeltaPl), Mag->BcFast, Accel->GpFast);
 
 		// get the orientation quaternion from the orientation matrix
-		fQuaternionFromRotationMatrix(SV->fRPl, &(SV->fqPl));
+		fQuaternionFromRotationMatrix(SV->RPl, &(SV->qPl));
 
 		// set the orientation lock flag so this initial alignment is only performed once
-		SV->iFirstOrientationLock = 1;
+		SV->FirstOrientationLock = 1;
 	}
 
 	// *********************************************************************************
@@ -219,31 +216,31 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 
 	// compute the angular velocity from the averaged high frequency gyro reading.
 	// omega[k] = yG[k] - b-[k] = yG[k] - b+[k-1] (deg/s)
-	SV->fOmega[X] = Gyro->fYp[X] - SV->fbPl[X];
+	SV->Omega[X] = Gyro->Yp[X] - SV->bPl[X];
-	SV->fOmega[Y] = Gyro->fYp[Y] - SV->fbPl[Y];
+	SV->Omega[Y] = Gyro->Yp[Y] - SV->bPl[Y];
-	SV->fOmega[Z] = Gyro->fYp[Z] - SV->fbPl[Z];
+	SV->Omega[Z] = Gyro->Yp[Z] - SV->bPl[Z];
 
 	// initialize the a priori orientation quaternion to the previous a posteriori estimate
-	SV->fqMi = SV->fqPl;
+	SV->qMi = SV->qPl;
 
 	// integrate the buffered high frequency (typically 200Hz) gyro readings
-	for (j = 0; j < iOverSampleRatio; j++) {
+	for (j = 0; j < OVERSAMPLE_RATIO; j++) {
 		// compute the incremental fast (typically 200Hz) rotation vector rvec (deg)
 		for (i = X; i <= Z; i++) {
-			rvec[i] = (((float)Gyro->iYpFast[j][i] * DEG_PER_SEC_PER_COUNT) - SV->fbPl[i])
+			rvec[i] = (((float)Gyro->YpFast[j][i] * DEG_PER_SEC_PER_COUNT) - SV->bPl[i])
-				* SV->fFastdeltat;
+				* SV->Fastdeltat;
 		}
 
 		// compute the incremental quaternion fDeltaq from the rotation vector
-		fQuaternionFromRotationVectorDeg(&(SV->fDeltaq), rvec, 1.0F);
+		fQuaternionFromRotationVectorDeg(&(SV->Deltaq), rvec, 1.0F);
 
 		// incrementally rotate the a priori orientation quaternion fqMi
 		// the a posteriori quaternion fqPl is re-normalized later so this update is stable
-		qAeqAxB(&(SV->fqMi), &(SV->fDeltaq));
+		qAeqAxB(&(SV->qMi), &(SV->Deltaq));
 	}
 
 	// get the a priori rotation matrix from the a priori quaternion
-	fRotationMatrixFromQuaternion(SV->fRMi, &(SV->fqMi));
+	fRotationMatrixFromQuaternion(SV->RMi, &(SV->qMi));
 
 	// *********************************************************************************
 	// calculate a priori gyro, accelerometer and magnetometer estimates
@@ -255,22 +252,25 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 		// compute the a priori gyro estimate of the gravitational vector (g, sensor frame)
 		// using an absolute rotation of the global frame gravity vector (with magnitude 1g)
 		// NED gravity is along positive z axis
-		SV->fgSeGyMi[i] = SV->fRMi[i][Z];
+		SV->gSeGyMi[i] = SV->RMi[i][Z];
 
-		// compute a priori acceleration (a-) (g, sensor frame) from decayed a posteriori estimate (g, sensor frame)
+		// compute a priori acceleration (a-) (g, sensor frame) from decayed a
-		SV->faSeMi[i] = FCA_9DOF_GBY_KALMAN * SV->faSePl[i];
+		// posteriori estimate (g, sensor frame)
+		SV->aSeMi[i] = FCA_9DOF_GBY_KALMAN * SV->aSePl[i];
 
-		// compute the a priori gravity error vector (accelerometer minus gyro estimates) (g, sensor frame)
+		// compute the a priori gravity error vector (accelerometer minus gyro estimates)
+		// (g, sensor frame)
 		// NED and Windows 8 have positive sign for gravity: y = g - a and g = y + a
-		SV->fgErrSeMi[i] = Accel->fGpFast[i] + SV->faSeMi[i] - SV->fgSeGyMi[i];
+		SV->gErrSeMi[i] = Accel->GpFast[i] + SV->aSeMi[i] - SV->gSeGyMi[i];
 
 		// compute the a priori gyro estimate of the geomagnetic vector (uT, sensor frame)
 		// using an absolute rotation of the global frame geomagnetic vector (with magnitude B uT)
 		// NED y component of geomagnetic vector in global frame is zero
-		SV->fmSeGyMi[i] = SV->fRMi[i][X] * SV->fmGl[X] + SV->fRMi[i][Z] * SV->fmGl[Z];
+		SV->mSeGyMi[i] = SV->RMi[i][X] * SV->mGl[X] + SV->RMi[i][Z] * SV->mGl[Z];
 
-		// compute the a priori geomagnetic error vector (magnetometer minus gyro estimates) (g, sensor frame)
+		// compute the a priori geomagnetic error vector (magnetometer minus gyro estimates)
-		SV->fmErrSeMi[i] = Mag->fBcFast[i] - SV->fmSeGyMi[i];
+		// (g, sensor frame)
+		SV->mErrSeMi[i] = Mag->BcFast[i] - SV->mSeGyMi[i];
 	}
 
 	// *********************************************************************************
@@ -278,30 +278,30 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 	// *********************************************************************************
 
 	// update measurement matrix C with -alpha(g-)x and -alpha(m-)x from gyro (g, uT, sensor frame)
-	SV->fC6x12[0][1] = FDEGTORAD * SV->fgSeGyMi[Z];
+	SV->C6x12[0][1] = FDEGTORAD * SV->gSeGyMi[Z];
-	SV->fC6x12[0][2] = -FDEGTORAD * SV->fgSeGyMi[Y];
+	SV->C6x12[0][2] = -FDEGTORAD * SV->gSeGyMi[Y];
-	SV->fC6x12[1][2] = FDEGTORAD * SV->fgSeGyMi[X];
+	SV->C6x12[1][2] = FDEGTORAD * SV->gSeGyMi[X];
-	SV->fC6x12[1][0] = -SV->fC6x12[0][1];
+	SV->C6x12[1][0] = -SV->C6x12[0][1];
-	SV->fC6x12[2][0] = -SV->fC6x12[0][2];
+	SV->C6x12[2][0] = -SV->C6x12[0][2];
-	SV->fC6x12[2][1] = -SV->fC6x12[1][2];
+	SV->C6x12[2][1] = -SV->C6x12[1][2];
-	SV->fC6x12[3][1] = FDEGTORAD * SV->fmSeGyMi[Z];
+	SV->C6x12[3][1] = FDEGTORAD * SV->mSeGyMi[Z];
-	SV->fC6x12[3][2] = -FDEGTORAD * SV->fmSeGyMi[Y];
+	SV->C6x12[3][2] = -FDEGTORAD * SV->mSeGyMi[Y];
-	SV->fC6x12[4][2] = FDEGTORAD * SV->fmSeGyMi[X];
+	SV->C6x12[4][2] = FDEGTORAD * SV->mSeGyMi[X];
-	SV->fC6x12[4][0] = -SV->fC6x12[3][1];
+	SV->C6x12[4][0] = -SV->C6x12[3][1];
-	SV->fC6x12[5][0] = -SV->fC6x12[3][2];
+	SV->C6x12[5][0] = -SV->C6x12[3][2];
-	SV->fC6x12[5][1] = -SV->fC6x12[4][2];
+	SV->C6x12[5][1] = -SV->C6x12[4][2];
-	SV->fC6x12[0][4] = -SV->fdeltat * SV->fC6x12[0][1];
+	SV->C6x12[0][4] = -SV->deltat * SV->C6x12[0][1];
-	SV->fC6x12[0][5] = -SV->fdeltat * SV->fC6x12[0][2];
+	SV->C6x12[0][5] = -SV->deltat * SV->C6x12[0][2];
-	SV->fC6x12[1][5] = -SV->fdeltat * SV->fC6x12[1][2];
+	SV->C6x12[1][5] = -SV->deltat * SV->C6x12[1][2];
-	SV->fC6x12[1][3]= -SV->fC6x12[0][4];
+	SV->C6x12[1][3]= -SV->C6x12[0][4];
-	SV->fC6x12[2][3]= -SV->fC6x12[0][5];
+	SV->C6x12[2][3]= -SV->C6x12[0][5];
-	SV->fC6x12[2][4]= -SV->fC6x12[1][5];
+	SV->C6x12[2][4]= -SV->C6x12[1][5];
-	SV->fC6x12[3][4] = -SV->fdeltat * SV->fC6x12[3][1];
+	SV->C6x12[3][4] = -SV->deltat * SV->C6x12[3][1];
-	SV->fC6x12[3][5] = -SV->fdeltat * SV->fC6x12[3][2];
+	SV->C6x12[3][5] = -SV->deltat * SV->C6x12[3][2];
-	SV->fC6x12[4][5] = -SV->fdeltat * SV->fC6x12[4][2];
+	SV->C6x12[4][5] = -SV->deltat * SV->C6x12[4][2];
-	SV->fC6x12[4][3] = -SV->fC6x12[3][4];
+	SV->C6x12[4][3] = -SV->C6x12[3][4];
-	SV->fC6x12[5][3] = -SV->fC6x12[3][5];
+	SV->C6x12[5][3] = -SV->C6x12[3][5];
-	SV->fC6x12[5][4] = -SV->fC6x12[4][5];
+	SV->C6x12[5][4] = -SV->C6x12[4][5];
 
 	// *********************************************************************************
 	// calculate the Kalman gain matrix K
@@ -322,10 +322,10 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 			*pftmpA12x6ij = 0.0F;
 
 			// initialize pfC6x12jk for current j, k=0
-			pfC6x12jk = SV->fC6x12[j];
+			pfC6x12jk = SV->C6x12[j];
 
 			// initialize pfQw12x12ik for current i, k=0
-			pfQw12x12ik = SV->fQw12x12[i];
+			pfQw12x12ik = SV->Qw12x12[i];
 
 			// sum matrix products over inner loop over k
 			for (k = 0; k < 12; k++) {
@@ -342,27 +342,27 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 				pfC6x12jk++;
 				pfQw12x12ik++;
 
-			} // end of loop over k
+			}
 
 			// increment pftmpA12x6ij for next iteration of j
 			pftmpA12x6ij++;
 
-		} // end of loop over j
+		}
-	} // end of loop over i
+	}
 
 	// set symmetric P+ (6x6 scratch sub-matrix) to C * P- * C^T + Qv
 	// = C * (Qw * C^T) + Qv = C * ftmpA12x6 + Qv
 	// both C and ftmpA12x6 are sparse but not symmetric
 	for (i = 0; i < 6; i++) { // loop over rows of P+
 		// initialize pfPPlus12x12ij for current i, j=i
-		pfPPlus12x12ij = SV->fPPlus12x12[i] + i;
+		pfPPlus12x12ij = SV->PPlus12x12[i] + i;
 
 		for (j = i; j < 6; j++) { // loop over above diagonal columns of P+
-			// zero P+[i][j] 
+			// zero P+[i][j]
 			*pfPPlus12x12ij = 0.0F;
 
 			// initialize pfC6x12ik for current i, k=0
-			pfC6x12ik = SV->fC6x12[i];
+			pfC6x12ik = SV->C6x12[i];
 
 			// initialize pftmpA12x6kj for current j, k=0
 			pftmpA12x6kj = *ftmpA12x6 + j;
@@ -379,33 +379,33 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 				}
 
 				// update pfC6x12ik and pftmpA12x6kj for next iteration of k
-				pfC6x12ik++;						
+				pfC6x12ik++;
 				pftmpA12x6kj += 6;
 
-			} // end of loop over k
+			}
 
 			// increment pfPPlus12x12ij for next iteration of j
 			pfPPlus12x12ij++;
 
-		} // end of loop over j
+		}
-	} // end of loop over i
+	}
 
 	// add in noise covariance terms to the diagonal
-	SV->fPPlus12x12[0][0] +=  SV->fQvAA;
+	SV->PPlus12x12[0][0] +=  SV->QvAA;
-	SV->fPPlus12x12[1][1] +=  SV->fQvAA;
+	SV->PPlus12x12[1][1] +=  SV->QvAA;
-	SV->fPPlus12x12[2][2] +=  SV->fQvAA;
+	SV->PPlus12x12[2][2] +=  SV->QvAA;
-	SV->fPPlus12x12[3][3] +=  SV->fQvMM;
+	SV->PPlus12x12[3][3] +=  SV->QvMM;
-	SV->fPPlus12x12[4][4] +=  SV->fQvMM;
+	SV->PPlus12x12[4][4] +=  SV->QvMM;
-	SV->fPPlus12x12[5][5] +=  SV->fQvMM;
+	SV->PPlus12x12[5][5] +=  SV->QvMM;
 
 	// copy above diagonal elements of P+ (6x6 scratch sub-matrix) to below diagonal
-	for (i = 1; i < 6; i++) 	
+	for (i = 1; i < 6; i++)
-		for (j = 0; j < i; j++) 
+		for (j = 0; j < i; j++)
-			SV->fPPlus12x12[i][j] = SV->fPPlus12x12[j][i];
+			SV->PPlus12x12[i][j] = SV->PPlus12x12[j][i];
 
 	// calculate inverse of P+ (6x6 scratch sub-matrix) = inv(C * P- * C^T + Qv) = inv(C * Qw * C^T + Qv)
 	for (i = 0; i < 6; i++) {
-		pfRows[i] = SV->fPPlus12x12[i];
+		pfRows[i] = SV->PPlus12x12[i];
 	}
 	fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 3);
 
@@ -415,7 +415,7 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 	// K is not symmetric because C is not symmetric
 	for (i = 0; i < 12; i++) { // loop over rows of K12x6
 		// initialize pfK12x6ij for current i, j=0
-		pfK12x6ij = SV->fK12x6[i];
+		pfK12x6ij = SV->K12x6[i];
 
 		for (j = 0; j < 6; j++) { // loop over columns of K12x6
 			// zero the matrix element fK12x6[i][j]
@@ -425,7 +425,7 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 			pftmpA12x6ik = ftmpA12x6[i];
 
 			// initialize pfPPlus12x12kj for current j, k=0
-			pfPPlus12x12kj = *SV->fPPlus12x12 + j;
+			pfPPlus12x12kj = *SV->PPlus12x12 + j;
 
 			// sum matrix products over inner loop over k
 			for (k = 0; k < 6; k++) {
@@ -437,13 +437,13 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 				pftmpA12x6ik++;
 				pfPPlus12x12kj += 12;
 
-			} // end of loop over k
+			}
 
 			// increment pfK12x6ij for the next iteration of j
 			pfK12x6ij++;
 
-		} // end of loop over j
+		}
-	} // end of loop over i
+	}
 
 	// *********************************************************************************
 	// calculate a posteriori error estimate: xe+ = K * ze-
@@ -452,40 +452,40 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 	// first calculate all four error vector components using accelerometer error component only
 	// for fThErrPl, fbErrPl, faErrSePl but also magnetometer for fdErrSePl
 	for (i = X; i <= Z; i++) {
-		SV->fThErrPl[i] = SV->fK12x6[i][0] * SV->fgErrSeMi[X] +
+		SV->ThErrPl[i] = SV->K12x6[i][0] * SV->gErrSeMi[X] +
-				SV->fK12x6[i][1] * SV->fgErrSeMi[Y] +
+				SV->K12x6[i][1] * SV->gErrSeMi[Y] +
-				SV->fK12x6[i][2] * SV->fgErrSeMi[Z];
+				SV->K12x6[i][2] * SV->gErrSeMi[Z];
-		SV->fbErrPl[i] = SV->fK12x6[i + 3][0] * SV->fgErrSeMi[X] + 
+		SV->bErrPl[i] = SV->K12x6[i + 3][0] * SV->gErrSeMi[X] +
-				SV->fK12x6[i + 3][1] * SV->fgErrSeMi[Y] + 
+				SV->K12x6[i + 3][1] * SV->gErrSeMi[Y] +
-				SV->fK12x6[i + 3][2] * SV->fgErrSeMi[Z];
+				SV->K12x6[i + 3][2] * SV->gErrSeMi[Z];
-		SV->faErrSePl[i] = SV->fK12x6[i + 6][0] * SV->fgErrSeMi[X] +
+		SV->aErrSePl[i] = SV->K12x6[i + 6][0] * SV->gErrSeMi[X] +
-				SV->fK12x6[i + 6][1] * SV->fgErrSeMi[Y] +
+				SV->K12x6[i + 6][1] * SV->gErrSeMi[Y] +
-				SV->fK12x6[i + 6][2] * SV->fgErrSeMi[Z];
+				SV->K12x6[i + 6][2] * SV->gErrSeMi[Z];
-		SV->fdErrSePl[i] = SV->fK12x6[i + 9][0] * SV->fgErrSeMi[X] +
+		SV->dErrSePl[i] = SV->K12x6[i + 9][0] * SV->gErrSeMi[X] +
-				SV->fK12x6[i + 9][1] * SV->fgErrSeMi[Y] +
+				SV->K12x6[i + 9][1] * SV->gErrSeMi[Y] +
-				SV->fK12x6[i + 9][2] * SV->fgErrSeMi[Z] +
+				SV->K12x6[i + 9][2] * SV->gErrSeMi[Z] +
-				SV->fK12x6[i + 9][3] * SV->fmErrSeMi[X] +
+				SV->K12x6[i + 9][3] * SV->mErrSeMi[X] +
-				SV->fK12x6[i + 9][4] * SV->fmErrSeMi[Y] +
+				SV->K12x6[i + 9][4] * SV->mErrSeMi[Y] +
-				SV->fK12x6[i + 9][5] * SV->fmErrSeMi[Z];
+				SV->K12x6[i + 9][5] * SV->mErrSeMi[Z];
 	}
 
 	// set the magnetic jamming flag if there is a significant magnetic error power after calibration
-	ftmp = SV->fdErrSePl[X] * SV->fdErrSePl[X] + SV->fdErrSePl[Y] * SV->fdErrSePl[Y] +
+	ftmp = SV->dErrSePl[X] * SV->dErrSePl[X] + SV->dErrSePl[Y] * SV->dErrSePl[Y] +
-			SV->fdErrSePl[Z] * SV->fdErrSePl[Z];
+			SV->dErrSePl[Z] * SV->dErrSePl[Z];
-	iMagJamming = (MagCal->iValidMagCal) && (ftmp > MagCal->fFourBsq);
+	iMagJamming = (MagCal->ValidMagCal) && (ftmp > MagCal->FourBsq);
 
 	// add the remaining magnetic error terms if there is calibration and no magnetic jamming
-	if (MagCal->iValidMagCal && !iMagJamming) {
+	if (MagCal->ValidMagCal && !iMagJamming) {
 		for (i = X; i <= Z; i++) {
-			SV->fThErrPl[i] += SV->fK12x6[i][3] * SV->fmErrSeMi[X] +
+			SV->ThErrPl[i] += SV->K12x6[i][3] * SV->mErrSeMi[X] +
-					SV->fK12x6[i][4] * SV->fmErrSeMi[Y] +
+					SV->K12x6[i][4] * SV->mErrSeMi[Y] +
-					SV->fK12x6[i][5] * SV->fmErrSeMi[Z];
+					SV->K12x6[i][5] * SV->mErrSeMi[Z];
-			SV->fbErrPl[i] += SV->fK12x6[i + 3][3] * SV->fmErrSeMi[X] +
+			SV->bErrPl[i] += SV->K12x6[i + 3][3] * SV->mErrSeMi[X] +
-					SV->fK12x6[i + 3][4] * SV->fmErrSeMi[Y] +
+					SV->K12x6[i + 3][4] * SV->mErrSeMi[Y] +
-					SV->fK12x6[i + 3][5] * SV->fmErrSeMi[Z]; 
+					SV->K12x6[i + 3][5] * SV->mErrSeMi[Z];
-			SV->faErrSePl[i] += SV->fK12x6[i + 6][3] * SV->fmErrSeMi[X] +
+			SV->aErrSePl[i] += SV->K12x6[i + 6][3] * SV->mErrSeMi[X] +
-					SV->fK12x6[i + 6][4] * SV->fmErrSeMi[Y] +
+					SV->K12x6[i + 6][4] * SV->mErrSeMi[Y] +
-					SV->fK12x6[i + 6][5] * SV->fmErrSeMi[Z]; 
+					SV->K12x6[i + 6][5] * SV->mErrSeMi[Z];
 		}
 	}
 
@@ -494,61 +494,61 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 	// *********************************************************************************
 
 	// get the a posteriori delta quaternion
-	fQuaternionFromRotationVectorDeg(&(SV->fDeltaq), SV->fThErrPl, -1.0F);
+	fQuaternionFromRotationVectorDeg(&(SV->Deltaq), SV->ThErrPl, -1.0F);
 
 	// compute the a posteriori orientation quaternion fqPl = fqMi * Deltaq(-thetae+)
 	// the resulting quaternion may have negative scalar component q0
-	qAeqBxC(&(SV->fqPl), &(SV->fqMi), &(SV->fDeltaq));
+	qAeqBxC(&(SV->qPl), &(SV->qMi), &(SV->Deltaq));
 
-	// normalize the a posteriori orientation quaternion to stop error propagation 
+	// normalize the a posteriori orientation quaternion to stop error propagation
 	// the renormalization function ensures that the scalar component q0 is non-negative
-	fqAeqNormqA(&(SV->fqPl));
+	fqAeqNormqA(&(SV->qPl));
 
 	// compute the a posteriori rotation matrix from the a posteriori quaternion
-	fRotationMatrixFromQuaternion(SV->fRPl, &(SV->fqPl));
+	fRotationMatrixFromQuaternion(SV->RPl, &(SV->qPl));
 
 	// compute the rotation vector from the a posteriori quaternion
-	fRotationVectorDegFromQuaternion(&(SV->fqPl), SV->fRVecPl);
+	fRotationVectorDegFromQuaternion(&(SV->qPl), SV->RVecPl);
 
 	// update the a posteriori gyro offset vector b+ and
 	// assign the entire linear acceleration error vector to update the linear acceleration
 	for (i = X; i <= Z; i++) {
 		// b+[k] = b-[k] - be+[k] = b+[k] - be+[k] (deg/s)
-		SV->fbPl[i] -= SV->fbErrPl[i];
+		SV->bPl[i] -= SV->bErrPl[i];
 		// a+ = a- - ae+ (g, sensor frame)
-		SV->faSePl[i] = SV->faSeMi[i] - SV->faErrSePl[i];
+		SV->aSePl[i] = SV->aSeMi[i] - SV->aErrSePl[i];
 	}
 
 	// compute the linear acceleration in the global frame from the accelerometer measurement (sensor frame).
-	// de-rotate the accelerometer measurement from the sensor to global frame using the inverse (transpose) 
+	// de-rotate the accelerometer measurement from the sensor to global frame using the inverse (transpose)
 	// of the a posteriori rotation matrix
-	SV->faGlPl[X] = SV->fRPl[X][X] * Accel->fGpFast[X] + SV->fRPl[Y][X] * Accel->fGpFast[Y] +
+	SV->aGlPl[X] = SV->RPl[X][X] * Accel->GpFast[X] + SV->RPl[Y][X] * Accel->GpFast[Y] +
-			SV->fRPl[Z][X] * Accel->fGpFast[Z];
+			SV->RPl[Z][X] * Accel->GpFast[Z];
-	SV->faGlPl[Y] = SV->fRPl[X][Y] * Accel->fGpFast[X] + SV->fRPl[Y][Y] * Accel->fGpFast[Y] +
+	SV->aGlPl[Y] = SV->RPl[X][Y] * Accel->GpFast[X] + SV->RPl[Y][Y] * Accel->GpFast[Y] +
-			SV->fRPl[Z][Y] * Accel->fGpFast[Z];
+			SV->RPl[Z][Y] * Accel->GpFast[Z];
-	SV->faGlPl[Z] = SV->fRPl[X][Z] * Accel->fGpFast[X] + SV->fRPl[Y][Z] * Accel->fGpFast[Y] +
+	SV->aGlPl[Z] = SV->RPl[X][Z] * Accel->GpFast[X] + SV->RPl[Y][Z] * Accel->GpFast[Y] +
-			SV->fRPl[Z][Z] * Accel->fGpFast[Z];
+			SV->RPl[Z][Z] * Accel->GpFast[Z];
-	// remove gravity and correct the sign if the coordinate system is gravity positive / acceleration negative 
+	// remove gravity and correct the sign if the coordinate system is gravity positive / acceleration negative
 	// gravity positive NED
-	SV->faGlPl[X] = -SV->faGlPl[X];
+	SV->aGlPl[X] = -SV->aGlPl[X];
-	SV->faGlPl[Y] = -SV->faGlPl[Y];
+	SV->aGlPl[Y] = -SV->aGlPl[Y];
-	SV->faGlPl[Z] = -(SV->faGlPl[Z] - 1.0F);
+	SV->aGlPl[Z] = -(SV->aGlPl[Z] - 1.0F);
 
 	// update the reference geomagnetic vector using magnetic disturbance error if valid calibration and no jamming
-	if (MagCal->iValidMagCal && !iMagJamming) {
+	if (MagCal->ValidMagCal && !iMagJamming) {
 		// de-rotate the NED magnetic disturbance error de+ from the sensor to the global reference frame
 		// using the inverse (transpose) of the a posteriori rotation matrix
-		SV->fdErrGlPl[X] = SV->fRPl[X][X] * SV->fdErrSePl[X] +
+		SV->dErrGlPl[X] = SV->RPl[X][X] * SV->dErrSePl[X] +
-				SV->fRPl[Y][X] * SV->fdErrSePl[Y] +
+				SV->RPl[Y][X] * SV->dErrSePl[Y] +
-				SV->fRPl[Z][X] * SV->fdErrSePl[Z];
+				SV->RPl[Z][X] * SV->dErrSePl[Z];
-		SV->fdErrGlPl[Z] = SV->fRPl[X][Z] * SV->fdErrSePl[X] +
+		SV->dErrGlPl[Z] = SV->RPl[X][Z] * SV->dErrSePl[X] +
-				SV->fRPl[Y][Z] * SV->fdErrSePl[Y] +
+				SV->RPl[Y][Z] * SV->dErrSePl[Y] +
-				SV->fRPl[Z][Z] * SV->fdErrSePl[Z];
+				SV->RPl[Z][Z] * SV->dErrSePl[Z];
 
 		// compute components of the new geomagnetic vector
 		// the north pointing component fadj must always be non-negative
-		fopp = SV->fmGl[Z] - SV->fdErrGlPl[Z];
+		fopp = SV->mGl[Z] - SV->dErrGlPl[Z];
-		fadj = SV->fmGl[X] - SV->fdErrGlPl[X];
+		fadj = SV->mGl[X] - SV->dErrGlPl[X];
 		if (fadj < 0.0F) {
 			fadj = 0.0F;
 		}
@@ -571,19 +571,19 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 			}
 
 			// compute the new geomagnetic vector (always north pointing)
-			SV->fDeltaPl = fasin_deg(fsindelta);
+			SV->DeltaPl = fasin_deg(fsindelta);
-			SV->fmGl[X] = MagCal->fB * fcosdelta;
+			SV->mGl[X] = MagCal->B * fcosdelta;
-			SV->fmGl[Z] = MagCal->fB * fsindelta;
+			SV->mGl[Z] = MagCal->B * fsindelta;
 		} // end hyp == 0.0F
-	} // end iValidMagCal
+	} // end ValidMagCal
 
 	// *********************************************************************************
 	// compute the a posteriori Euler angles from the orientation matrix
 	// *********************************************************************************
 
 	// calculate the NED Euler angles
-	fNEDAnglesDegFromRotationMatrix(SV->fRPl, &(SV->fPhiPl), &(SV->fThePl), &(SV->fPsiPl),
+	fNEDAnglesDegFromRotationMatrix(SV->RPl, &(SV->PhiPl), &(SV->ThePl), &(SV->PsiPl),
-			&(SV->fRhoPl), &(SV->fChiPl));
+			&(SV->RhoPl), &(SV->ChiPl));
 
 	// ***********************************************************************************
 	// calculate (symmetric) a posteriori error covariance matrix P+
@@ -597,17 +597,17 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 	// the resulting matrix is sparse but not symmetric
 	for (i = 0; i < 6; i++) {
 		// initialize pfPPlus12x12ij for current i, j=0
-		pfPPlus12x12ij = SV->fPPlus12x12[i];
+		pfPPlus12x12ij = SV->PPlus12x12[i];
 
 		for (j = 0; j < 12; j++) {
-			// zero P+[i][j] 
+			// zero P+[i][j]
 			*pfPPlus12x12ij = 0.0F;
 
 			// initialize pfC6x12ik for current i, k=0
-			pfC6x12ik = SV->fC6x12[i];
+			pfC6x12ik = SV->C6x12[i];
 
 			// initialize pfQw12x12kj for current j, k=0
-			pfQw12x12kj = &SV->fQw12x12[0][j];
+			pfQw12x12kj = &SV->Qw12x12[0][j];
 
 			// sum matrix products over inner loop over k
 			for (k = 0; k < 12; k++) {
@@ -624,13 +624,13 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 				pfC6x12ik++;
 				pfQw12x12kj += 12;
 
-			} // end of loop over k
+			}
 
 			// increment pfPPlus12x12ij for next iteration of j
 			pfPPlus12x12ij++;
 
-		} // end of loop over j
+		}
-	} // end of loop over i
+	}
 
 	// compute P+ = (I12 - K * C) * Qw = Qw - K * (C * Qw) = Qw - K * P+ (6x12 sub-matrix)
 	// storing result P+ in Qw and over-writing Qw which is OK since Qw is later computed from P+
@@ -638,14 +638,14 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 	// only on and above diagonal terms of P+ are computed since P+ is symmetric
 	for (i = 0; i < 12; i++) {
 		// initialize pfQw12x12ij for current i, j=i
-		pfQw12x12ij = SV->fQw12x12[i] + i;
+		pfQw12x12ij = SV->Qw12x12[i] + i;
 
 		for (j = i; j < 12; j++) {
 			// initialize pfK12x6ik for current i, k=0
-			pfK12x6ik = SV->fK12x6[i];
+			pfK12x6ik = SV->K12x6[i];
 
 			// initialize pfPPlus12x12kj for current j, k=0
-			pfPPlus12x12kj = *SV->fPPlus12x12 + j;
+			pfPPlus12x12kj = *SV->PPlus12x12 + j;
 
 			// compute on and above diagonal matrix entry
 			for (k = 0; k < 6; k++) {
@@ -653,34 +653,30 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 				if (*pfPPlus12x12kj != 0.0F) {
 					*pfQw12x12ij -= *pfK12x6ik * *pfPPlus12x12kj;
 				}
-
 				// increment pfK12x6ik and pfPPlus12x12kj for next iteration of k
 				pfK12x6ik++;
 				pfPPlus12x12kj += 12;
-
+			}
-			} // end of loop over k
 
 			// increment pfQw12x12ij for next iteration of j
 			pfQw12x12ij++;
-
+		}
-		} // end of loop over j
+	}
-	} // end of loop over i
 
 	// Qw now holds the on and above diagonal elements of P+
 	// so perform a simple copy to the all elements of P+
 	// after execution of this code P+ is valid but Qw remains invalid
 	for (i = 0; i < 12; i++) {
 		// initialize pfPPlus12x12ij and pfQw12x12ij for current i, j=i
-		pfPPlus12x12ij = SV->fPPlus12x12[i] + i;
+		pfPPlus12x12ij = SV->PPlus12x12[i] + i;
-		pfQw12x12ij = SV->fQw12x12[i] + i;
+		pfQw12x12ij = SV->Qw12x12[i] + i;
 
 		// copy the on-diagonal elements and increment pointers to enter loop at j=i+1
 		*(pfPPlus12x12ij++) = *(pfQw12x12ij++);
 
 		// loop over above diagonal columns j copying to below-diagonal elements
-		for (j = i + 1; j < 12; j++)
+		for (j = i + 1; j < 12; j++) {
-		{
+			*(pfPPlus12x12ij++)= SV->PPlus12x12[j][i] = *(pfQw12x12ij++);
-			*(pfPPlus12x12ij++)= SV->fPPlus12x12[j][i] = *(pfQw12x12ij++);
 		}
 	}
 
@@ -693,26 +689,26 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 	// zero the matrix Qw
 	for (i = 0; i < 12; i++) {
 		for (j = 0; j < 12; j++) {
-			SV->fQw12x12[i][j] = 0.0F;
+			SV->Qw12x12[i][j] = 0.0F;
 		}
 	}
 
 	// update the covariance matrix components
 	for (i = 0; i < 3; i++) {
 		// Qw[th-th-] = Qw[0-2][0-2] = E[th-(th-)^T] = Q[th+th+] + deltat^2 * (Q[b+b+] + (Qwb + QvG) * I)
-		SV->fQw12x12[i][i] = SV->fPPlus12x12[i][i] + SV->fdeltatsq * (SV->fPPlus12x12[i + 3][i + 3] + SV->fQwbplusQvG);
+		SV->Qw12x12[i][i] = SV->PPlus12x12[i][i] + SV->deltatsq * (SV->PPlus12x12[i + 3][i + 3] + SV->QwbplusQvG);
 
 		// Qw[b-b-] = Qw[3-5][3-5] = E[b-(b-)^T] = Q[b+b+] + Qwb * I
-		SV->fQw12x12[i + 3][i + 3] = SV->fPPlus12x12[i + 3][i + 3] + FQWB_9DOF_GBY_KALMAN;
+		SV->Qw12x12[i + 3][i + 3] = SV->PPlus12x12[i + 3][i + 3] + FQWB_9DOF_GBY_KALMAN;
 
 		// Qw[th-b-] = Qw[0-2][3-5] = E[th-(b-)^T] = -deltat * (Q[b+b+] + Qwb * I) = -deltat * Qw[b-b-]
-		SV->fQw12x12[i][i + 3] = SV->fQw12x12[i + 3][i] = -SV->fdeltat * SV->fQw12x12[i + 3][i + 3];
+		SV->Qw12x12[i][i + 3] = SV->Qw12x12[i + 3][i] = -SV->deltat * SV->Qw12x12[i + 3][i + 3];
 
 		// Qw[a-a-] = Qw[6-8][6-8] = E[a-(a-)^T] = ca^2 * Q[a+a+] + Qwa * I
-		SV->fQw12x12[i + 6][i + 6] = SV->fcasq * SV->fPPlus12x12[i + 6][i + 6] + FQWA_9DOF_GBY_KALMAN;
+		SV->Qw12x12[i + 6][i + 6] = SV->casq * SV->PPlus12x12[i + 6][i + 6] + FQWA_9DOF_GBY_KALMAN;
 
 		// Qw[d-d-] = Qw[9-11][9-11] = E[d-(d-)^T] = cd^2 * Q[d+d+] + Qwd * I
-		SV->fQw12x12[i + 9][i + 9] = SV->fcdsq * SV->fPPlus12x12[i + 9][i + 9] + FQWD_9DOF_GBY_KALMAN;
+		SV->Qw12x12[i + 9][i + 9] = SV->cdsq * SV->PPlus12x12[i + 9][i + 9] + FQWD_9DOF_GBY_KALMAN;
 	}
 }
 
@@ -731,7 +727,7 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 void f3DOFTiltNED(float fR[][3], float fGp[])
 {
 	// the NED self-consistency twist occurs at 90 deg pitch
-	
+
 	// local variables
 	int16_t i;				// counter
 	float fmodGxyz;			// modulus of the x, y, z accelerometer readings
@@ -788,7 +784,7 @@ void f3DOFTiltNED(float fR[][3], float fGp[])
 
 // Aerospace NED magnetometer 3DOF flat eCompass function computing rotation matrix fR
 void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
-{	
+{
 	// local variables
 	float fmodBxy;			// modulus of the x, y magnetometer readings
 
@@ -804,7 +800,7 @@ void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
 	// define the fixed entries in the z row and column
 	fR[Z][X] = fR[Z][Y] = fR[X][Z] = fR[Y][Z] = 0.0F;
 	fR[Z][Z] = 1.0F;
-	
+
 	// define the remaining entries
 	fR[X][X] = fR[Y][Y] = fBc[X] / fmodBxy;
 	fR[Y][X] = fBc[Y] / fmodBxy;
@@ -912,7 +908,7 @@ static void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg,
 	// for NED, the compass heading Rho equals the yaw angle Psi
 	*pfRhoDeg = *pfPsiDeg;
 
-	// calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg) 
+	// calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
 	*pfChiDeg = facos_deg(R[Z][Z]);
 
 	return;
@@ -1099,7 +1095,7 @@ void fRotationVectorDegFromRotationMatrix(float R[][3], float rvecdeg[])
 		if ((R[Y][Z] - R[Z][Y]) < 0.0F) rvecdeg[X] = -rvecdeg[X];
 		if ((R[Z][X] - R[X][Z]) < 0.0F) rvecdeg[Y] = -rvecdeg[Y];
 		if ((R[X][Y] - R[Y][X]) < 0.0F) rvecdeg[Z] = -rvecdeg[Z];
-		
+
 	}
 }
 
@@ -1117,12 +1113,12 @@ static void fRotationVectorDegFromQuaternion(Quaternion_t *pq, float rvecdeg[])
 		fetarad = 0.0F;
 		fetadeg = 0.0F;
 	} else {
-		// general case returning 0 < eta < 360 deg 
+		// general case returning 0 < eta < 360 deg
-		fetarad = 2.0F * acosf(pq->q0); 
+		fetarad = 2.0F * acosf(pq->q0);
 		fetadeg = fetarad * FRADTODEG;
 	}
 
-	// map the rotation angle onto the range -180 deg <= eta < 180 deg 
+	// map the rotation angle onto the range -180 deg <= eta < 180 deg
 	if (fetadeg >= 180.0F) {
 		fetadeg -= 360.0F;
 		fetarad = fetadeg * FDEGTORAD;
@@ -1133,7 +1129,7 @@ static void fRotationVectorDegFromQuaternion(Quaternion_t *pq, float rvecdeg[])
 
 	// calculate the rotation vector (deg)
 	if (sinhalfeta == 0.0F) {
-		// the rotation angle eta is zero and the axis is irrelevant 
+		// the rotation angle eta is zero and the axis is irrelevant
 		rvecdeg[X] = rvecdeg[Y] = rvecdeg[Z] = 0.0F;
 	} else {
 		// general case with non-zero rotation angle
@@ -1339,7 +1335,7 @@ static float fatan_deg(float x)
 
 	// reset all flags
 	ixisnegative = ixexceeds1 = ixmapped = 0;
-	
+
 	// test for negative argument to allow use of tan(-x)=-tan(x)
 	if (x < 0.0F) {
 		x = -x;
@@ -1369,7 +1365,7 @@ static float fatan_deg(float x)
 	if (ixmapped) fangledeg += 30.0F;
 	if (ixexceeds1) fangledeg = 90.0F - fangledeg;
 	if (ixisnegative) fangledeg = -fangledeg;
-	
+
 	return (fangledeg);
 }
 
@@ -1386,7 +1382,7 @@ static float fatan2_deg(float y, float x)
 		// otherwise y= 0.0 and return 0 deg (invalid arguments)
 		return 0.0F;
 	}
-	
+
 	// from here onwards, x is guaranteed to be non-zero
 	// compute atan2 for quadrant 1 (0 to 90 deg) and quadrant 4 (-90 to 0 deg)
 	if (x > 0.0F) return (fatan_deg(y / x));

imuread.c

@@ -23,18 +23,18 @@ static void glut_display_callback(void)
 
 static void glut_keystroke_callback(unsigned char ch, int x, int y)
 {
-	if (magcal.fFitErrorpc > 9.0) {
+	if (magcal.FitError > 9.0) {
 		printf("Poor Calibration: ");
-		printf("soft iron fit error = %.1f%%\n", magcal.fFitErrorpc);
+		printf("soft iron fit error = %.1f%%\n", magcal.FitError);
 		return;
 	}
-	printf("Magnetic Calibration:   (%.1f%% fit error)\n", magcal.fFitErrorpc);
+	printf("Magnetic Calibration:   (%.1f%% fit error)\n", magcal.FitError);
 	printf("   %7.2f   %6.3f %6.3f %6.3f\n",
-		magcal.fV[0], magcal.finvW[0][0], magcal.finvW[0][1], magcal.finvW[0][2]);
+		magcal.V[0], magcal.invW[0][0], magcal.invW[0][1], magcal.invW[0][2]);
 	printf("   %7.2f   %6.3f %6.3f %6.3f\n",
-		magcal.fV[1], magcal.finvW[1][0], magcal.finvW[1][1], magcal.finvW[1][2]);
+		magcal.V[1], magcal.invW[1][0], magcal.invW[1][1], magcal.invW[1][2]);
 	printf("   %7.2f   %6.3f %6.3f %6.3f\n",
-		magcal.fV[2], magcal.finvW[2][0], magcal.finvW[2][1], magcal.finvW[2][2]);
+		magcal.V[2], magcal.invW[2][0], magcal.invW[2][1], magcal.invW[2][2]);
 	send_calibration();
 }
 

imuread.h

@@ -76,26 +76,26 @@ void MagCal_Run(void);
 
 // magnetic calibration & buffer structure
 typedef struct {
-    float fV[3];                  // current hard iron offset x, y, z, (uT)
+    float V[3];                  // current hard iron offset x, y, z, (uT)
-    float finvW[3][3];            // current inverse soft iron matrix
+    float invW[3][3];            // current inverse soft iron matrix
-    float fB;                     // current geomagnetic field magnitude (uT)
+    float B;                     // current geomagnetic field magnitude (uT)
-    float fFourBsq;               // current 4*B*B (uT^2)
+    float FourBsq;               // current 4*B*B (uT^2)
-    float fFitErrorpc;            // current fit error %
+    float FitError;              // current fit error %
-    float fFitErrorAge;           // current fit error % (grows automatically with age)
+    float FitErrorAge;           // current fit error % (grows automatically with age)
-    float ftrV[3];                // trial value of hard iron offset z, y, z (uT)
+    float trV[3];                // trial value of hard iron offset z, y, z (uT)
-    float ftrinvW[3][3];          // trial inverse soft iron matrix size
+    float trinvW[3][3];          // trial inverse soft iron matrix size
-    float ftrB;                   // trial value of geomagnetic field magnitude in uT
+    float trB;                   // trial value of geomagnetic field magnitude in uT
-    float ftrFitErrorpc;          // trial value of fit error %
+    float trFitErrorpc;          // trial value of fit error %
-    float fA[3][3];               // ellipsoid matrix A
+    float A[3][3];               // ellipsoid matrix A
-    float finvA[3][3];            // inverse of ellipsoid matrix A
+    float invA[3][3];            // inverse of ellipsoid matrix A
-    float fmatA[10][10];          // scratch 10x10 matrix used by calibration algorithms
+    float matA[10][10];          // scratch 10x10 matrix used by calibration algorithms
-    float fmatB[10][10];          // scratch 10x10 matrix used by calibration algorithms
+    float matB[10][10];          // scratch 10x10 matrix used by calibration algorithms
-    float fvecA[10];              // scratch 10x1 vector used by calibration algorithms
+    float vecA[10];              // scratch 10x1 vector used by calibration algorithms
-    float fvecB[4];               // scratch 4x1 vector used by calibration algorithms
+    float vecB[4];               // scratch 4x1 vector used by calibration algorithms
-    int8_t iValidMagCal;          // integer value 0, 4, 7, 10 denoting both valid calibration and solver used
+    int8_t ValidMagCal;          // integer value 0, 4, 7, 10 denoting both valid calibration and solver used
-    int16_t iBpFast[3][MAGBUFFSIZE];   // uncalibrated magnetometer readings
+    int16_t BpFast[3][MAGBUFFSIZE];   // uncalibrated magnetometer readings
     int8_t  valid[MAGBUFFSIZE];        // 1=has data, 0=empty slot
-    int16_t iMagBufferCount;           // number of magnetometer readings
+    int16_t MagBufferCount;           // number of magnetometer readings
 } MagCalibration_t;
 
 extern MagCalibration_t magcal;
@@ -113,15 +113,17 @@ void fmatrixAeqInvA(float *A[], int8_t iColInd[], int8_t iRowInd[], int8_t iPivo
 void fmatrixAeqRenormRotA(float A[][3]);
 
 
+#define SENSORFS 100
 #define OVERSAMPLE_RATIO 4
 
+
 // accelerometer sensor structure definition
 #define G_PER_COUNT 0.0001220703125F  // = 1/8192
 typedef struct
 {
 	//int32_t iSumGpFast[3];    // sum of fast measurements
-	float fGpFast[3];       // fast (typically 200Hz) readings (g)
+	float GpFast[3];       // fast (typically 200Hz) readings (g)
-	float fGp[3];           // slow (typically 25Hz) averaged readings (g)
+	float Gp[3];           // slow (typically 25Hz) averaged readings (g)
 	//float fgPerCount;       // initialized to FGPERCOUNT
 	//int16_t iGpFast[3];       // fast (typically 200Hz) readings
 	//int16_t iGp[3];           // slow (typically 25Hz) averaged readings (counts)
@@ -134,8 +136,8 @@ typedef struct
 	//int32_t iSumBpFast[3];    // sum of fast measurements
 	//float fBpFast[3];       // fast (typically 200Hz) raw readings (uT)
 	//float fBp[3];           // slow (typically 25Hz) averaged raw readings (uT)
-	float fBcFast[3];       // fast (typically 200Hz) calibrated readings (uT)
+	float BcFast[3];       // fast (typically 200Hz) calibrated readings (uT)
-	float fBc[3];           // slow (typically 25Hz) averaged calibrated readings (uT)
+	float Bc[3];           // slow (typically 25Hz) averaged calibrated readings (uT)
 	//float fuTPerCount;      // initialized to FUTPERCOUNT
 	//float fCountsPeruT;     // initialized to FCOUNTSPERUT
 	//int16_t iBpFast[3];       // fast (typically 200Hz) raw readings (counts)
@@ -148,9 +150,9 @@ typedef struct
 typedef struct
 {
 	//int32_t iSumYpFast[3];                    // sum of fast measurements
-	float fYp[3];                           // raw gyro sensor output (deg/s)
+	float Yp[3];                           // raw gyro sensor output (deg/s)
 	//float fDegPerSecPerCount;               // initialized to FDEGPERSECPERCOUNT
-	int16_t iYpFast[OVERSAMPLE_RATIO][3];     // fast (typically 200Hz) readings
+	int16_t YpFast[OVERSAMPLE_RATIO][3];     // fast (typically 200Hz) readings
 	//int16_t iYp[3];                           // averaged gyro sensor output (counts)
 } GyroSensor_t;
 
@@ -161,62 +163,62 @@ typedef struct
 {
 	// start: elements common to all motion state vectors
 	// Euler angles
-	float fPhiPl;			// roll (deg)
+	float PhiPl;			// roll (deg)
-	float fThePl;			// pitch (deg)
+	float ThePl;			// pitch (deg)
-	float fPsiPl;			// yaw (deg)
+	float PsiPl;			// yaw (deg)
-	float fRhoPl;			// compass (deg)
+	float RhoPl;			// compass (deg)
-	float fChiPl;			// tilt from vertical (deg)
+	float ChiPl;			// tilt from vertical (deg)
 	// orientation matrix, quaternion and rotation vector
-	float fRPl[3][3];		// a posteriori orientation matrix
+	float RPl[3][3];		// a posteriori orientation matrix
-	Quaternion_t fqPl;		// a posteriori orientation quaternion
+	Quaternion_t qPl;		// a posteriori orientation quaternion
-	float fRVecPl[3];		// rotation vector
+	float RVecPl[3];		// rotation vector
 	// angular velocity
-	float fOmega[3];		// angular velocity (deg/s)
+	float Omega[3];			// angular velocity (deg/s)
 	// systick timer for benchmarking
 	int32_t systick;		// systick timer;
 	// end: elements common to all motion state vectors
 
 	// elements transmitted over bluetooth in kalman packet
-	float fbPl[3];			// gyro offset (deg/s)
+	float bPl[3];			// gyro offset (deg/s)
-	float fThErrPl[3];		// orientation error (deg)
+	float ThErrPl[3];		// orientation error (deg)
-	float fbErrPl[3];		// gyro offset error (deg/s)
+	float bErrPl[3];		// gyro offset error (deg/s)
 	// end elements transmitted in kalman packet
 
-	float fdErrGlPl[3];		// magnetic disturbance error (uT, global frame)
+	float dErrGlPl[3];		// magnetic disturbance error (uT, global frame)
-	float fdErrSePl[3];		// magnetic disturbance error (uT, sensor frame)
+	float dErrSePl[3];		// magnetic disturbance error (uT, sensor frame)
-	float faErrSePl[3];		// linear acceleration error (g, sensor frame)
+	float aErrSePl[3];		// linear acceleration error (g, sensor frame)
-	float faSeMi[3];		// linear acceleration (g, sensor frame)
+	float aSeMi[3];			// linear acceleration (g, sensor frame)
-	float fDeltaPl;			// inclination angle (deg)
+	float DeltaPl;			// inclination angle (deg)
-	float faSePl[3];		// linear acceleration (g, sensor frame)
+	float aSePl[3];			// linear acceleration (g, sensor frame)
-	float faGlPl[3];		// linear acceleration (g, global frame)
+	float aGlPl[3];			// linear acceleration (g, global frame)
-	float fgErrSeMi[3];		// difference (g, sensor frame) of gravity vector (accel) and gravity vector (gyro)
+	float gErrSeMi[3];		// difference (g, sensor frame) of gravity vector (accel) and gravity vector (gyro)
-	float fmErrSeMi[3];		// difference (uT, sensor frame) of geomagnetic vector (magnetometer) and geomagnetic vector (gyro)
+	float mErrSeMi[3];		// difference (uT, sensor frame) of geomagnetic vector (magnetometer) and geomagnetic vector (gyro)
-	float fgSeGyMi[3];		// gravity vector (g, sensor frame) measurement from gyro
+	float gSeGyMi[3];		// gravity vector (g, sensor frame) measurement from gyro
-	float fmSeGyMi[3];		// geomagnetic vector (uT, sensor frame) measurement from gyro
+	float mSeGyMi[3];		// geomagnetic vector (uT, sensor frame) measurement from gyro
-	float fmGl[3];			// geomagnetic vector (uT, global frame)
+	float mGl[3];			// geomagnetic vector (uT, global frame)
-	float fQvAA;			// accelerometer terms of Qv
+	float QvAA;			// accelerometer terms of Qv
-	float fQvMM;			// magnetometer terms of Qv
+	float QvMM;			// magnetometer terms of Qv
-	float fPPlus12x12[12][12];	// covariance matrix P+
+	float PPlus12x12[12][12];	// covariance matrix P+
-	float fK12x6[12][6];		// kalman filter gain matrix K
+	float K12x6[12][6];		// kalman filter gain matrix K
-	float fQw12x12[12][12];		// covariance matrix Qw
+	float Qw12x12[12][12];		// covariance matrix Qw
-	float fC6x12[6][12];		// measurement matrix C
+	float C6x12[6][12];		// measurement matrix C
-	float fRMi[3][3];		// a priori orientation matrix
+	float RMi[3][3];		// a priori orientation matrix
-	Quaternion_t fDeltaq;		// delta quaternion
+	Quaternion_t Deltaq;		// delta quaternion
-	Quaternion_t fqMi;		// a priori orientation quaternion
+	Quaternion_t qMi;		// a priori orientation quaternion
-	float fcasq;			// FCA * FCA;
+	float casq;			// FCA * FCA;
-	float fcdsq;			// FCD * FCD;
+	float cdsq;			// FCD * FCD;
-	float fFastdeltat;		// sensor sampling interval (s) = 1 / SENSORFS
+	float Fastdeltat;		// sensor sampling interval (s) = 1 / SENSORFS
-	float fdeltat;			// kalman filter sampling interval (s) = OVERSAMPLE_RATIO / SENSORFS
+	float deltat;			// kalman filter sampling interval (s) = OVERSAMPLE_RATIO / SENSORFS
-	float fdeltatsq;		// fdeltat * fdeltat
+	float deltatsq;			// fdeltat * fdeltat
-	float fQwbplusQvG;		// FQWB + FQVG
+	float QwbplusQvG;		// FQWB + FQVG
-	int16_t iFirstOrientationLock;	// denotes that 9DOF orientation has locked to 6DOF
+	int16_t FirstOrientationLock;	// denotes that 9DOF orientation has locked to 6DOF
 	int8_t resetflag;		// flag to request re-initialization on next pass
 } SV_9DOF_GBY_KALMAN_t;
 
-void fInit_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV, int16_t iSensorFS, int16_t iOverSampleRatio);
+void fInit_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV);
 void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
 	const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro,
-	const MagCalibration_t *MagCal, int16_t iOverSampleRatio);
+	const MagCalibration_t *MagCal);
 
 
 

magcal.c

@@ -44,8 +44,8 @@
 #define MINMEASUREMENTS4CAL 40      // minimum number of measurements for 4 element calibration
 #define MINMEASUREMENTS7CAL 100     // minimum number of measurements for 7 element calibration
 #define MINMEASUREMENTS10CAL 150    // minimum number of measurements for 10 element calibration
-#define MINBFITUT 22.0F             // minimum geomagnetic field B (uT) for valid calibration
+#define MINBFITUT 15.0F             // minimum geomagnetic field B (uT) for valid calibration
-#define MAXBFITUT 67.0F             // maximum geomagnetic field B (uT) for valid calibration
+#define MAXBFITUT 80.0F             // maximum geomagnetic field B (uT) for valid calibration
 #define FITERRORAGINGSECS 7200.0F   // 2 hours: time for fit error to increase (age) by e=2.718
 
 static void fUpdateCalibration4INV(MagCalibration_t *MagCal);
@@ -73,9 +73,9 @@ void MagCal_Run(void)
 
 	if (count < MINMEASUREMENTS4CAL) return;
 
-	if (magcal.iValidMagCal) {
+	if (magcal.ValidMagCal) {
 		// age the existing fit error to avoid one good calibration locking out future updates
-		magcal.fFitErrorAge *= 1.01f;
+		magcal.FitErrorAge *= 1.01f;
 	}
 
 	// is enough data collected
@@ -91,25 +91,25 @@ void MagCal_Run(void)
 	}
 
 	// the trial geomagnetic field must be in range (earth is 22uT to 67uT)
-	if ((magcal.ftrB >= MINBFITUT) && (magcal.ftrB <= MAXBFITUT))	{
+	if ((magcal.trB >= MINBFITUT) && (magcal.trB <= MAXBFITUT))	{
 		// always accept the calibration if
 		//  1: no previous calibration exists
 		//  2: the calibration fit is reduced or
 		//  3: an improved solver was used giving a good trial calibration (4% or under)
-		if ((magcal.iValidMagCal == 0) ||
+		if ((magcal.ValidMagCal == 0) ||
-				(magcal.ftrFitErrorpc <= magcal.fFitErrorAge) ||
+				(magcal.trFitErrorpc <= magcal.FitErrorAge) ||
-				((isolver > magcal.iValidMagCal) && (magcal.ftrFitErrorpc <= 4.0F))) {
+				((isolver > magcal.ValidMagCal) && (magcal.trFitErrorpc <= 4.0F))) {
 			// accept the new calibration solution
-			//printf("new magnetic cal, B=%.2f uT\n", magcal.ftrB);
+			//printf("new magnetic cal, B=%.2f uT\n", magcal.trB);
-			magcal.iValidMagCal = isolver;
+			magcal.ValidMagCal = isolver;
-			magcal.fFitErrorpc = magcal.ftrFitErrorpc;
+			magcal.FitError = magcal.trFitErrorpc;
-			magcal.fFitErrorAge = magcal.ftrFitErrorpc;
+			magcal.FitErrorAge = magcal.trFitErrorpc;
-			magcal.fB = magcal.ftrB;
+			magcal.B = magcal.trB;
-			magcal.fFourBsq = 4.0F * magcal.ftrB * magcal.ftrB;
+			magcal.FourBsq = 4.0F * magcal.trB * magcal.trB;
 			for (i = X; i <= Z; i++) {
-				magcal.fV[i] = magcal.ftrV[i];
+				magcal.V[i] = magcal.trV[i];
 				for (j = X; j <= Z; j++) {
-					magcal.finvW[i][j] = magcal.ftrinvW[i][j];
+					magcal.invW[i][j] = magcal.trinvW[i][j];
 				}
 			}
 		}
@@ -128,7 +128,7 @@ static void fUpdateCalibration4INV(MagCalibration_t *MagCal)
 	int16_t iOffset[3];			// offset to remove large DC hard iron bias in matrix
 	int16_t iCount;				// number of measurements counted
 	int i, j, k;				// loop counters
-	
+
 	// working arrays for 4x4 matrix inversion
 	float *pfRows[4];
 	int8_t iColInd[4];
@@ -140,15 +140,15 @@ static void fUpdateCalibration4INV(MagCalibration_t *MagCal)
 
 	// the trial inverse soft iron matrix invW always equals
 	// the identity matrix for 4 element calibration
-	f3x3matrixAeqI(MagCal->ftrinvW);
+	f3x3matrixAeqI(MagCal->trinvW);
 
-	// zero fSumBp4=Y^T.Y, fvecB=X^T.Y (4x1) and on and above
+	// zero fSumBp4=Y^T.Y, vecB=X^T.Y (4x1) and on and above
-	// diagonal elements of fmatA=X^T*X (4x4)
+	// diagonal elements of matA=X^T*X (4x4)
 	fSumBp4 = 0.0F;
 	for (i = 0; i < 4; i++) {
-		MagCal->fvecB[i] = 0.0F;
+		MagCal->vecB[i] = 0.0F;
 		for (j = i; j < 4; j++) {
-			MagCal->fmatA[i][j] = 0.0F;
+			MagCal->matA[i][j] = 0.0F;
 		}
 	}
 
@@ -162,41 +162,41 @@ static void fUpdateCalibration4INV(MagCalibration_t *MagCal)
 			// use first valid magnetic buffer entry as estimate (in counts) for offset
 			if (iCount == 0) {
 				for (k = X; k <= Z; k++) {
-					iOffset[k] = MagCal->iBpFast[k][j];
+					iOffset[k] = MagCal->BpFast[k][j];
 				}
 			}
 
-			// store scaled and offset fBp[XYZ] in fvecA[0-2] and fBp[XYZ]^2 in fvecA[3-5]
+			// store scaled and offset fBp[XYZ] in vecA[0-2] and fBp[XYZ]^2 in vecA[3-5]
 			for (k = X; k <= Z; k++) {
-				MagCal->fvecA[k] = (float)((int32_t)MagCal->iBpFast[k][j]
+				MagCal->vecA[k] = (float)((int32_t)MagCal->BpFast[k][j]
 					- (int32_t)iOffset[k]) * fscaling;
-				MagCal->fvecA[k + 3] = MagCal->fvecA[k] * MagCal->fvecA[k];
+				MagCal->vecA[k + 3] = MagCal->vecA[k] * MagCal->vecA[k];
 			}
 
-			// calculate fBp2 = fBp[X]^2 + fBp[Y]^2 + fBp[Z]^2 (scaled uT^2)
+			// calculate fBp2 = Bp[X]^2 + Bp[Y]^2 + Bp[Z]^2 (scaled uT^2)
-			fBp2 = MagCal->fvecA[3] + MagCal->fvecA[4] + MagCal->fvecA[5];
+			fBp2 = MagCal->vecA[3] + MagCal->vecA[4] + MagCal->vecA[5];
 
 			// accumulate fBp^4 over all measurements into fSumBp4=Y^T.Y
 			fSumBp4 += fBp2 * fBp2;
 
-			// now we have fBp2, accumulate fvecB[0-2] = X^T.Y =sum(fBp2.fBp[XYZ])
+			// now we have fBp2, accumulate vecB[0-2] = X^T.Y =sum(Bp2.Bp[XYZ])
 			for (k = X; k <= Z; k++) {
-				MagCal->fvecB[k] += MagCal->fvecA[k] * fBp2;
+				MagCal->vecB[k] += MagCal->vecA[k] * fBp2;
 			}
 
-			//accumulate fvecB[3] = X^T.Y =sum(fBp2)
+			//accumulate vecB[3] = X^T.Y =sum(fBp2)
-			MagCal->fvecB[3] += fBp2;
+			MagCal->vecB[3] += fBp2;
 
-			// accumulate on and above-diagonal terms of fmatA = X^T.X ignoring fmatA[3][3]
+			// accumulate on and above-diagonal terms of matA = X^T.X ignoring matA[3][3]
-			MagCal->fmatA[0][0] += MagCal->fvecA[X + 3];
+			MagCal->matA[0][0] += MagCal->vecA[X + 3];
-			MagCal->fmatA[0][1] += MagCal->fvecA[X] * MagCal->fvecA[Y];
+			MagCal->matA[0][1] += MagCal->vecA[X] * MagCal->vecA[Y];
-			MagCal->fmatA[0][2] += MagCal->fvecA[X] * MagCal->fvecA[Z];
+			MagCal->matA[0][2] += MagCal->vecA[X] * MagCal->vecA[Z];
-			MagCal->fmatA[0][3] += MagCal->fvecA[X];
+			MagCal->matA[0][3] += MagCal->vecA[X];
-			MagCal->fmatA[1][1] += MagCal->fvecA[Y + 3];
+			MagCal->matA[1][1] += MagCal->vecA[Y + 3];
-			MagCal->fmatA[1][2] += MagCal->fvecA[Y] * MagCal->fvecA[Z];
+			MagCal->matA[1][2] += MagCal->vecA[Y] * MagCal->vecA[Z];
-			MagCal->fmatA[1][3] += MagCal->fvecA[Y];
+			MagCal->matA[1][3] += MagCal->vecA[Y];
-			MagCal->fmatA[2][2] += MagCal->fvecA[Z + 3];
+			MagCal->matA[2][2] += MagCal->vecA[Z + 3];
-			MagCal->fmatA[2][3] += MagCal->fvecA[Z];
+			MagCal->matA[2][3] += MagCal->vecA[Z];
 
 			// increment the counter for next iteration
 			iCount++;
@@ -204,77 +204,77 @@ static void fUpdateCalibration4INV(MagCalibration_t *MagCal)
 	}
 
 	// set the last element of the measurement matrix to the number of buffer elements used
-	MagCal->fmatA[3][3] = (float) iCount;
+	MagCal->matA[3][3] = (float) iCount;
 
 	// store the number of measurements accumulated
-	MagCal->iMagBufferCount = iCount;
+	MagCal->MagBufferCount = iCount;
 
-	// use above diagonal elements of symmetric fmatA to set both fmatB and fmatA to X^T.X
+	// use above diagonal elements of symmetric matA to set both matB and matA to X^T.X
 	for (i = 0; i < 4; i++) {
 		for (j = i; j < 4; j++) {
-			MagCal->fmatB[i][j] = MagCal->fmatB[j][i]
+			MagCal->matB[i][j] = MagCal->matB[j][i]
-				= MagCal->fmatA[j][i] = MagCal->fmatA[i][j];
+				= MagCal->matA[j][i] = MagCal->matA[i][j];
 		}
 	}
 
-	// calculate in situ inverse of fmatB = inv(X^T.X) (4x4) while fmatA still holds X^T.X
+	// calculate in situ inverse of matB = inv(X^T.X) (4x4) while matA still holds X^T.X
 	for (i = 0; i < 4; i++) {
-		pfRows[i] = MagCal->fmatB[i];
+		pfRows[i] = MagCal->matB[i];
 	}
 	fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 4);
 
-	// calculate fvecA = solution beta (4x1) = inv(X^T.X).X^T.Y = fmatB * fvecB
+	// calculate vecA = solution beta (4x1) = inv(X^T.X).X^T.Y = matB * vecB
 	for (i = 0; i < 4; i++) {
-		MagCal->fvecA[i] = 0.0F;
+		MagCal->vecA[i] = 0.0F;
 		for (k = 0; k < 4; k++) {
-			MagCal->fvecA[i] += MagCal->fmatB[i][k] * MagCal->fvecB[k];
+			MagCal->vecA[i] += MagCal->matB[i][k] * MagCal->vecB[k];
 		}
 	}
 
 	// calculate P = r^T.r = Y^T.Y - 2 * beta^T.(X^T.Y) + beta^T.(X^T.X).beta
-	// = fSumBp4 - 2 * fvecA^T.fvecB + fvecA^T.fmatA.fvecA
+	// = fSumBp4 - 2 * vecA^T.vecB + vecA^T.matA.vecA
-	// first set P = Y^T.Y - 2 * beta^T.(X^T.Y) = fSumBp4 - 2 * fvecA^T.fvecB
+	// first set P = Y^T.Y - 2 * beta^T.(X^T.Y) = SumBp4 - 2 * vecA^T.vecB
 	fE = 0.0F;
 	for (i = 0; i < 4; i++) {
-		fE += MagCal->fvecA[i] * MagCal->fvecB[i];
+		fE += MagCal->vecA[i] * MagCal->vecB[i];
 	}
 	fE = fSumBp4 - 2.0F * fE;
 
-	// set fvecB = (X^T.X).beta = fmatA.fvecA	
+	// set vecB = (X^T.X).beta = matA.vecA
 	for (i = 0; i < 4; i++) {
-		MagCal->fvecB[i] = 0.0F;
+		MagCal->vecB[i] = 0.0F;
 		for (k = 0; k < 4; k++) {
-			MagCal->fvecB[i] += MagCal->fmatA[i][k] * MagCal->fvecA[k];
+			MagCal->vecB[i] += MagCal->matA[i][k] * MagCal->vecA[k];
 		}
 	}
 
-	// complete calculation of P by adding beta^T.(X^T.X).beta = fvecA^T * fvecB
+	// complete calculation of P by adding beta^T.(X^T.X).beta = vecA^T * vecB
 	for (i = 0; i < 4; i++) {
-		fE += MagCal->fvecB[i] * MagCal->fvecA[i];
+		fE += MagCal->vecB[i] * MagCal->vecA[i];
 	}
 
 	// compute the hard iron vector (in uT but offset and scaled by FMATRIXSCALING)
 	for (k = X; k <= Z; k++) {
-		MagCal->ftrV[k] = 0.5F * MagCal->fvecA[k];
+		MagCal->trV[k] = 0.5F * MagCal->vecA[k];
 	}
 
 	// compute the scaled geomagnetic field strength B (in uT but scaled by FMATRIXSCALING)
-	MagCal->ftrB = sqrtf(MagCal->fvecA[3] + MagCal->ftrV[X] * MagCal->ftrV[X] +
+	MagCal->trB = sqrtf(MagCal->vecA[3] + MagCal->trV[X] * MagCal->trV[X] +
-			MagCal->ftrV[Y] * MagCal->ftrV[Y] + MagCal->ftrV[Z] * MagCal->ftrV[Z]);
+			MagCal->trV[Y] * MagCal->trV[Y] + MagCal->trV[Z] * MagCal->trV[Z]);
 
 	// calculate the trial fit error (percent) normalized to number of measurements
 	// and scaled geomagnetic field strength
-	MagCal->ftrFitErrorpc = sqrtf(fE / (float) MagCal->iMagBufferCount) * 100.0F /
+	MagCal->trFitErrorpc = sqrtf(fE / (float) MagCal->MagBufferCount) * 100.0F /
-			(2.0F * MagCal->ftrB * MagCal->ftrB);
+			(2.0F * MagCal->trB * MagCal->trB);
 
 	// correct the hard iron estimate for FMATRIXSCALING and the offsets applied (result in uT)
 	for (k = X; k <= Z; k++) {
-		MagCal->ftrV[k] = MagCal->ftrV[k] * DEFAULTB
+		MagCal->trV[k] = MagCal->trV[k] * DEFAULTB
 			+ (float)iOffset[k] * FXOS8700_UTPERCOUNT;
 	}
 
 	// correct the geomagnetic field strength B to correct scaling (result in uT)
-	MagCal->ftrB *= DEFAULTB;
+	MagCal->trB *= DEFAULTB;
 }
 
 
@@ -302,43 +302,43 @@ static void fUpdateCalibration7EIG(MagCalibration_t *MagCal)
 	// the offsets are guaranteed to be set from the first element but to avoid compiler error
 	iOffset[X] = iOffset[Y] = iOffset[Z] = 0;
 
-	// zero the on and above diagonal elements of the 7x7 symmetric measurement matrix fmatA
+	// zero the on and above diagonal elements of the 7x7 symmetric measurement matrix matA
 	for (m = 0; m < 7; m++) {
 		for (n = m; n < 7; n++) {
-			MagCal->fmatA[m][n] = 0.0F;
+			MagCal->matA[m][n] = 0.0F;
 		}
 	}
 
-	// place from MINEQUATIONS to MAXEQUATIONS entries into product matrix fmatA
+	// place from MINEQUATIONS to MAXEQUATIONS entries into product matrix matA
 	iCount = 0;
 	for (j = 0; j < MAGBUFFSIZE; j++) {
 		if (MagCal->valid[j]) {
 			// use first valid magnetic buffer entry as offset estimate (bit counts)
 			if (iCount == 0) {
 				for (k = X; k <= Z; k++) {
-					iOffset[k] = MagCal->iBpFast[k][j];
+					iOffset[k] = MagCal->BpFast[k][j];
 				}
 			}
 
-			// apply the offset and scaling and store in fvecA
+			// apply the offset and scaling and store in vecA
 			for (k = X; k <= Z; k++) {
-				MagCal->fvecA[k + 3] = (float)((int32_t)MagCal->iBpFast[k][j]
+				MagCal->vecA[k + 3] = (float)((int32_t)MagCal->BpFast[k][j]
 					- (int32_t)iOffset[k]) * fscaling;
-				MagCal->fvecA[k] = MagCal->fvecA[k + 3] * MagCal->fvecA[k + 3];
+				MagCal->vecA[k] = MagCal->vecA[k + 3] * MagCal->vecA[k + 3];
 			}
 
 			// accumulate the on-and above-diagonal terms of
-			// MagCal->fmatA=Sigma{fvecA^T * fvecA}
+			// MagCal->matA=Sigma{vecA^T * vecA}
-			// with the exception of fmatA[6][6] which will sum to the number
+			// with the exception of matA[6][6] which will sum to the number
-			// of measurements and remembering that fvecA[6] equals 1.0F
+			// of measurements and remembering that vecA[6] equals 1.0F
-			// update the right hand column [6] of fmatA except for fmatA[6][6]
+			// update the right hand column [6] of matA except for matA[6][6]
 			for (m = 0; m < 6; m++) {
-				MagCal->fmatA[m][6] += MagCal->fvecA[m];
+				MagCal->matA[m][6] += MagCal->vecA[m];
 			}
 			// update the on and above diagonal terms except for right hand column 6
 			for (m = 0; m < 6; m++) {
 				for (n = m; n < 6; n++) {
-					MagCal->fmatA[m][n] += MagCal->fvecA[m] * MagCal->fvecA[n];
+					MagCal->matA[m][n] += MagCal->vecA[m] * MagCal->vecA[n];
 				}
 			}
 
@@ -347,71 +347,71 @@ static void fUpdateCalibration7EIG(MagCalibration_t *MagCal)
 		}
 	}
 
-	// finally set the last element fmatA[6][6] to the number of measurements
+	// finally set the last element matA[6][6] to the number of measurements
-	MagCal->fmatA[6][6] = (float) iCount;
+	MagCal->matA[6][6] = (float) iCount;
 
 	// store the number of measurements accumulated
-	MagCal->iMagBufferCount = iCount;
+	MagCal->MagBufferCount = iCount;
 
-	// copy the above diagonal elements of fmatA to below the diagonal
+	// copy the above diagonal elements of matA to below the diagonal
 	for (m = 1; m < 7; m++) {
 		for (n = 0; n < m; n++) {
-			MagCal->fmatA[m][n] = MagCal->fmatA[n][m];
+			MagCal->matA[m][n] = MagCal->matA[n][m];
 		}
 	}
 
-	// set tmpA7x1 to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA
+	// set tmpA7x1 to the unsorted eigenvalues and matB to the unsorted eigenvectors of matA
-	eigencompute(MagCal->fmatA, MagCal->fvecA, MagCal->fmatB, 7);
+	eigencompute(MagCal->matA, MagCal->vecA, MagCal->matB, 7);
 
 	// find the smallest eigenvalue
 	j = 0;
 	for (i = 1; i < 7; i++) {
-		if (MagCal->fvecA[i] < MagCal->fvecA[j]) {
+		if (MagCal->vecA[i] < MagCal->vecA[j]) {
 			j = i;
 		}
 	}
 
 	// set ellipsoid matrix A to the solution vector with smallest eigenvalue,
 	// compute its determinant and the hard iron offset (scaled and offset)
-	f3x3matrixAeqScalar(MagCal->fA, 0.0F);
+	f3x3matrixAeqScalar(MagCal->A, 0.0F);
 	det = 1.0F;
 	for (k = X; k <= Z; k++) {
-		MagCal->fA[k][k] = MagCal->fmatB[k][j];
+		MagCal->A[k][k] = MagCal->matB[k][j];
-		det *= MagCal->fA[k][k];
+		det *= MagCal->A[k][k];
-		MagCal->ftrV[k] = -0.5F * MagCal->fmatB[k + 3][j] / MagCal->fA[k][k];
+		MagCal->trV[k] = -0.5F * MagCal->matB[k + 3][j] / MagCal->A[k][k];
 	}
 
 	// negate A if it has negative determinant
 	if (det < 0.0F) {
-		f3x3matrixAeqMinusA(MagCal->fA);
+		f3x3matrixAeqMinusA(MagCal->A);
-		MagCal->fmatB[6][j] = -MagCal->fmatB[6][j];
+		MagCal->matB[6][j] = -MagCal->matB[6][j];
 		det = -det;
 	}
 
 	// set ftmp to the square of the trial geomagnetic field strength B
 	// (counts times FMATRIXSCALING)
-	ftmp = -MagCal->fmatB[6][j];
+	ftmp = -MagCal->matB[6][j];
 	for (k = X; k <= Z; k++) {
-		ftmp += MagCal->fA[k][k] * MagCal->ftrV[k] * MagCal->ftrV[k];
+		ftmp += MagCal->A[k][k] * MagCal->trV[k] * MagCal->trV[k];
 	}
 
 	// calculate the trial normalized fit error as a percentage
-	MagCal->ftrFitErrorpc = 50.0F *
+	MagCal->trFitErrorpc = 50.0F *
-		sqrtf(fabs(MagCal->fvecA[j]) / (float) MagCal->iMagBufferCount) / fabs(ftmp);
+		sqrtf(fabs(MagCal->vecA[j]) / (float) MagCal->MagBufferCount) / fabs(ftmp);
 
 	// normalize the ellipsoid matrix A to unit determinant
-	f3x3matrixAeqAxScalar(MagCal->fA, powf(det, -(ONETHIRD)));
+	f3x3matrixAeqAxScalar(MagCal->A, powf(det, -(ONETHIRD)));
 
 	// convert the geomagnetic field strength B into uT for normalized
 	// soft iron matrix A and normalize
-	MagCal->ftrB = sqrtf(fabs(ftmp)) * DEFAULTB * powf(det, -(ONESIXTH));
+	MagCal->trB = sqrtf(fabs(ftmp)) * DEFAULTB * powf(det, -(ONESIXTH));
 
 	// compute trial invW from the square root of A also with normalized
 	// determinant and hard iron offset in uT
-	f3x3matrixAeqI(MagCal->ftrinvW);
+	f3x3matrixAeqI(MagCal->trinvW);
 	for (k = X; k <= Z; k++) {
-		MagCal->ftrinvW[k][k] = sqrtf(fabs(MagCal->fA[k][k]));
+		MagCal->trinvW[k][k] = sqrtf(fabs(MagCal->A[k][k]));
-		MagCal->ftrV[k] = MagCal->ftrV[k] * DEFAULTB + (float)iOffset[k] * FXOS8700_UTPERCOUNT;
+		MagCal->trV[k] = MagCal->trV[k] * DEFAULTB + (float)iOffset[k] * FXOS8700_UTPERCOUNT;
 	}
 }
 
@@ -435,14 +435,14 @@ static void fUpdateCalibration10EIG(MagCalibration_t *MagCal)
 	// the offsets are guaranteed to be set from the first element but to avoid compiler error
 	iOffset[X] = iOffset[Y] = iOffset[Z] = 0;
 
-	// zero the on and above diagonal elements of the 10x10 symmetric measurement matrix fmatA
+	// zero the on and above diagonal elements of the 10x10 symmetric measurement matrix matA
 	for (m = 0; m < 10; m++) {
 		for (n = m; n < 10; n++) {
-			MagCal->fmatA[m][n] = 0.0F;
+			MagCal->matA[m][n] = 0.0F;
 		}
 	}
 
-	// sum between MINEQUATIONS to MAXEQUATIONS entries into the 10x10 product matrix fmatA
+	// sum between MINEQUATIONS to MAXEQUATIONS entries into the 10x10 product matrix matA
 	iCount = 0;
 	for (j = 0; j < MAGBUFFSIZE; j++) {
 		if (MagCal->valid[j] != -1) {
@@ -450,34 +450,34 @@ static void fUpdateCalibration10EIG(MagCalibration_t *MagCal)
 			// to help solution (bit counts)
 			if (iCount == 0) {
 				for (k = X; k <= Z; k++) {
-					iOffset[k] = MagCal->iBpFast[k][j];
+					iOffset[k] = MagCal->BpFast[k][j];
 				}
 			}
 
-			// apply the fixed offset and scaling and enter into fvecA[6-8]
+			// apply the fixed offset and scaling and enter into vecA[6-8]
 			for (k = X; k <= Z; k++) {
-				MagCal->fvecA[k + 6] = (float)((int32_t)MagCal->iBpFast[k][j]
+				MagCal->vecA[k + 6] = (float)((int32_t)MagCal->BpFast[k][j]
 					- (int32_t)iOffset[k]) * fscaling;
 			}
 
-			// compute measurement vector elements fvecA[0-5] from fvecA[6-8]
+			// compute measurement vector elements vecA[0-5] from vecA[6-8]
-			MagCal->fvecA[0] = MagCal->fvecA[6] * MagCal->fvecA[6];
+			MagCal->vecA[0] = MagCal->vecA[6] * MagCal->vecA[6];
-			MagCal->fvecA[1] = 2.0F * MagCal->fvecA[6] * MagCal->fvecA[7];
+			MagCal->vecA[1] = 2.0F * MagCal->vecA[6] * MagCal->vecA[7];
-			MagCal->fvecA[2] = 2.0F * MagCal->fvecA[6] * MagCal->fvecA[8];
+			MagCal->vecA[2] = 2.0F * MagCal->vecA[6] * MagCal->vecA[8];
-			MagCal->fvecA[3] = MagCal->fvecA[7] * MagCal->fvecA[7];
+			MagCal->vecA[3] = MagCal->vecA[7] * MagCal->vecA[7];
-			MagCal->fvecA[4] = 2.0F * MagCal->fvecA[7] * MagCal->fvecA[8];
+			MagCal->vecA[4] = 2.0F * MagCal->vecA[7] * MagCal->vecA[8];
-			MagCal->fvecA[5] = MagCal->fvecA[8] * MagCal->fvecA[8];
+			MagCal->vecA[5] = MagCal->vecA[8] * MagCal->vecA[8];
 
-			// accumulate the on-and above-diagonal terms of fmatA=Sigma{fvecA^T * fvecA}
+			// accumulate the on-and above-diagonal terms of matA=Sigma{vecA^T * vecA}
-			// with the exception of fmatA[9][9] which equals the number of measurements
+			// with the exception of matA[9][9] which equals the number of measurements
-			// update the right hand column [9] of fmatA[0-8][9] ignoring fmatA[9][9]
+			// update the right hand column [9] of matA[0-8][9] ignoring matA[9][9]
 			for (m = 0; m < 9; m++) {
-				MagCal->fmatA[m][9] += MagCal->fvecA[m];
+				MagCal->matA[m][9] += MagCal->vecA[m];
 			}
-			// update the on and above diagonal terms of fmatA ignoring right hand column 9
+			// update the on and above diagonal terms of matA ignoring right hand column 9
 			for (m = 0; m < 9; m++) {
 				for (n = m; n < 9; n++) {
-					MagCal->fmatA[m][n] += MagCal->fvecA[m] * MagCal->fvecA[n];
+					MagCal->matA[m][n] += MagCal->vecA[m] * MagCal->vecA[n];
 				}
 			}
 
@@ -486,121 +486,121 @@ static void fUpdateCalibration10EIG(MagCalibration_t *MagCal)
 		}
 	}
 
-	// set the last element fmatA[9][9] to the number of measurements
+	// set the last element matA[9][9] to the number of measurements
-	MagCal->fmatA[9][9] = (float) iCount;
+	MagCal->matA[9][9] = (float) iCount;
 
 	// store the number of measurements accumulated
-	MagCal->iMagBufferCount = iCount;
+	MagCal->MagBufferCount = iCount;
 
-	// copy the above diagonal elements of symmetric product matrix fmatA to below the diagonal
+	// copy the above diagonal elements of symmetric product matrix matA to below the diagonal
 	for (m = 1; m < 10; m++) {
 		for (n = 0; n < m; n++) {
-			MagCal->fmatA[m][n] = MagCal->fmatA[n][m];
+			MagCal->matA[m][n] = MagCal->matA[n][m];
 		}
 	}
 
-	// set MagCal->fvecA to the unsorted eigenvalues and fmatB to the unsorted
+	// set MagCal->vecA to the unsorted eigenvalues and matB to the unsorted
-	// normalized eigenvectors of fmatA
+	// normalized eigenvectors of matA
-	eigencompute(MagCal->fmatA, MagCal->fvecA, MagCal->fmatB, 10);
+	eigencompute(MagCal->matA, MagCal->vecA, MagCal->matB, 10);
 
 	// set ellipsoid matrix A from elements of the solution vector column j with
 	// smallest eigenvalue
 	j = 0;
 	for (i = 1; i < 10; i++) {
-		if (MagCal->fvecA[i] < MagCal->fvecA[j]) {
+		if (MagCal->vecA[i] < MagCal->vecA[j]) {
 			j = i;
 		}
 	}
-	MagCal->fA[0][0] = MagCal->fmatB[0][j];
+	MagCal->A[0][0] = MagCal->matB[0][j];
-	MagCal->fA[0][1] = MagCal->fA[1][0] = MagCal->fmatB[1][j];
+	MagCal->A[0][1] = MagCal->A[1][0] = MagCal->matB[1][j];
-	MagCal->fA[0][2] = MagCal->fA[2][0] = MagCal->fmatB[2][j];
+	MagCal->A[0][2] = MagCal->A[2][0] = MagCal->matB[2][j];
-	MagCal->fA[1][1] = MagCal->fmatB[3][j];
+	MagCal->A[1][1] = MagCal->matB[3][j];
-	MagCal->fA[1][2] = MagCal->fA[2][1] = MagCal->fmatB[4][j];
+	MagCal->A[1][2] = MagCal->A[2][1] = MagCal->matB[4][j];
-	MagCal->fA[2][2] = MagCal->fmatB[5][j];
+	MagCal->A[2][2] = MagCal->matB[5][j];
 
 	// negate entire solution if A has negative determinant
-	det = f3x3matrixDetA(MagCal->fA);
+	det = f3x3matrixDetA(MagCal->A);
 	if (det < 0.0F) {
-		f3x3matrixAeqMinusA(MagCal->fA);
+		f3x3matrixAeqMinusA(MagCal->A);
-		MagCal->fmatB[6][j] = -MagCal->fmatB[6][j];
+		MagCal->matB[6][j] = -MagCal->matB[6][j];
-		MagCal->fmatB[7][j] = -MagCal->fmatB[7][j];
+		MagCal->matB[7][j] = -MagCal->matB[7][j];
-		MagCal->fmatB[8][j] = -MagCal->fmatB[8][j];
+		MagCal->matB[8][j] = -MagCal->matB[8][j];
-		MagCal->fmatB[9][j] = -MagCal->fmatB[9][j];
+		MagCal->matB[9][j] = -MagCal->matB[9][j];
 		det = -det;
 	}
 
 	// compute the inverse of the ellipsoid matrix
-	f3x3matrixAeqInvSymB(MagCal->finvA, MagCal->fA);
+	f3x3matrixAeqInvSymB(MagCal->invA, MagCal->A);
 
 	// compute the trial hard iron vector in offset bit counts times FMATRIXSCALING
 	for (k = X; k <= Z; k++) {
-		MagCal->ftrV[k] = 0.0F;
+		MagCal->trV[k] = 0.0F;
 		for (m = X; m <= Z; m++) {
-			MagCal->ftrV[k] += MagCal->finvA[k][m] * MagCal->fmatB[m + 6][j];
+			MagCal->trV[k] += MagCal->invA[k][m] * MagCal->matB[m + 6][j];
 		}
-		MagCal->ftrV[k] *= -0.5F;
+		MagCal->trV[k] *= -0.5F;
 	}
 
 	// compute the trial geomagnetic field strength B in bit counts times FMATRIXSCALING
-	MagCal->ftrB = sqrtf(fabs(MagCal->fA[0][0] * MagCal->ftrV[X] * MagCal->ftrV[X] +
+	MagCal->trB = sqrtf(fabs(MagCal->A[0][0] * MagCal->trV[X] * MagCal->trV[X] +
-			2.0F * MagCal->fA[0][1] * MagCal->ftrV[X] * MagCal->ftrV[Y] +
+			2.0F * MagCal->A[0][1] * MagCal->trV[X] * MagCal->trV[Y] +
-			2.0F * MagCal->fA[0][2] * MagCal->ftrV[X] * MagCal->ftrV[Z] +
+			2.0F * MagCal->A[0][2] * MagCal->trV[X] * MagCal->trV[Z] +
-			MagCal->fA[1][1] * MagCal->ftrV[Y] * MagCal->ftrV[Y] +
+			MagCal->A[1][1] * MagCal->trV[Y] * MagCal->trV[Y] +
-			2.0F * MagCal->fA[1][2] * MagCal->ftrV[Y] * MagCal->ftrV[Z] +
+			2.0F * MagCal->A[1][2] * MagCal->trV[Y] * MagCal->trV[Z] +
-			MagCal->fA[2][2] * MagCal->ftrV[Z] * MagCal->ftrV[Z] - MagCal->fmatB[9][j]));
+			MagCal->A[2][2] * MagCal->trV[Z] * MagCal->trV[Z] - MagCal->matB[9][j]));
 
 	// calculate the trial normalized fit error as a percentage
-	MagCal->ftrFitErrorpc = 50.0F * sqrtf(
+	MagCal->trFitErrorpc = 50.0F * sqrtf(
-		fabs(MagCal->fvecA[j]) / (float) MagCal->iMagBufferCount) /
+		fabs(MagCal->vecA[j]) / (float) MagCal->MagBufferCount) /
-		(MagCal->ftrB * MagCal->ftrB);
+		(MagCal->trB * MagCal->trB);
 
 	// correct for the measurement matrix offset and scaling and
 	// get the computed hard iron offset in uT
 	for (k = X; k <= Z; k++) {
-		MagCal->ftrV[k] = MagCal->ftrV[k] * DEFAULTB + (float)iOffset[k] * FXOS8700_UTPERCOUNT;
+		MagCal->trV[k] = MagCal->trV[k] * DEFAULTB + (float)iOffset[k] * FXOS8700_UTPERCOUNT;
 	}
 
 	// convert the trial geomagnetic field strength B into uT for
 	// un-normalized soft iron matrix A
-	MagCal->ftrB *= DEFAULTB;
+	MagCal->trB *= DEFAULTB;
 
 	// normalize the ellipsoid matrix A to unit determinant and
 	// correct B by root of this multiplicative factor
-	f3x3matrixAeqAxScalar(MagCal->fA, powf(det, -(ONETHIRD)));
+	f3x3matrixAeqAxScalar(MagCal->A, powf(det, -(ONETHIRD)));
-	MagCal->ftrB *= powf(det, -(ONESIXTH));
+	MagCal->trB *= powf(det, -(ONESIXTH));
 
-	// compute trial invW from the square root of fA (both with normalized determinant)	
+	// compute trial invW from the square root of fA (both with normalized determinant)
-	// set fvecA to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA
+	// set vecA to the unsorted eigenvalues and matB to the unsorted eigenvectors of matA
-	// where fmatA holds the 3x3 matrix fA in its top left elements
+	// where matA holds the 3x3 matrix fA in its top left elements
 	for (i = 0; i < 3; i++) {
 		for (j = 0; j < 3; j++) {
-			MagCal->fmatA[i][j] = MagCal->fA[i][j];
+			MagCal->matA[i][j] = MagCal->A[i][j];
 		}
 	}
-	eigencompute(MagCal->fmatA, MagCal->fvecA, MagCal->fmatB, 3);
+	eigencompute(MagCal->matA, MagCal->vecA, MagCal->matB, 3);
 
-	// set MagCal->fmatB to be eigenvectors . diag(sqrt(sqrt(eigenvalues))) =
+	// set MagCal->matB to be eigenvectors . diag(sqrt(sqrt(eigenvalues))) =
-	//   fmatB . diag(sqrt(sqrt(fvecA))
+	//   matB . diag(sqrt(sqrt(vecA))
 	for (j = 0; j < 3; j++) { // loop over columns j
-		ftmp = sqrtf(sqrtf(fabs(MagCal->fvecA[j])));
+		ftmp = sqrtf(sqrtf(fabs(MagCal->vecA[j])));
 		for (i = 0; i < 3; i++) { // loop over rows i
-			MagCal->fmatB[i][j] *= ftmp;
+			MagCal->matB[i][j] *= ftmp;
 		}
 	}
 
-	// set ftrinvW to eigenvectors * diag(sqrt(eigenvalues)) * eigenvectors^T =
+	// set trinvW to eigenvectors * diag(sqrt(eigenvalues)) * eigenvectors^T =
-	//   fmatB * fmatB^T = sqrt(fA) (guaranteed symmetric)
+	//   matB * matB^T = sqrt(fA) (guaranteed symmetric)
 	// loop over rows
 	for (i = 0; i < 3; i++) {
 		// loop over on and above diagonal columns
 		for (j = i; j < 3; j++) {
-			MagCal->ftrinvW[i][j] = 0.0F;
+			MagCal->trinvW[i][j] = 0.0F;
 			// accumulate the matrix product
 			for (k = 0; k < 3; k++) {
-				MagCal->ftrinvW[i][j] += MagCal->fmatB[i][k] * MagCal->fmatB[j][k];
+				MagCal->trinvW[i][j] += MagCal->matB[i][k] * MagCal->matB[j][k];
 			}
 			// copy to below diagonal element
-			MagCal->ftrinvW[j][i] = MagCal->ftrinvW[i][j];
+			MagCal->trinvW[j][i] = MagCal->trinvW[i][j];
 		}
 	}
 }

matrix.c

@@ -1,7 +1,7 @@
 // Copyright (c) 2014, Freescale Semiconductor, Inc.
 // All rights reserved.
 // vim: set ts=4:
-// 
+//
 // 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
@@ -12,7 +12,7 @@
 //     * Neither the name of Freescale Semiconductor, Inc. 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

rawdata.c

@@ -10,14 +10,14 @@ SV_9DOF_GBY_KALMAN_t fusionstate;
 void raw_data_reset(void)
 {
 	rawcount = OVERSAMPLE_RATIO;
-	fInit_9DOF_GBY_KALMAN(&fusionstate, 100, OVERSAMPLE_RATIO);
+	fInit_9DOF_GBY_KALMAN(&fusionstate);
 	memset(&magcal, 0, sizeof(magcal));
-	magcal.fV[2] = 80.0f;  // initial guess
+	magcal.V[2] = 80.0f;  // initial guess
-	magcal.finvW[0][0] = 1.0f;
+	magcal.invW[0][0] = 1.0f;
-	magcal.finvW[1][1] = 1.0f;
+	magcal.invW[1][1] = 1.0f;
-	magcal.finvW[2][2] = 1.0f;
+	magcal.invW[2][2] = 1.0f;
-	magcal.fFitErrorpc = 100.0f;
+	magcal.FitError = 100.0f;
-	magcal.fFitErrorAge = 100.0f;
+	magcal.FitErrorAge = 100.0f;
 }
 
 static void add_magcal_data(const int16_t *data)
@@ -36,9 +36,9 @@ static void add_magcal_data(const int16_t *data)
 	if (i >= MAGBUFFSIZE) {
 		for (i=0; i < MAGBUFFSIZE; i++) {
 			for (j=i+1; j < MAGBUFFSIZE; j++) {
-				dx = magcal.iBpFast[0][i] - magcal.iBpFast[0][j];
+				dx = magcal.BpFast[0][i] - magcal.BpFast[0][j];
-				dy = magcal.iBpFast[1][i] - magcal.iBpFast[1][j];
+				dy = magcal.BpFast[1][i] - magcal.BpFast[1][j];
-				dz = magcal.iBpFast[2][i] - magcal.iBpFast[2][j];
+				dz = magcal.BpFast[2][i] - magcal.BpFast[2][j];
 				distsq = (int64_t)dx * (int64_t)dx;
 				distsq += (int64_t)dy * (int64_t)dy;
 				distsq += (int64_t)dz * (int64_t)dz;
@@ -51,9 +51,9 @@ static void add_magcal_data(const int16_t *data)
 		i = minindex;
 	}
 	// add it to the cal buffer
-	magcal.iBpFast[0][i] = data[6];
+	magcal.BpFast[0][i] = data[6];
-	magcal.iBpFast[1][i] = data[7];
+	magcal.BpFast[1][i] = data[7];
-	magcal.iBpFast[2][i] = data[8];
+	magcal.BpFast[2][i] = data[8];
 	magcal.valid[i] = 1;
 }
 
@@ -75,47 +75,46 @@ void raw_data(const int16_t *data)
 	x = (float)data[0] * G_PER_COUNT;
 	y = (float)data[1] * G_PER_COUNT;
 	z = (float)data[2] * G_PER_COUNT;
-	accel.fGpFast[0] = x;
+	accel.GpFast[0] = x;
-	accel.fGpFast[1] = y;
+	accel.GpFast[1] = y;
-	accel.fGpFast[2] = y;
+	accel.GpFast[2] = y;
-	accel.fGp[0] += x;
+	accel.Gp[0] += x;
-	accel.fGp[1] += y;
+	accel.Gp[1] += y;
-	accel.fGp[2] += y;
+	accel.Gp[2] += y;
 
 	x = (float)data[3] * DEG_PER_SEC_PER_COUNT;
 	y = (float)data[4] * DEG_PER_SEC_PER_COUNT;
 	z = (float)data[5] * DEG_PER_SEC_PER_COUNT;
-	gyro.fYp[0] += x;
+	gyro.Yp[0] += x;
-	gyro.fYp[1] += y;
+	gyro.Yp[1] += y;
-	gyro.fYp[2] += z;
+	gyro.Yp[2] += z;
-	gyro.iYpFast[rawcount][0] = data[3];
+	gyro.YpFast[rawcount][0] = data[3];
-	gyro.iYpFast[rawcount][1] = data[4];
+	gyro.YpFast[rawcount][1] = data[4];
-	gyro.iYpFast[rawcount][2] = data[5];
+	gyro.YpFast[rawcount][2] = data[5];
 
 	apply_calibration(data[6], data[7], data[8], &point);
-	mag.fBcFast[0] = point.x;
+	mag.BcFast[0] = point.x;
-	mag.fBcFast[1] = point.y;
+	mag.BcFast[1] = point.y;
-	mag.fBcFast[2] = point.z;
+	mag.BcFast[2] = point.z;
-	mag.fBc[0] += point.x;
+	mag.Bc[0] += point.x;
-	mag.fBc[1] += point.y;
+	mag.Bc[1] += point.y;
-	mag.fBc[2] += point.z;
+	mag.Bc[2] += point.z;
 
 	rawcount++;
 	if (rawcount >= OVERSAMPLE_RATIO) {
 		ratio = 1.0f / (float)OVERSAMPLE_RATIO;
-		accel.fGp[0] *= ratio;
+		accel.Gp[0] *= ratio;
-		accel.fGp[1] *= ratio;
+		accel.Gp[1] *= ratio;
-		accel.fGp[2] *= ratio;
+		accel.Gp[2] *= ratio;
-		gyro.fYp[0] *= ratio;
+		gyro.Yp[0] *= ratio;
-		gyro.fYp[1] *= ratio;
+		gyro.Yp[1] *= ratio;
-		gyro.fYp[2] *= ratio;
+		gyro.Yp[2] *= ratio;
-		mag.fBc[0] *= ratio;
+		mag.Bc[0] *= ratio;
-		mag.fBc[1] *= ratio;
+		mag.Bc[1] *= ratio;
-		mag.fBc[2] *= ratio;
+		mag.Bc[2] *= ratio;
-		fRun_9DOF_GBY_KALMAN(&fusionstate, &accel, &mag, &gyro,
+		fRun_9DOF_GBY_KALMAN(&fusionstate, &accel, &mag, &gyro, &magcal);
-			&magcal, OVERSAMPLE_RATIO);
 
-		memcpy(&current_orientation, &(fusionstate.fqPl), sizeof(Quaternion_t));
+		memcpy(&current_orientation, &(fusionstate.qPl), sizeof(Quaternion_t));
 	}
 }
 
@@ -159,11 +158,11 @@ int send_calibration(void)
 	*p++ = 117; // 2 byte signature
 	*p++ = 84;
 	for (i=0; i < 3; i++) {
-		p = copy_lsb_first(p, magcal.fV[i]); // 12 bytes offset/hardiron
+		p = copy_lsb_first(p, magcal.V[i]); // 12 bytes offset/hardiron
 	}
 	for (i=0; i < 3; i++) {
 		for (j=0; j < 3; j++) {
-			p = copy_lsb_first(p, magcal.finvW[i][j]); // 36 bytes softiron
+			p = copy_lsb_first(p, magcal.invW[i][j]); // 36 bytes softiron
 		}
 	}
 	crc = 0xFFFF;

serialdata.c

@@ -50,30 +50,30 @@ static int packet_magnetic_cal(const unsigned char *data)
 	z = (data[13] << 8) | data[12];
 
 	if (id == 1) {
-		magcal.fV[0] = (float)x * 0.1f;
+		magcal.V[0] = (float)x * 0.1f;
-		magcal.fV[1] = (float)y * 0.1f;
+		magcal.V[1] = (float)y * 0.1f;
-		magcal.fV[2] = (float)z * 0.1f;
+		magcal.V[2] = (float)z * 0.1f;
 		return 1;
 	} else if (id == 2) {
-		magcal.finvW[0][0] = (float)x * 0.001f;
+		magcal.invW[0][0] = (float)x * 0.001f;
-		magcal.finvW[1][1] = (float)y * 0.001f;
+		magcal.invW[1][1] = (float)y * 0.001f;
-		magcal.finvW[2][2] = (float)z * 0.001f;
+		magcal.invW[2][2] = (float)z * 0.001f;
 		return 1;
 	} else if (id == 3) {
-		magcal.finvW[0][1] = (float)x / 1000.0f;
+		magcal.invW[0][1] = (float)x / 1000.0f;
-		magcal.finvW[1][0] = (float)x / 1000.0f; // TODO: check this assignment
+		magcal.invW[1][0] = (float)x / 1000.0f; // TODO: check this assignment
-		magcal.finvW[0][2] = (float)y / 1000.0f;
+		magcal.invW[0][2] = (float)y / 1000.0f;
-		magcal.finvW[1][2] = (float)y / 1000.0f; // TODO: check this assignment
+		magcal.invW[1][2] = (float)y / 1000.0f; // TODO: check this assignment
-		magcal.finvW[1][2] = (float)z / 1000.0f;
+		magcal.invW[1][2] = (float)z / 1000.0f;
-		magcal.finvW[2][1] = (float)z / 1000.0f; // TODO: check this assignment
+		magcal.invW[2][1] = (float)z / 1000.0f; // TODO: check this assignment
 		return 1;
 	} else if (id >= 10 && id < MAGBUFFSIZE+10) {
 		n = id - 10;
-		if (magcal.valid[n] == 0 || x != magcal.iBpFast[0][n]
+		if (magcal.valid[n] == 0 || x != magcal.BpFast[0][n]
-		  || y != magcal.iBpFast[1][n] || z != magcal.iBpFast[2][n]) {
+		  || y != magcal.BpFast[1][n] || z != magcal.BpFast[2][n]) {
-			magcal.iBpFast[0][n] = x;
+			magcal.BpFast[0][n] = x;
-			magcal.iBpFast[1][n] = y;
+			magcal.BpFast[1][n] = y;
-			magcal.iBpFast[2][n] = z;
+			magcal.BpFast[2][n] = z;
 			magcal.valid[n] = 1;
 			printf("mag cal, n=%3d: %5d %5d %5d\n", n, x, y, z);
 		}

visualize.c

@@ -50,12 +50,12 @@ void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out)
 {
 	float x, y, z;
 
-	x = ((float)rawx * UT_PER_COUNT) - magcal.fV[0];
+	x = ((float)rawx * UT_PER_COUNT) - magcal.V[0];
-	y = ((float)rawy * UT_PER_COUNT) - magcal.fV[1];
+	y = ((float)rawy * UT_PER_COUNT) - magcal.V[1];
-	z = ((float)rawz * UT_PER_COUNT) - magcal.fV[2];
+	z = ((float)rawz * UT_PER_COUNT) - magcal.V[2];
-	out->x = x * magcal.finvW[0][0] + y * magcal.finvW[0][1] + z * magcal.finvW[0][2];
+	out->x = x * magcal.invW[0][0] + y * magcal.invW[0][1] + z * magcal.invW[0][2];
-	out->y = x * magcal.finvW[1][0] + y * magcal.finvW[1][1] + z * magcal.finvW[1][2];
+	out->y = x * magcal.invW[1][0] + y * magcal.invW[1][1] + z * magcal.invW[1][2];
-	out->z = x * magcal.finvW[2][0] + y * magcal.finvW[2][1] + z * magcal.finvW[2][2];
+	out->z = x * magcal.invW[2][0] + y * magcal.invW[2][1] + z * magcal.invW[2][2];
 }
 
 static void quad_to_rotation(const Quaternion_t *quat, float *rmatrix)
@@ -119,8 +119,8 @@ void display_callback(void)
 			//if (caldata[i].valid) {
 			if (magcal.valid[i]) {
 				//apply_calibration(&caldata[i], &point);
-				apply_calibration(magcal.iBpFast[0][i], magcal.iBpFast[1][i],
+				apply_calibration(magcal.BpFast[0][i], magcal.BpFast[1][i],
-					magcal.iBpFast[2][i], &point);
+					magcal.BpFast[2][i], &point);
 				sumx += point.x;
 				sumy += point.y;
 				sumz += point.z;