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();
}
// qtarget = qcurr*qdisp
// qcurr'*qtarget = qdisp
void slewProcess(Quaternion *input, Quaternion *output) {
Quaternion conjugate, displacement;
bams32_t input_angle, limited_angle;
float sin_input, sin_limited, scale;
if(!is_ready || !is_running || max_angular_displacement == 0) {
quatCopy(output, input);
return;
}
// Calculate displacement
quatConj(&prev_ref, &conjugate);
quatMult(&conjugate, input, &displacement);
// Calculate displacement magnitude
input_angle = bams16ToBams32(bams16Acos(displacement.w)*2);
if(input_angle == 0) { // Check for no displacement case
quatCopy(output, input);
return;
}
sin_input = bams32Sin(input_angle/2);
// Apply displacement limits
if(input_angle > max_angular_displacement) {
limited_angle = max_angular_displacement;
} else if(input_angle < -max_angular_displacement) {
limited_angle = -max_angular_displacement;
} else {
limited_angle = input_angle;
}
sin_limited = bams32SinFine(limited_angle/2);
scale = sin_limited/sin_input;
displacement.w = bams32CosFine(limited_angle/2);
displacement.x = displacement.x*scale;
displacement.y = displacement.y*scale;
displacement.z = displacement.z*scale;
// Apply limited displacement
quatMult(&prev_ref, &displacement, output);
quatNormalize(output);
quatCopy(&prev_ref, output);
}
