void AHRS::updateAngles(float timeStep)
{
float aX = accel.x.evaluate();
float aY = accel.y.evaluate();
float aZ = accel.z.evaluate();
aX = accel.x.computeFilter(aX);
aY = accel.y.computeFilter(aY);
aZ = accel.z.computeFilter(aZ);
float gX = gyro.x.evaluate();
float gY = gyro.y.evaluate();
float gZ = gyro.z.evaluate();
gX *= 0.070f * PI / 180.0f;
gY *= 0.070f * PI / 180.0f;
gZ *= 0.070f * PI / 180.0f;
gyro.x.lastValue = gX;
gyro.y.lastValue = gY;
gyro.z.lastValue = gZ;
argUpdate(gX, gY, gZ, -aX, -aY, -aZ, timeStep);
angles.pitch = atan2(2 * (q0*q1 + q2*q3), 1 - 2 * (q1*q1 + q2*q2));
angles.roll = asin(2 * (q0*q2 - q1*q3));
//angles.yaw = atan2(2 * (q0*q3 + q1*q2), 1 - 2 * (q2*q2 + q3*q3));
angles.pitch *= 180.0f / PI;
angles.roll *= -180.0f / PI;
//angles.yaw *= -180.0f / PI;
}
////////////////////////////////////////////////////////////////////////////////
// Calculate ARG
////////////////////////////////////////////////////////////////////////////////
void calculateKinematics(float rollRate, float pitchRate, float yawRate,
float longitudinalAccel, float lateralAccel, float verticalAccel,
float G_DT) {
argUpdate(rollRate, pitchRate, yawRate,
longitudinalAccel, lateralAccel, verticalAccel,
G_Dt);
eulerAngles();
}
