Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 22:35:23 +03:00
Code Issues Wiki Activity

Applied 'USE_ACC' consistently.

Browse source
This commit is contained in:
mikeller 2019-02-06 17:08:51 +13:00
parent 59ea4becb3
commit b5908f5bab
29 changed files with 250 additions and 155 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -90,7 +90,6 @@ float accAverage[XYZ_AXIS_COUNT];
uint32_t accTimeSum = 0; // keep track for integration of acc
int accSumCount = 0;
float accVelScale;
bool canUseGPSHeading = true;
static float throttleAngleScale;
@ -169,8 +168,6 @@ void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correctio
void imuInit(void)
{
accVelScale = 9.80665f * acc.dev.acc_1G_rec / 10000.0f;
#ifdef USE_GPS
canUseGPSHeading = true;
#else
@ -195,6 +192,7 @@ void imuResetAccelerationSum(void)
accTimeSum = 0;
}
#if defined(USE_ACC)
static float invSqrt(float x)
{
return 1.0f / sqrtf(x);
@ -351,7 +349,6 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
}
}
#ifdef USE_ACC
static bool imuIsAccelerometerHealthy(float *accAverage)
{
float accMagnitudeSq = 0;
@ -365,7 +362,6 @@ static bool imuIsAccelerometerHealthy(float *accAverage)
// Accept accel readings only in range 0.9g - 1.1g
return (0.81f < accMagnitudeSq) && (accMagnitudeSq < 1.21f);
}
#endif
// Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.dcm_kp * 1.0 scaling.
// When disarmed after initial boot, the scaling is set to 10.0 for the first 20 seconds to speed up initial convergence.
@ -373,7 +369,7 @@ static bool imuIsAccelerometerHealthy(float *accAverage)
// - wait for a 250ms period of low gyro activity to ensure the craft is not moving
// - use a large dcmKpGain value for 500ms to allow the attitude estimate to quickly converge
// - reset the gain back to the standard setting
float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage)
static float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage)
{
static bool lastArmState = false;
static timeUs_t gyroQuietPeriodTimeEnd = 0;
@ -487,16 +483,15 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
// printf("[imu]deltaT = %u, imuDeltaT = %u, currentTimeUs = %u, micros64_real = %lu\n", deltaT, imuDeltaT, currentTimeUs, micros64_real());
UNUSED(imuCalcKpGain);
deltaT = imuDeltaT;
#endif
float gyroAverage[XYZ_AXIS_COUNT];
gyroGetAccumulationAverage(gyroAverage);
#ifdef USE_ACC
if (accGetAccumulationAverage(accAverage)) {
useAcc = imuIsAccelerometerHealthy(accAverage);
}
#endif
imuMahonyAHRSupdate(deltaT * 1e-6f,
DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),
@ -508,7 +503,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
#endif
}
int calculateThrottleAngleCorrection(void)
static int calculateThrottleAngleCorrection(void)
{
/*
* Use 0 as the throttle angle correction if we are inverted, vertical or with a
@ -551,6 +546,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
acc.accADC[Z] = 0;
}
}
#endif // USE_ACC
bool shouldInitializeGPSHeading()
{