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.P8[YAW] = 85;
|
||||||
cfg.I8[YAW] = 45;
|
cfg.I8[YAW] = 45;
|
||||||
cfg.D8[YAW] = 0;
|
cfg.D8[YAW] = 0;
|
||||||
cfg.P8[PIDALT] = 40;
|
cfg.P8[PIDALT] = 50;
|
||||||
cfg.I8[PIDALT] = 25;
|
cfg.I8[PIDALT] = 0;
|
||||||
cfg.D8[PIDALT] = 60;
|
cfg.D8[PIDALT] = 0;
|
||||||
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;
|
||||||
|
@ -246,9 +246,9 @@ static void resetConf(void)
|
||||||
cfg.I8[PIDLEVEL] = 10;
|
cfg.I8[PIDLEVEL] = 10;
|
||||||
cfg.D8[PIDLEVEL] = 100;
|
cfg.D8[PIDLEVEL] = 100;
|
||||||
cfg.P8[PIDMAG] = 40;
|
cfg.P8[PIDMAG] = 40;
|
||||||
cfg.P8[PIDVEL] = 0;
|
cfg.P8[PIDVEL] = 120;
|
||||||
cfg.I8[PIDVEL] = 0;
|
cfg.I8[PIDVEL] = 45;
|
||||||
cfg.D8[PIDVEL] = 0;
|
cfg.D8[PIDVEL] = 1;
|
||||||
cfg.rcRate8 = 90;
|
cfg.rcRate8 = 90;
|
||||||
cfg.rcExpo8 = 65;
|
cfg.rcExpo8 = 65;
|
||||||
cfg.rollPitchRate = 0;
|
cfg.rollPitchRate = 0;
|
||||||
|
@ -266,8 +266,8 @@ static void resetConf(void)
|
||||||
cfg.accxy_deadband = 40;
|
cfg.accxy_deadband = 40;
|
||||||
cfg.baro_tab_size = 21;
|
cfg.baro_tab_size = 21;
|
||||||
cfg.baro_noise_lpf = 0.6f;
|
cfg.baro_noise_lpf = 0.6f;
|
||||||
cfg.baro_cf_vel = 0.995f;
|
cfg.baro_cf_vel = 0.985f;
|
||||||
cfg.baro_cf_alt = 0.950f;
|
cfg.baro_cf_alt = 0.965f;
|
||||||
cfg.acc_unarmedcal = 1;
|
cfg.acc_unarmedcal = 1;
|
||||||
|
|
||||||
// Radio
|
// Radio
|
||||||
|
|
|
@ -174,7 +174,7 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
|
||||||
return value;
|
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);
|
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
|
||||||
|
@ -332,8 +332,11 @@ int getEstimatedAltitude(void)
|
||||||
int32_t baroVel;
|
int32_t baroVel;
|
||||||
int32_t vel_tmp;
|
int32_t vel_tmp;
|
||||||
int32_t BaroAlt_tmp;
|
int32_t BaroAlt_tmp;
|
||||||
|
int32_t setVel;
|
||||||
float dt;
|
float dt;
|
||||||
float vel_acc;
|
float vel_acc;
|
||||||
|
float accZ_tmp;
|
||||||
|
static float accZ_old = 0.0f;
|
||||||
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;
|
||||||
|
@ -364,7 +367,8 @@ int getEstimatedAltitude(void)
|
||||||
dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
|
dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
|
||||||
|
|
||||||
// Integrator - velocity, cm/sec
|
// 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
|
// Integrator - Altitude in cm
|
||||||
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
|
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();
|
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;
|
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
|
||||||
lastBaroAlt = BaroAlt;
|
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).
|
// 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
|
// 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 = 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 = 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);
|
|
||||||
|
// 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;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue