int main(int argc, char *argv[])
{
(void)argc; (void)argv;
#ifdef __SSE__
printf("SSE ");
#endif
#ifdef __SSE2__
printf("SSE2 ");
#endif
#ifdef __SSE3__
printf("SSE3 ");
#endif
#ifdef __SSE4__
printf("SSE4 ");
#endif
#ifdef __SSE4_1__
printf("SSE4.1 ");
#endif
#ifdef __SSE4_2__
printf("SSE4.2 ");
#endif
#ifdef __AVX__
printf("AVX ");
#endif
#ifdef __FMA4__
printf("FMA4 ");
#endif
printf("\n");
printv(vec(1, 2, 3, 4));
printv(vzero());
printm(mzero());
printm(midentity());
vec4 a = { 1, 2, 3, 4 }, b = { 5, 6, 7, 8 };
printv(a);
printv(b);
printf("\nshuffles:\n");
printv(vshuffle(a, a, 0, 1, 2, 3));
printv(vshuffle(a, a, 3, 2, 1, 0));
printv(vshuffle(a, b, 0, 1, 0, 1));
printv(vshuffle(a, b, 2, 3, 2, 3));
printf("\ndot products:\n");
printv(vdot(a, b));
printv(vdot(b, a));
printv(vdot3(a, b));
printv(vdot3(b, a));
//vec4 blendmask = { 1, -1, 1, -1 };
//printv(vblend(x, y, blendmask));
vec4 x = { 1, 0, 0, 0 }, y = { 0, 1, 0, 0 }, z = { 0, 0, 1, 0 }, w = { 0, 0, 0, 1 };
printf("\ncross products:\n");
printv(vcross(x, y));
printv(vcross(y, x));
printv(vcross_scalar(x, y));
printv(vcross_scalar(y, x));
printf("\nquaternion products:\n");
printv(qprod(x, y));
printv(qprod(y, x));
printv(qprod_mad(x, y));
printv(qprod_mad(y, x));
printv(qprod_scalar(x, y));
printv(qprod_scalar(y, x));
printf("\nquaternion conjugates:\n");
printv(qconj(x));
printv(qconj(y));
printv(qconj(z));
printv(qconj(w));
printf("\nmat from quat:\n");
printm(quat_to_mat(w));
printm(quat_to_mat_mmul(w));
printm(quat_to_mat_scalar(w));
vec4 angles = { 0.0, 0.0, 0.0, 0.0 };
printf("\neuler to quaternion:\n");
printv(quat_euler(angles));
printv(quat_euler_scalar(angles));
printv(quat_euler_gems(angles));
printf("\neuler to matrix:\n");
printm(mat_euler(angles));
printm(mat_euler_scalar(angles));
printm(quat_to_mat(quat_euler(angles)));
printf("\nperspective matrix:\n");
printm(mat_perspective_fovy(M_PI/4.0, 16.0/9.0, 0.1, 100.0));
printm(mat_perspective_fovy_inf_z(M_PI/4.0, 16.0/9.0, 0.1));
printm(mat_perspective_fovy_scalar(M_PI/4.0, 16.0/9.0, 0.1, 100.0));
printf("\northogonal matrix:\n");
printm(mat_ortho(-1.0, 1.0, -1.0, 1.0, -1.0, 1.0));
printm(mat_ortho(-1.0, 2.0, -1.0, 2.0, -1.0, 2.0));
printf("\ntranslate matrix:\n");
printm(mtranslate(a));
printf("\nscale matrix:\n");
printm(mscale(a));
return 0;
}
void AHRS_AccMagCorrect(void)
{
// Quit if the biases are still being calculated
if( SensorCal.biasCountGyro < SensorCal.biasTotalGyro)
return;
// $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
// Read raw accel values from A2D registers, A2D automatically scans inputs at 5KHz
// $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
SensorData.accX = xaccel; SensorData.accX -= SensorCal.accelRawBias;
SensorData.accY = yaccel; SensorData.accY -= SensorCal.accelRawBias;
SensorData.accZ = zaccel; SensorData.accZ -= SensorCal.accelRawBias;
// $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
// Accel scaling, to g
// $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
int16toQ16(&AHRSdata.ax, &SensorData.accX);
AHRSdata.ax = -mult( AHRSdata.ax, SensorCal.accelScale);
int16toQ16(&AHRSdata.ay, &SensorData.accY);
AHRSdata.ay = mult( AHRSdata.ay, SensorCal.accelScale );
int16toQ16(&AHRSdata.az, &SensorData.accZ);
AHRSdata.az = -mult( AHRSdata.az, SensorCal.accelScale );
// $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
// Initial acc bias calculation
// $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
if( SensorCal.biasCountAcc < SensorCal.biasTotalAcc){
// Do some blank reads to clear any garbage in the initial transient
if(--SensorCal.blankReadsAcc > 0)
return;
SensorCal.axBias += AHRSdata.ax;
SensorCal.ayBias += AHRSdata.ay;
SensorCal.azBias += AHRSdata.az;
led_on(LED_RED);
if( ++SensorCal.biasCountAcc == SensorCal.biasTotalAcc ){
_Q16 tmp = _Q16ftoi(1.0 / ((float)SensorCal.biasTotalAcc ));
SensorCal.axBias = mult( SensorCal.axBias, tmp);
SensorCal.ayBias = mult( SensorCal.ayBias, tmp);
SensorCal.azBias = mult( SensorCal.azBias, tmp);
led_off(LED_RED);
led_off(LED_GREEN);
}
return;
}
// $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
// Acc bias correction
// $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
AHRSdata.ax = AHRSdata.ax - SensorCal.axBias;
AHRSdata.ay = AHRSdata.ay - SensorCal.ayBias;
AHRSdata.az = AHRSdata.az - SensorCal.azBias;
_Q16 ax = AHRSdata.ax;
_Q16 ay = AHRSdata.ay;
_Q16 az = AHRSdata.az;
// $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
// Maneuver detector, do not use accels during fast movement
// $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
// TODO: Check angular rates
// Roll and pitch calculation, assumes accelerometer units are 10000*g
// Normalize the acceleration vector to length 1
_Q16 root = _Q16sqrt( mult(ax,ax) + mult(ay,ay) + mult(az,az));
// TODO: Make sure we're around 1g
if( root < SensorCal.acc_window_min || root > SensorCal.acc_window_max){
return;
}
// Normalize
ax = _IQ16div(ax, root);
ay = _IQ16div(ay, root);
az = _IQ16div(az, root);
// Too close to singularity (due to numerical precision limits)
if( ax > num0p998 || -ax > num0p998 )
return;
root = _Q16sqrt( mult(ay,ay) + mult(az,az));
if(root < num0p0001 )
root = num0p0001;
// Calculate sin/cos of roll and pitch
_Q16 sinR = - _IQ16div(ay,root);
_Q16 cosR = - _IQ16div(az,root);
_Q16 sinP = ax;
_Q16 cosP = -( mult(ay,sinR) + mult(az,cosR) );
// Calculate half-angles
_Q16 cosR2 = _Q16sqrt( mult( num1p0 + cosR , num0p5 ));
if(cosR2 < num0p0001 )
cosR2 = num0p0001;
_Q16 sinR2 = mult(_IQ16div( sinR , cosR2) , num0p5 ); // WARNING: This step is numerically ill-behaved!
_Q16 cosP2 = _Q16sqrt( mult( num1p0 + cosP , num0p5 ));
if(cosP2 < num0p0001 )
cosP2 = num0p0001;
_Q16 sinP2 = mult(_IQ16div( sinP , cosP2) , num0p5 ); // WARNING: This step is numerically ill-behaved!
// Too close to singularity (due to numerical precision limits)
if( mult(cosR2,cosR2) + mult(sinR2,sinR2) > num1p1 || mult(cosP2,cosP2) + mult(sinP2,sinP2) > num1p1 )
return;
// Yaw calculation
// Normalize the magnetometer vector to length 1
/* magx = (float)AHRSdata.magY;
magy = -(float)AHRSdata.magX;
magz = (float)AHRSdata.magZ;
// Todo: magx*magx can be done in fixed pt
root = sqrt( magx*magx + magy*magy + magz*magz );
magx /= root;
magy /= root;
magz /= root;
yaw = atan2(-cosR*magy - sinR*magz , cosP*magx+sinP*sinR*magy-sinP*cosR*magz);
yaw += PI;
if(yaw > PI){
yaw -= 2*PI;
}
sinY2 = sin(yaw/2.0);
cosY2 = cos(yaw/2.0);
*/
_Q16 cosY2 = _Q16ftoi(1.0);
_Q16 sinY2 = 0;
// Finally get quaternion
tQuaternion qroll,qpitch,qyaw;
qyaw.o = cosY2; qyaw.x = 0; qyaw.y = 0; qyaw.z = sinY2;
qpitch.o = cosP2; qpitch.x = 0; qpitch.y = sinP2; qpitch.z = 0;
qroll.o = cosR2; qroll.x = sinR2; qroll.y = 0; qroll.z = 0;
AHRSdata.q_meas = qprod(qyaw,qpitch);
AHRSdata.q_meas = qprod(AHRSdata.q_meas, qroll);
// Check if flipped from last measurement
if( mult(AHRSdata.q_meas.x,AHRSdata.q_est.x) + mult(AHRSdata.q_meas.y,AHRSdata.q_est.y) + mult(AHRSdata.q_meas.z,AHRSdata.q_est.z) + mult(AHRSdata.q_meas.o,AHRSdata.q_est.o) < 0 )
{
AHRSdata.q_meas.o = - AHRSdata.q_meas.o;
AHRSdata.q_meas.x = - AHRSdata.q_meas.x;
AHRSdata.q_meas.y = - AHRSdata.q_meas.y;
AHRSdata.q_meas.z = - AHRSdata.q_meas.z;
}
// Gyro bias estimation
// Make the correction
AHRSdata.q_est.o -= mult(AHRSdata.q_est.o-AHRSdata.q_meas.o, SensorCal.K_AttFilter);
AHRSdata.q_est.x -= mult(AHRSdata.q_est.x-AHRSdata.q_meas.x, SensorCal.K_AttFilter);
AHRSdata.q_est.y -= mult(AHRSdata.q_est.y-AHRSdata.q_meas.y, SensorCal.K_AttFilter);
AHRSdata.q_est.z -= mult(AHRSdata.q_est.z-AHRSdata.q_meas.z, SensorCal.K_AttFilter);
}
