Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00
Code Issues Wiki Activity

Convert to using magDev_t as in betaflight

Browse source
This commit is contained in:
Martin Budden 2016-12-02 22:27:22 +00:00
parent 8e86ce34c9
commit 5dcb8fa90c
20 changed files with 65 additions and 62 deletions
Download patch file Download diff file Expand all files Collapse all files

6
src/main/flight/imu.c
View file

@ -392,7 +392,7 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
/* Compute pitch/roll angles */
attitude.values.roll = RADIANS_TO_DECIDEGREES(atan2_approx(rMat[2][1], rMat[2][2]));
attitude.values.pitch = RADIANS_TO_DECIDEGREES((0.5f * M_PIf) - acos_approx(-rMat[2][0]));
attitude.values.yaw = RADIANS_TO_DECIDEGREES(-atan2_approx(rMat[1][0], rMat[0][0])) + magneticDeclination;
attitude.values.yaw = RADIANS_TO_DECIDEGREES(-atan2_approx(rMat[1][0], rMat[0][0])) + mag.magneticDeclination;
if (attitude.values.yaw < 0)
attitude.values.yaw += 3600;
@ -425,7 +425,7 @@ static int imuCalculateAccelerometerConfidence(void)
static bool isMagnetometerHealthy(void)
{
return (magADC[X] != 0) && (magADC[Y] != 0) && (magADC[Z] != 0);
return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0);
}
static void imuCalculateEstimatedAttitude(float dT)
@ -480,7 +480,7 @@ static void imuCalculateEstimatedAttitude(float dT)
imuMahonyAHRSupdate(dT, imuMeasuredRotationBF.A[X], imuMeasuredRotationBF.A[Y], imuMeasuredRotationBF.A[Z],
accWeight, imuMeasuredGravityBF.A[X], imuMeasuredGravityBF.A[Y], imuMeasuredGravityBF.A[Z],
useMag, magADC[X], magADC[Y], magADC[Z],
useMag, mag.magADC[X], mag.magADC[Y], mag.magADC[Z],
useCOG, courseOverGround);
imuUpdateEulerAngles();