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

new althold PID

Browse source 
althold should work better now, the PID settings are preliminary. There
is definetly room for improvement.
For aquiring your own PID settings set ALT_P = 0 and tune the VEL pid
until the copter only drifts a little from its position when you
activate althold. Then set ALT_P to a value where it holds the position
stable
This commit is contained in:
Lukas S 2014-01-27 16:10:17 +01:00
parent cae8682c62
commit 073f5116bb
2 changed files with 34 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

41
src/imu.c Executable file → Normal file
View file

@ -174,7 +174,7 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
return value;
}
#define F_CUT_ACCZ 20.0f
#define F_CUT_ACCZ 10.0f // 10Hz should still be fast enough
static const float fc_acc = 0.5f / (M_PI * F_CUT_ACCZ);
// rotate acc into Earth frame and calculate acceleration in it
@ -332,8 +332,11 @@ int getEstimatedAltitude(void)
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;
@ -364,7 +367,8 @@ int getEstimatedAltitude(void)
dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
// Integrator - velocity, cm/sec
vel_acc = (float)accSum[2] * accVelScale * (float)accTimeSum / (float)accSumCount;
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)
@ -380,16 +384,6 @@ int getEstimatedAltitude(void)
accSum_reset();
//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), -200, +200);
//I
errorAltitudeI += cfg.I8[PIDALT] * error / 64;
errorAltitudeI = constrain(errorAltitudeI, -50000, 50000);
BaroPID += errorAltitudeI / 512; // I in range +/-100
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = BaroAlt;
@ -399,13 +393,30 @@ 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);
vel = constrain(vel, -1000, 1000); // limit max velocity to +/- 10m/s (36km/h)
// D
// set vario
vel_tmp = lrintf(vel);
vel_tmp = applyDeadband(vel_tmp, 5);
vario = vel_tmp;
BaroPID -= constrain(cfg.D8[PIDALT] * vel_tmp / 16, -150, 150);
// 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 - lrintf(vel);
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
accZ_old = accZ_tmp;
BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150);
return 1;
}