Updated / simplified some calculations.
This commit is contained in:
parent
6a03b48e7f
commit
396388bf84
|
@ -236,12 +236,11 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
|
||||||
|
|
||||||
#ifdef SONAR
|
#ifdef SONAR
|
||||||
if (sensors(SENSOR_SONAR)) {
|
if (sensors(SENSOR_SONAR)) {
|
||||||
int32_t sonarAlt = sonarRead();
|
int32_t sonarAlt = sonarCalculateAltitude(sonarRead(), getCosTiltAngle());
|
||||||
sonarAlt = sonarCalculateAltitude(sonarAlt, getCosTiltAngle());
|
|
||||||
if (sonarAlt > 0 && sonarAlt >= sonarCfAltCm && sonarAlt <= sonarMaxAltWithTiltCm) {
|
if (sonarAlt > 0 && sonarAlt >= sonarCfAltCm && sonarAlt <= sonarMaxAltWithTiltCm) {
|
||||||
// SONAR in range, so use complementary filter
|
// SONAR in range, so use complementary filter
|
||||||
float sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
|
float sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
|
||||||
sonarAlt = sonarAlt * sonarTransition + baroAlt * (1.0f - sonarTransition);
|
sonarAlt = (float)sonarAlt * sonarTransition + baroAlt * (1.0f - sonarTransition);
|
||||||
estimatedAltitude = sonarAlt;
|
estimatedAltitude = sonarAlt;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -254,7 +253,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
|
||||||
|
|
||||||
// Integrator - velocity, cm/sec
|
// Integrator - velocity, cm/sec
|
||||||
if (accSumCount) {
|
if (accSumCount) {
|
||||||
accZ_tmp = (float)accSum[2] / (float)accSumCount;
|
accZ_tmp = (float)accSum[2] / accSumCount;
|
||||||
}
|
}
|
||||||
const float vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
|
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).
|
// 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
|
// 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);
|
int32_t vel_tmp = lrintf(vel);
|
||||||
|
|
||||||
// set vario
|
// set vario
|
||||||
|
|
Loading…
Reference in New Issue