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; int32_t sonarAlt = -1; 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 = sonarRead(); sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle); #endif if (sonarAlt > 0 && sonarAlt < 200) { baroAlt_offset = BaroAlt - sonarAlt; BaroAlt = sonarAlt; } else { BaroAlt -= baroAlt_offset; if (sonarAlt > 0 && sonarAlt <= 300) { 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 if (accSumCount) { accZ_tmp = (float)accSum[2] / (float)accSumCount; } else { accZ_tmp = 0; } vel_acc = accZ_tmp * accVelScale * (float)accTimeSum; // Integrator - Altitude in cm accAlt += (vel_acc * 0.5f) * dt + vel * dt; accalttemp=lrintf(100*accAlt); //Checking how acc measures height // 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; #ifdef DEBUG_ALT_HOLD debug[1] = accSum[2] / accSumCount; // acceleration debug[2] = vel; // velocity debug[3] = accAlt; // height #endif imuResetAccelerationSum(); #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); if (1)//!(ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband)) { altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old); }//dronadrona_1200am Temp=pidProfile->I8[PIDALT]; barometerConfig->baro_cf_alt=1-Temp/1000 ; accZ_old = accZ_tmp; }
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; if (!isBaroCalibrationComplete()) { performBaroCalibrationCycle(); vel = 0; accAlt = 0; } BaroAlt = baroCalculateAltitude(); #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 1 debug[1] = accSum[2] / accSumCount; // acceleration debug[2] = vel; // velocity debug[3] = accAlt; // height #endif accSum_reset(); if (!isBaroCalibrationComplete()) { return; } 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, -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 * barometerConfig->baro_cf_vel + baroVel * (1.0f - barometerConfig->baro_cf_vel); vel_tmp = lrintf(vel); // set vario vario = applyDeadband(vel_tmp, 5); BaroPID = calculateBaroPid(vel_tmp, accZ_tmp, accZ_old); accZ_old = accZ_tmp; }