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

old altitude calculation used again, added lpf for accZ by Luggi09

Browse source 
git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@462 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2013-10-30 11:57:54 +00:00
parent 2d076db908
commit 35f0a8e4b0
2 changed files with 26 additions and 17 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/config.c
View file

@ -227,9 +227,9 @@ static void resetConf(void)
cfg.P8[YAW] = 85; cfg.P8[YAW] = 85;
cfg.I8[YAW] = 45; cfg.I8[YAW] = 45;
cfg.D8[YAW] = 0; cfg.D8[YAW] = 0;
cfg.P8[PIDALT] = 50; cfg.P8[PIDALT] = 40;
cfg.I8[PIDALT] = 25; cfg.I8[PIDALT] = 25;
cfg.D8[PIDALT] = 80; cfg.D8[PIDALT] = 60;
cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100; cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100;
cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100; cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100;
cfg.D8[PIDPOS] = 0; cfg.D8[PIDPOS] = 0;

39
src/imu.c
View file

@ -172,17 +172,21 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
return value; return value;
} }
#define F_CUT_ACCZ 20.0f
static const float fc_acc = 0.5f / (M_PI * F_CUT_ACCZ);
// rotate acc into Earth frame and calculate acceleration in it // rotate acc into Earth frame and calculate acceleration in it
void acc_calc(uint32_t deltaT) void acc_calc(uint32_t deltaT)
{ {
static int32_t accZoffset = 0; static int32_t accZoffset = 0;
static float accz_smooth;
float rpy[3]; float rpy[3];
t_fp_vector accel_ned; t_fp_vector accel_ned;
// the accel values have to be rotated into the earth frame // the accel values have to be rotated into the earth frame
rpy[0] = -(float)anglerad[ROLL]; rpy[0] = -(float)anglerad[ROLL];
rpy[1] = -(float)anglerad[PITCH]; rpy[1] = -(float)anglerad[PITCH];
rpy[2] = -(float)heading * RADX10 * 10.0f; rpy[2] = -(float)heading * RAD;
accel_ned.V.X = accSmooth[0]; accel_ned.V.X = accSmooth[0];
accel_ned.V.Y = accSmooth[1]; accel_ned.V.Y = accSmooth[1];
@ -198,19 +202,21 @@ void acc_calc(uint32_t deltaT)
accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
} else } else
accel_ned.V.Z -= acc_1G; accel_ned.V.Z -= acc_1G;
accz_smooth = accz_smooth + (deltaT / (fc_acc + deltaT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
// apply Deadband to reduce integration drift and vibration influence // apply Deadband to reduce integration drift and vibration influence
accel_ned.V.Z = applyDeadband(accel_ned.V.Z, cfg.accz_deadband); accel_ned.V.Z = applyDeadband(lrintf(accz_smooth), cfg.accz_deadband);
accel_ned.V.X = applyDeadband(accel_ned.V.X, cfg.accxy_deadband); accel_ned.V.X = applyDeadband(lrintf(accel_ned.V.X), cfg.accxy_deadband);
accel_ned.V.Y = applyDeadband(accel_ned.V.Y, cfg.accxy_deadband); accel_ned.V.Y = applyDeadband(lrintf(accel_ned.V.Y), cfg.accxy_deadband);
// sum up Values for later integration to get velocity and distance // sum up Values for later integration to get velocity and distance
accTimeSum += deltaT; accTimeSum += deltaT;
accSumCount++; accSumCount++;
accSum[0] += accel_ned.V.X; accSum[X] += lrintf(accel_ned.V.X);
accSum[1] += accel_ned.V.Y; accSum[Y] += lrintf(accel_ned.V.Y);
accSum[2] += accel_ned.V.Z; accSum[Z] += lrintf(accel_ned.V.Z);
} }
void accSum_reset(void) void accSum_reset(void)
@ -317,7 +323,6 @@ static void getEstimatedAttitude(void)
int getEstimatedAltitude(void) int getEstimatedAltitude(void)
{ {
static int32_t baroGroundPressure;
static uint32_t previousT; static uint32_t previousT;
uint32_t currentT = micros(); uint32_t currentT = micros();
uint32_t dTime; uint32_t dTime;
@ -326,11 +331,12 @@ int getEstimatedAltitude(void)
int32_t vel_tmp; int32_t vel_tmp;
int32_t BaroAlt_tmp; int32_t BaroAlt_tmp;
float dt; float dt;
float PressureScaling;
float vel_acc; float vel_acc;
static float vel = 0.0f; static float vel = 0.0f;
static float accAlt = 0.0f; static float accAlt = 0.0f;
static int32_t lastBaroAlt; static int32_t lastBaroAlt;
static int32_t baroGroundAltitude = 0;
static int32_t baroGroundPressure = 0;
dTime = currentT - previousT; dTime = currentT - previousT;
if (dTime < UPDATE_INTERVAL) if (dTime < UPDATE_INTERVAL)
@ -338,17 +344,20 @@ int getEstimatedAltitude(void)
previousT = currentT; previousT = currentT;
if (calibratingB > 0) { if (calibratingB > 0) {
baroGroundPressure = baroPressureSum / (cfg.baro_tab_size - 1); baroGroundPressure -= baroGroundPressure / 8;
calibratingB--; baroGroundPressure += baroPressureSum / (cfg.baro_tab_size - 1);
baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f;
vel = 0; vel = 0;
accAlt = 0; accAlt = 0;
calibratingB--;
} }
// calculates height from ground via baro readings // calculates height from ground via baro readings
// see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140 // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140
PressureScaling = (float)baroPressureSum / ((float)baroGroundPressure * (float)(cfg.baro_tab_size - 1)); BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / (cfg.baro_tab_size - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
BaroAlt_tmp = 153.8462f * (baroTemperature + 27315) * (1.0f - expf(0.190259f * logf(PressureScaling))); // in cm BaroAlt_tmp -= baroGroundAltitude;
BaroAlt = (float)BaroAlt * cfg.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf); // additional LPF to reduce baro noise BaroAlt = lrintf((float)BaroAlt * cfg.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf)); // additional LPF to reduce baro noise
dt = accTimeSum * 1e-6f; // delta acc reading time in seconds dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
@ -391,7 +400,7 @@ int getEstimatedAltitude(void)
vel = constrain(vel, -1000, 1000); // limit max velocity to +/- 10m/s (36km/h) vel = constrain(vel, -1000, 1000); // limit max velocity to +/- 10m/s (36km/h)
// D // D
vel_tmp = vel; vel_tmp = lrintf(vel);
vel_tmp = applyDeadband(vel_tmp, 5); vel_tmp = applyDeadband(vel_tmp, 5);
vario = vel_tmp; vario = vel_tmp;
BaroPID -= constrain(cfg.D8[PIDALT] * vel_tmp / 16, -150, 150); BaroPID -= constrain(cfg.D8[PIDALT] * vel_tmp / 16, -150, 150);