// -----------------------------------------------------------------------------
static void HandleAttitudeReset(void)
{
quat_[0] = -AccelerationVector()[Z_BODY_AXIS];
quat_[1] = -AccelerationVector()[Y_BODY_AXIS];
quat_[2] = AccelerationVector()[X_BODY_AXIS];
quat_[3] = 0.0;
quat_[0] += QuaternionNorm(quat_);
QuaternionNormalize(quat_);
reset_attitude_ = 0;
}
AccelerationVector ConverterBox2D::convertLinearAcceleration(b2Vec2 acceleration) const noexcept
{
const auto coeff = (getTimeCoefficient() * getTimeCoefficient()) / getLengthCoefficient();
return coeff * AccelerationVector(
units::Acceleration(acceleration.x),
units::Acceleration(acceleration.y)
);
}
AccelerationVector convertAccToEarthFrame(Matrix<double,2>& DCM, AccelerationVector raw)
{
if((DCM.dim1()!=3)||(DCM.dim2()!=3)) {
cerr<<"Error, DCM MAtrix must be 3x3"<<endl;
return raw;
}
AccelerationVector earth_frame = AccelerationVector(raw.x, raw.y, raw.z);
earth_frame.x = DCM(0,0)*raw.x + DCM(1,0)*raw.y + DCM(2,0)*raw.z;
earth_frame.y = DCM(0,1)*raw.x + DCM(1,1)*raw.y + DCM(2,1)*raw.z;
earth_frame.z = DCM(0,2)*raw.x + DCM(1,2)*raw.y + DCM(2,2)*raw.z;
return earth_frame;
}
static float * CorrectQuaternionWithAccelerometer(float quat[4])
{
// Assume that the accelerometer measures ONLY the resistance to gravity
// (opposite the gravity vector). The direction of rotation that takes the
// body from predicted to estimated gravity is (-accelerometer x g_b_ x). This
// is equivalent to (g_b_ x accelerometer). Form a corrective quaternion from
// this rotation.
float quat_c[4] = { 1.0, 0.0, 0.0, 0.0 };
VectorCross(g_b_, AccelerationVector(), &quat_c[1]);
quat_c[1] *= 0.5 * ACCELEROMETER_CORRECTION_GAIN;
quat_c[2] *= 0.5 * ACCELEROMETER_CORRECTION_GAIN;
quat_c[3] *= 0.5 * ACCELEROMETER_CORRECTION_GAIN;
// Apply the correction to the attitude quaternion.
float result[4];
QuaternionMultiply(quat, quat_c, result);
quat[0] = result[0];
quat[1] = result[1];
quat[2] = result[2];
quat[3] = result[3];
return quat;
}
