Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 12:25:20 +03:00
Code Issues Wiki Activity

oops. gyro only does NOT need getEstimatedAttitude. bad!

Browse source 
ms5611 driver improvements (was failing below 20c)
merged some althold cleanups

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@411 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2013-09-21 11:18:15 +00:00
parent 1ff4bcec5a
commit 64e8f247bf
4 changed files with 23 additions and 26 deletions
Download patch file Download diff file Expand all files Collapse all files

12
src/imu.c
View file

@ -8,7 +8,7 @@ int accSumCount = 0;
int16_t acc_25deg = 0;
int32_t baroPressure = 0;
int32_t baroTemperature = 0;
int32_t baroPressureSum = 0;
uint32_t baroPressureSum = 0;
int32_t BaroAlt = 0;
int32_t sonarAlt; // to think about the unit
int32_t EstAlt; // in cm
@ -50,12 +50,12 @@ void computeIMU(void)
Gyro_getADC();
if (sensors(SENSOR_ACC)) {
ACC_getADC();
getEstimatedAttitude();
} else {
accADC[X] = 0;
accADC[Y] = 0;
accADC[Z] = 0;
}
getEstimatedAttitude();
if (feature(FEATURE_GYRO_SMOOTHING)) {
static uint8_t Smoothing[3] = { 0, 0, 0 };
@ -367,13 +367,12 @@ int getEstimatedAltitude(void)
//P
error = constrain(AltHold - EstAlt, -300, 300);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
BaroPID = constrain((cfg.P8[PIDALT] * error / 128), -150, +150);
BaroPID = constrain((cfg.P8[PIDALT] * error / 128), -200, +200);
//I
errorAltitudeI += cfg.I8[PIDALT] * error / 64;
errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
BaroPID += errorAltitudeI / 512; // I in range +/-60
errorAltitudeI = constrain(errorAltitudeI, -50000, 50000);
BaroPID += errorAltitudeI / 512; // I in range +/-100
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = BaroAlt;
@ -384,6 +383,7 @@ int getEstimatedAltitude(void)
// 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);
constrain(vel, -1000, 1000); // limit max velocity to +/- 10m/s (36km/h)
// D
vel_tmp = vel;