// ----------------------------------------------------------------------------- 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; }