int32_t baroCalculateAltitude(void) { int32_t BaroAlt_tmp; // calculates height from ground via baro readings // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140 if (isBaroCalibrationComplete()) { BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm BaroAlt_tmp -= baroGroundAltitude; baro.BaroAlt = lrintf((float)baro.BaroAlt * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_noise_lpf) + (float)BaroAlt_tmp * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_noise_lpf))); // additional LPF to reduce baro noise } else { baro.BaroAlt = 0; } return baro.BaroAlt; }
void calculateEstimatedAltitude(timeUs_t currentTimeUs) { static timeUs_t previousTimeUs = 0; const uint32_t dTime = currentTimeUs - previousTimeUs; if (dTime < BARO_UPDATE_FREQUENCY_40HZ) { return; } previousTimeUs = currentTimeUs; static float vel = 0.0f; static float accAlt = 0.0f; int32_t baroAlt = 0; #ifdef BARO if (sensors(SENSOR_BARO)) { if (!isBaroCalibrationComplete()) { performBaroCalibrationCycle(); vel = 0; accAlt = 0; } else { baroAlt = baroCalculateAltitude(); estimatedAltitude = baroAlt; } } #endif #ifdef SONAR if (sensors(SENSOR_SONAR)) { 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 = (float)sonarAlt * sonarTransition + baroAlt * (1.0f - sonarTransition); estimatedAltitude = sonarAlt; } } #endif float accZ_tmp = 0; #ifdef ACC if (sensors(SENSOR_ACC)) { const float dt = accTimeSum * 1e-6f; // delta acc reading time in seconds // Integrator - velocity, cm/sec if (accSumCount) { accZ_tmp = (float)accSum[2] / accSumCount; } const float vel_acc = accZ_tmp * accVelScale * (float)accTimeSum; // Integrator - Altitude in cm accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2) accAlt = accAlt * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_alt) + (float)baro.BaroAlt * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_alt)); // complementary filter for altitude estimation (baro & acc) vel += vel_acc; estimatedAltitude = accAlt; } #endif DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_ACC, accSum[2] / accSumCount); DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_VEL, vel); DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_HEIGHT, accAlt); imuResetAccelerationSum(); int32_t baroVel = 0; #ifdef BARO if (sensors(SENSOR_BARO)) { if (!isBaroCalibrationComplete()) { return; } static int32_t lastBaroAlt = 0; baroVel = (baroAlt - lastBaroAlt) * 1000000.0f / dTime; lastBaroAlt = baroAlt; baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero } #endif // 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 * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel) + baroVel * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel)); int32_t vel_tmp = lrintf(vel); // set vario estimatedVario = applyDeadband(vel_tmp, 5); static float accZ_old = 0.0f; altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old); accZ_old = accZ_tmp; }