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 rateApplyLocalRotation(Quaternion *rot) {
Quaternion current_ref;
rgltrGetQuatRef(¤t_ref);
quatMult(¤t_ref, rot, ¤t_ref);
quatNormalize(¤t_ref);
rgltrSetQuatRef(¤t_ref);
}
void rateProcess(void) {
Quaternion current_ref;
if(!is_ready || !is_running) { return; }
rgltrGetQuatRef(¤t_ref);
// q_body*q_current*q_global
quatMult(&global_displacement, ¤t_ref, ¤t_ref);
quatMult(¤t_ref, &body_displacement, ¤t_ref);
quatNormalize(¤t_ref);
rgltrSetQuatRef(¤t_ref);
}
// 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);
}
void attZero(void) {
float gxy, sina_2, xl[3], temp;
bams16_t a_2;
xlGetFloatXYZ(xl);
// Convert frames so that z axis is oriented upwards, x is forward, y is side
xl[2] = -xl[2];
temp = xl[0];
xl[0] = -xl[1];
xl[1] = temp;
gxy = sqrtf(xl[0]*xl[0] + xl[1]*xl[1]);
a_2 = (BAMS16_PI_2 + bams16Atan2(xl[2], gxy))/2;
sina_2 = bams16SinFine(a_2);
pose_quat.w = bams16CosFine(a_2)*gxy;
pose_quat.x = sina_2*(-xl[1]);
pose_quat.y = sina_2*(xl[0]);
pose_quat.z = 0.0;
quatNormalize(&pose_quat);
}
static void
check_quaternion(generator_t &gen, distribution_t &dist)
{
// check identity
{
quat q;
glm::quat g;
quatIdentity(&q);
assert(q == g);
}
for(size_t x = 0; x < 10000; ++x)
{
// check negation
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
assert(quatNegate(q) == -g);
}
// check addition
{
quat q1 = randomQuat(gen, dist);
quat q2 = randomQuat(gen, dist);
glm::quat g1 = loadQuat(q1);
glm::quat g2 = loadQuat(q2);
assert(quatAdd(q1, q2) == g1+g2);
}
// check subtraction
{
quat q1 = randomQuat(gen, dist);
quat q2 = randomQuat(gen, dist);
glm::quat g1 = loadQuat(q1);
glm::quat g2 = loadQuat(q2);
assert(quatSubtract(q1, q2) == g1 + (-g2));
}
// check scale
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
float f = dist(gen);
assert(quatScale(q, f) == g*f);
}
// check normalize
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
assert(quatNormalize(q) == glm::normalize(g));
}
// check dot
{
quat q1 = randomQuat(gen, dist);
quat q2 = randomQuat(gen, dist);
glm::quat g1 = loadQuat(q1);
glm::quat g2 = loadQuat(q2);
assert(std::abs(quatDot(q1, q2) - glm::dot(g1, g2)) < 0.0001f);
}
// check conjugate
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
assert(quatConjugate(q) == glm::conjugate(g));
}
// check inverse
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
assert(quatInverse(q) == glm::inverse(g));
}
// check quaternion multiplication
{
quat q1 = randomQuat(gen, dist);
quat q2 = randomQuat(gen, dist);
glm::quat g1 = loadQuat(q1);
glm::quat g2 = loadQuat(q2);
assert(quatMultiply(q1, q2) == g1*g2);
}
// check vector multiplication
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
glm::vec3 v = randomVector(gen, dist);
assert(quatMultiplyVec3f(q, (vec3f){ v.x, v.y, v.z }) == g*v);
}
// check rotation
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
glm::vec3 v = randomVector(gen, dist);
float r = randomAngle(gen, dist);
assert(quatRotate(q, (vec3f){ v.x, v.y, v.z }, r) == glm::rotate(g, r, v));
}
// check rotate X
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
float r = randomAngle(gen, dist);
assert(quatRotateX(q, r) == glm::rotate(g, r, x_axis));
}
// check rotate Y
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
float r = randomAngle(gen, dist);
assert(quatRotateY(q, r) == glm::rotate(g, r, y_axis));
}
// check rotate Z
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
float r = randomAngle(gen, dist);
assert(quatRotateZ(q, r) == glm::rotate(g, r, z_axis));
}
// check conversion from matrix
{
quat q = randomQuat(gen, dist);
glm::quat g = loadQuat(q);
mtx44 m;
quatToMtx44(&m, q);
assert(m == glm::mat4_cast(g));
}
}
}