int getEstimatedAltitude(void) { static uint32_t previousT; uint32_t currentT = micros(); uint32_t dTime; int32_t error; int32_t baroVel; int32_t vel_tmp; int32_t BaroAlt_tmp; int32_t setVel; float dt; float vel_acc; float accZ_tmp; static float accZ_old = 0.0f; static float vel = 0.0f; static float accAlt = 0.0f; static int32_t lastBaroAlt; static int32_t baroGroundAltitude = 0; static int32_t baroGroundPressure = 0; int16_t tiltAngle = max(abs(angle[ROLL]), abs(angle[PITCH])); dTime = currentT - previousT; if (dTime < UPDATE_INTERVAL) return 0; previousT = currentT; if (calibratingB > 0) { baroGroundPressure -= baroGroundPressure / 8; baroGroundPressure += baroPressureSum / (cfg.baro_tab_size - 1); baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f; vel = 0; accAlt = 0; calibratingB--; } // calculates height from ground via baro readings // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140 BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / (cfg.baro_tab_size - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm BaroAlt_tmp -= baroGroundAltitude; BaroAlt = lrintf((float)BaroAlt * cfg.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf)); // additional LPF to reduce baro noise // calculate sonar altitude only if the sonar is facing downwards(<25deg) if (tiltAngle > 250) sonarAlt = -1; else sonarAlt = sonarAlt * (900.0f - tiltAngle) / 900.0f; // do sonarAlt and baroAlt fusion if (sonarAlt > 0 && sonarAlt < 200) { baroAlt_offset = BaroAlt - sonarAlt; BaroAlt = sonarAlt; } else { BaroAlt -= baroAlt_offset; if (sonarAlt > 0) { sonarTransition = (300 - sonarAlt) / 100.0f; BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition); } } dt = accTimeSum * 1e-6f; // delta acc reading time in seconds // Integrator - velocity, cm/sec accZ_tmp = (float)accSum[2] / (float)accSumCount; 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 * cfg.baro_cf_alt + (float)BaroAlt * (1.0f - cfg.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc) // when the sonar is in his best range if (sonarAlt > 0 && sonarAlt < 200) EstAlt = BaroAlt; else EstAlt = accAlt; vel += vel_acc; #if 0 debug[0] = accSum[2] / accSumCount; // acceleration debug[1] = vel; // velocity debug[2] = accAlt; // height #endif accSum_reset(); baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime; lastBaroAlt = BaroAlt; baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero // 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 * cfg.baro_cf_vel + baroVel * (1 - cfg.baro_cf_vel); vel_tmp = lrintf(vel); // set vario vario = applyDeadband(vel_tmp, 5); if (tiltAngle < 800) { // only calculate pid if the copters thrust is facing downwards(<80deg) // Altitude P-Controller error = constrain(AltHold - EstAlt, -500, 500); error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position setVel = constrain((cfg.P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s // Velocity PID-Controller // P error = setVel - vel_tmp; BaroPID = constrain((cfg.P8[PIDVEL] * error / 32), -300, +300); // I errorAltitudeI += (cfg.I8[PIDVEL] * error) / 8; errorAltitudeI = constrain(errorAltitudeI, -(1024 * 200), (1024 * 200)); BaroPID += errorAltitudeI / 1024; // I in range +/-200 // D BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150); } else { BaroPID = 0; } accZ_old = accZ_tmp; return 1; }
void calculateEstimatedAltitude(uint32_t currentTime) { static uint32_t previousTime; uint32_t dTime; int32_t baroVel; float dt; float vel_acc; int32_t vel_tmp; float accZ_tmp; static float accZ_old = 0.0f; static float vel = 0.0f; static float accAlt = 0.0f; static int32_t lastBaroAlt; static int32_t baroAlt_offset = 0; float sonarTransition; #ifdef SONAR int16_t tiltAngle; #endif dTime = currentTime - previousTime; if (dTime < BARO_UPDATE_FREQUENCY_40HZ) return; previousTime = currentTime; #ifdef BARO if (!isBaroCalibrationComplete()) { performBaroCalibrationCycle(); vel = 0; accAlt = 0; } BaroAlt = baroCalculateAltitude(); #else BaroAlt = 0; #endif #ifdef SONAR tiltAngle = calculateTiltAngle(&inclination); sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle); #endif if (sonarAlt > 0 && sonarAlt < 200) { baroAlt_offset = BaroAlt - sonarAlt; BaroAlt = sonarAlt; } else { BaroAlt -= baroAlt_offset; if (sonarAlt > 0) { sonarTransition = (300 - sonarAlt) / 100.0f; BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition); } } dt = accTimeSum * 1e-6f; // delta acc reading time in seconds // Integrator - velocity, cm/sec accZ_tmp = (float)accSum[2] / (float)accSumCount; 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 * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for altitude estimation (baro & acc) vel += vel_acc; #if 0 debug[1] = accSum[2] / accSumCount; // acceleration debug[2] = vel; // velocity debug[3] = accAlt; // height #endif accSum_reset(); #ifdef BARO if (!isBaroCalibrationComplete()) { return; } #endif if (sonarAlt > 0 && sonarAlt < 200) { // the sonar has the best range EstAlt = BaroAlt; } else { EstAlt = accAlt; } 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 // 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_tmp = lrintf(vel); // set vario vario = applyDeadband(vel_tmp, 5); altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old); accZ_old = accZ_tmp; }