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

Made scaling of gyro and acc in updates more consistent

Browse source
This commit is contained in:
Martin Budden 2017-11-26 09:32:26 +00:00
parent 6152dec4ab
commit da4dfe1f1c
2 changed files with 7 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -21,11 +21,10 @@
#include <stdint.h>
#include <math.h>
#include "build/build_config.h"
#include "platform.h"
#include "blackbox/blackbox.h"
#include "build/build_config.h"
#include "common/axis.h"
@ -88,8 +87,6 @@ FASTRAM attitudeEulerAngles_t attitude; // absolute angle inclinatio
STATIC_FASTRAM imuRuntimeConfig_t imuRuntimeConfig;
static const float gyroScale = (M_PIf / 180.0f); // gyro output scaled to rad per second
STATIC_FASTRAM bool gpsHeadingInitialized;
PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
@ -116,7 +113,7 @@ void imuUpdateGyroscope(timeUs_t gyroUpdateDeltaUs)
const float gyroUpdateDelta = gyroUpdateDeltaUs * 1e-6f;
for (int axis = 0; axis < 3; axis++) {
imuAccumulatedRate[axis] += gyro.gyroADCf[axis] * gyroScale * gyroUpdateDelta;
imuAccumulatedRate[axis] += gyro.gyroADCf[axis] * gyroUpdateDelta;
}
imuAccumulatedRateTimeUs += gyroUpdateDeltaUs;
@ -543,12 +540,12 @@ static void imuUpdateMeasuredRotationRate(void)
imuAccumulatedRateTimeUs = 0;
for (axis = 0; axis < 3; axis++) {
imuMeasuredRotationBF.A[axis] = imuAccumulatedRate[axis] / imuAccumulatedRateTime;
imuMeasuredRotationBF.A[axis] = DEGREES_TO_RADIANS(imuAccumulatedRate[axis] / imuAccumulatedRateTime);
imuAccumulatedRate[axis] = 0.0f;
}
#else
for (axis = 0; axis < 3; axis++) {
imuMeasuredRotationBF.A[axis] = gyro.gyroADCf[axis] * gyroScale;
imuMeasuredRotationBF.A[axis] = DEGREES_TO_RADIANS(gyro.gyroADCf[axis]);
}
#endif
}
@ -560,7 +557,7 @@ static void imuUpdateMeasuredAcceleration(void)
#ifdef ASYNC_GYRO_PROCESSING
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
imuMeasuredAccelBF.A[axis] = imuAccumulatedAcc[axis] / imuAccumulatedAccCount;
imuMeasuredAccelBF.A[axis] = imuAccumulatedAcc[axis] * GRAVITY_CMSS / imuAccumulatedAccCount;
imuAccumulatedAcc[axis] = 0;
}
imuAccumulatedAccCount = 0;;
@ -606,7 +603,7 @@ void imuUpdateAccelerometer(void)
#ifdef ASYNC_GYRO_PROCESSING
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
imuAccumulatedAcc[axis] += acc.accADCf[axis] * GRAVITY_CMSS;
imuAccumulatedAcc[axis] += acc.accADCf[axis];
}
imuAccumulatedAccCount++;
#endif