Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 05:15:25 +03:00
Code Issues Wiki Activity

Decoupling imu from config - barometer config.

Browse source
This commit is contained in:
Dominic Clifton 2014-06-06 20:59:59 +01:00
parent 82bb6b7982
commit 51eee3d62c
4 changed files with 14 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -106,11 +106,13 @@ void calculateThrottleAngleScale(uint16_t throttle_correction_angle)
imuRuntimeConfig_t *imuRuntimeConfig;
pidProfile_t *pidProfile;
barometerConfig_t *barometerConfig;
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile)
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig)
{
imuRuntimeConfig = initialImuRuntimeConfig;
pidProfile = initialPidProfile;
barometerConfig = intialBarometerConfig;
}
void computeIMU(rollAndPitchTrims_t *accelerometerTrims)
@ -455,7 +457,7 @@ int getEstimatedAltitude(void)
// Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * currentProfile.barometerConfig.baro_cf_alt + (float)BaroAlt * (1.0f - currentProfile.barometerConfig.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc)
accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for Altitude estimation (baro & acc)
EstAlt = accAlt;
vel += vel_acc;
@ -475,7 +477,7 @@ int getEstimatedAltitude(void)
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * currentProfile.barometerConfig.baro_cf_vel + baroVel * (1 - currentProfile.barometerConfig.baro_cf_vel);
vel = vel * barometerConfig->baro_cf_vel + baroVel * (1 - barometerConfig->baro_cf_vel);
vel_tmp = lrintf(vel);
// set vario