void generateDisplacement(Rate rate, Quaternion *q) {
float norm, sina_2;
bams32_t a_2;
norm = sqrtf( rate->yaw_rate*rate->yaw_rate +
rate->pitch_rate*rate->pitch_rate +
rate->roll_rate*rate->roll_rate );
if(norm == 0.0) {
q->w = 1.0;
q->x = 0.0;
q->y = 0.0;
q->z = 0.0;
} else {
a_2 = floatToBams32Rad(norm*period)/2;
sina_2 = bams32SinFine(a_2);
q->w = bams32CosFine(a_2)*norm;
q->x = sina_2*rate->roll_rate;
q->y = sina_2*rate->pitch_rate;
q->z = sina_2*rate->yaw_rate;
}
quatNormalize(q);
}
// 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();
}
void slewSetLimit(float rate) {
max_slew_rate = rate;
max_angular_displacement = floatToBams32Rad(rate*period);
}