rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Updated / simplified some calculations.

This commit is contained in:
Michael Keller 2017-04-10 10:13:56 +12:00
parent 6a03b48e7f
commit 396388bf84
1 changed files with 4 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/main/flight/altitude.c
View File

@ -236,12 +236,11 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
#ifdef SONAR
if (sensors(SENSOR_SONAR)) {
int32_t sonarAlt = sonarRead();
sonarAlt = sonarCalculateAltitude(sonarAlt, getCosTiltAngle());
int32_t sonarAlt = sonarCalculateAltitude(sonarRead(), getCosTiltAngle());
if (sonarAlt > 0 && sonarAlt >= sonarCfAltCm && sonarAlt <= sonarMaxAltWithTiltCm) {
// SONAR in range, so use complementary filter
float sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
sonarAlt = sonarAlt * sonarTransition + baroAlt * (1.0f - sonarTransition);
sonarAlt = (float)sonarAlt * sonarTransition + baroAlt * (1.0f - sonarTransition);
estimatedAltitude = sonarAlt;
}
}
@ -254,7 +253,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
// Integrator - velocity, cm/sec
if (accSumCount) {
accZ_tmp = (float)accSum[2] / (float)accSumCount;
accZ_tmp = (float)accSum[2] / accSumCount;
}
const float vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
@ -290,7 +289,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
// 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 * barometerConfig()->baro_cf_vel + baroVel * (1.0f - barometerConfig()->baro_cf_vel);
vel = vel * barometerConfig()->baro_cf_vel + (float)baroVel * (1.0f - barometerConfig()->baro_cf_vel);
int32_t vel_tmp = lrintf(vel);
// set vario