void attEstimatePose(void) {
// Fixed point implementation
_Q16 dphi, dtheta, dpsi;
float rate[3];
_Q16 wx, wy, wz;
_Q16 sin_phi, tan_theta, cos_phi, cos_theta, temp;
gyroGetRadXYZ(rate); // 50 us
wx = _Q16ftoi(rate[0]);
wy = _Q16ftoi(rate[1]);
wz = _Q16ftoi(rate[2]);
sin_phi = _Q16sin(PoseQ.qdata.phi);
cos_phi = _Q16cos(PoseQ.qdata.phi);
cos_theta = _Q16cos(PoseQ.qdata.theta);
tan_theta = _Q16tan(PoseQ.qdata.theta);
temp = _Q16mult(wy, sin_phi) + _Q16mult(wz, cos_phi);
dphi = _Q16mult(temp, tan_theta) + wx;
dtheta = _Q16mult(wy, cos_phi) + _Q16neg(_Q16mult(wz, sin_phi));
dpsi = _IQ16div(temp, cos_theta);
PoseQ.qdata.phi += _Q16mult(dphi, samplePeriod);
PoseQ.qdata.theta += _Q16mult(dtheta, samplePeriod);
PoseQ.qdata.psi += _Q16mult(dpsi, samplePeriod);
}
// 12000 cycles?
void attEstimatePose(void) {
Quaternion displacement_quat;
float rate[3], norm, sina_2;
bams32_t a_2;
if(!is_ready) { return; }
if(!is_running) { return; }
gyroGetRadXYZ(rate); // Get last read gyro values
timestamp = swatchToc(); // Record timestamp
// Calculate magnitude and disiplacement
norm = sqrtf(rate[0]*rate[0] + rate[1]*rate[1] + rate[2]*rate[2]);
// Special case when no movement occurs due to simplification below
if(norm == 0.0) {
displacement_quat.w = 1.0;
displacement_quat.x = 0.0;
displacement_quat.y = 0.0;
displacement_quat.z = 0.0;
} else {
// Generate displacement rotation quaternion
// Normally this is w = cos(a/2), but we can delay normalizing
// by multiplying all terms by norm
a_2 = floatToBams32Rad(norm*sample_period)/2;
sina_2 = bams32SinFine(a_2);
displacement_quat.w = bams32CosFine(a_2)*norm;
displacement_quat.x = sina_2*rate[0];
displacement_quat.y = sina_2*rate[1];
displacement_quat.z = sina_2*rate[2];
quatNormalize(&displacement_quat);
}
// Apply displacement to pose
quatMult(&pose_quat, &displacement_quat, &pose_quat);
// Normalize pose quaternion to account for unnormalized displacement quaternion
quatNormalize(&pose_quat);
calculateEulerAngles();
}
