new althold PID
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:
parent
cae8682c62
commit
073f5116bb
16
src/config.c
16
src/config.c
|
@ -230,9 +230,9 @@ static void resetConf(void)
|
|||
cfg.P8[YAW] = 85;
|
||||
cfg.I8[YAW] = 45;
|
||||
cfg.D8[YAW] = 0;
|
||||
cfg.P8[PIDALT] = 40;
|
||||
cfg.I8[PIDALT] = 25;
|
||||
cfg.D8[PIDALT] = 60;
|
||||
cfg.P8[PIDALT] = 50;
|
||||
cfg.I8[PIDALT] = 0;
|
||||
cfg.D8[PIDALT] = 0;
|
||||
cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100;
|
||||
cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100;
|
||||
cfg.D8[PIDPOS] = 0;
|
||||
|
@ -246,9 +246,9 @@ static void resetConf(void)
|
|||
cfg.I8[PIDLEVEL] = 10;
|
||||
cfg.D8[PIDLEVEL] = 100;
|
||||
cfg.P8[PIDMAG] = 40;
|
||||
cfg.P8[PIDVEL] = 0;
|
||||
cfg.I8[PIDVEL] = 0;
|
||||
cfg.D8[PIDVEL] = 0;
|
||||
cfg.P8[PIDVEL] = 120;
|
||||
cfg.I8[PIDVEL] = 45;
|
||||
cfg.D8[PIDVEL] = 1;
|
||||
cfg.rcRate8 = 90;
|
||||
cfg.rcExpo8 = 65;
|
||||
cfg.rollPitchRate = 0;
|
||||
|
@ -266,8 +266,8 @@ static void resetConf(void)
|
|||
cfg.accxy_deadband = 40;
|
||||
cfg.baro_tab_size = 21;
|
||||
cfg.baro_noise_lpf = 0.6f;
|
||||
cfg.baro_cf_vel = 0.995f;
|
||||
cfg.baro_cf_alt = 0.950f;
|
||||
cfg.baro_cf_vel = 0.985f;
|
||||
cfg.baro_cf_alt = 0.965f;
|
||||
cfg.acc_unarmedcal = 1;
|
||||
|
||||
// Radio
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue