just tried it with
accVelScale = 9.80665f / acc_1G / 10000.0f;
still the same...... hardly able to hold altitude..... as bad as the old code (before your modifications)....
It DOES hold, just very bad..... PID tuning does not seem to make it any better.... had to reduce Alt_P to below 1 to stop it from yoyo-ing.... it will drop like 10 meters or more... Alt D tried everyting from 10 to 150...
This is the code I'm running (in the latest dev from SVN (1152), and which is
not performing well. (it seems to do everything else just fine)
Code: Select all
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY 4000000 // 4 sec initialization delay
#define BARO_TAB_SIZE 21
#define ACC_Z_DEADBAND (acc_1G/50)
#define applyDeadband(value, deadband) \
if(abs(value) < deadband) { \
value = 0; \
} else if(value > 0){ \
value -= deadband; \
} else if(value < 0){ \
value += deadband; \
}
void getEstimatedAltitude(){
static uint32_t deadLine = INIT_DELAY;
static int16_t baroHistTab[BARO_TAB_SIZE];
static int8_t baroHistIdx;
static int32_t baroHigh;
if (abs(currentTime - deadLine) < UPDATE_INTERVAL) return;
uint16_t dTime = currentTime - deadLine;
deadLine = currentTime;
//**** Alt. Set Point stabilization PID ****
baroHistTab[baroHistIdx] = BaroAlt/10;
baroHigh += baroHistTab[baroHistIdx];
baroHigh -= baroHistTab[(baroHistIdx + 1)%BARO_TAB_SIZE];
baroHistIdx++;
if (baroHistIdx == BARO_TAB_SIZE) baroHistIdx = 0;
//EstAlt = baroHigh*10/(BARO_TAB_SIZE-1);
EstAlt = EstAlt*0.6f + (baroHigh*10.0f/(BARO_TAB_SIZE - 1))*0.4f; // additional LPF to reduce baro noise
#ifndef SUPPRESS_BARO_ALTHOLD
//P
int16_t error = constrain(AltHold - EstAlt, -300, 300);
applyDeadband(error, 10); //remove small P parametr to reduce noise near zero position
BaroPID = constrain((conf.P8[PIDALT] * error / 100), -150, +150);
//I
errorAltitudeI += error * conf.I8[PIDALT]/50;
errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
BaroPID += (errorAltitudeI / 500); //I in range +/-60
// projection of ACC vector to global Z, with 1G subtructed
// Math: accZ = A * G / |G| - 1G
float invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
//int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - 1/invG;
applyDeadband(accZ, ACC_Z_DEADBAND);
debug[0] = accZ;
static float vel = 0.0f;
//static float accVelScale = 9.80665f / 10000.0f / acc_1G ;
static float accVelScale = 9.80665f / acc_1G / 10000.0f; //wilco
// Integrator - velocity, cm/sec
vel+= accZ * accVelScale * dTime;
static int32_t lastBaroAlt;
float baroVel = (EstAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = EstAlt;
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
applyDeadband(baroVel, 10); // to reduce noise near zero
debug[1] = baroVel;
// 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 * 0.985f + baroVel * 0.015f;
//vel = constrain(vel, -300, 300); // constrain velocity +/- 300cm/s
debug[2] = vel;
//D
applyDeadband(vel, 5);
BaroPID -= constrain(conf.D8[PIDALT] * vel / 20, -150, 150);
debug[3] = BaroPID;
#endif
}
And this code works perfect..... (with almost all possible PID parameters.... currently 5, 0.020, 30 (default).
Code: Select all
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY 4000000 // 4 sec initialization delay
#define BARO_TAB_SIZE 21
#define ACC_Z_DEADBAND (acc_1G/50)
void getEstimatedAltitude(){
static uint32_t deadLine = INIT_DELAY;
static int16_t baroHistTab[BARO_TAB_SIZE];
static int8_t baroHistIdx;
static int32_t baroHigh;
if (abs(currentTime - deadLine) < UPDATE_INTERVAL) return;
uint16_t dTime = currentTime - deadLine;
deadLine = currentTime;
//**** Alt. Set Point stabilization PID ****
baroHistTab[baroHistIdx] = BaroAlt/10;
baroHigh += baroHistTab[baroHistIdx];
baroHigh -= baroHistTab[(baroHistIdx + 1)%BARO_TAB_SIZE];
baroHistIdx++;
if (baroHistIdx == BARO_TAB_SIZE) baroHistIdx = 0;
//EstAlt = baroHigh*10/(BARO_TAB_SIZE-1);
EstAlt = EstAlt*0.6f + (baroHigh*10.0f/(BARO_TAB_SIZE - 1))*0.4f; // additional LPF to reduce baro noise
//P
int16_t error = constrain(AltHold - EstAlt, -300, 300);
error = applyDeadband16(error, 10); //remove small P parametr to reduce noise near zero position
BaroPID = constrain((conf.P8[PIDALT] * error / 100), -150, +150);
//I
errorAltitudeI += error * conf.I8[PIDALT]/50;
errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
BaroPID += (errorAltitudeI / 500); //I in range +/-60
// projection of ACC vector to global Z, with 1G subtructed
// Math: accZ = A * G / |G| - 1G
float invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
//int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - 1/invG;
accZ = applyDeadband16(accZ, ACC_Z_DEADBAND);
debug[0] = accZ;
static float vel = 0.0f;
static float accVelScale = 9.80665f / acc_1G / 10000.0f;
// Integrator - velocity, cm/sec
vel+= accZ * accVelScale * dTime;
static int32_t lastBaroAlt = EstAlt;
float baroVel = (EstAlt - lastBaroAlt) / (dTime/1000000.0f);
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
baroVel = applyDeadbandFloat(baroVel, 10); // to reduce noise near zero
lastBaroAlt = EstAlt;
debug[1] = baroVel;
// 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 * 0.985f + baroVel * 0.015f;
//vel = constrain(vel, -300, 300); // constrain velocity +/- 300cm/s
debug[2] = vel;
//D
BaroPID -= constrain(conf.D8[PIDALT] * applyDeadbandFloat(vel, 5) / 20, -150, 150);
debug[3] = BaroPID;
}
int16_t applyDeadband16(int16_t value, int16_t deadband) {
if(abs(value) < deadband) {
value = 0;
} else if(value > 0){
value -= deadband;
} else if(value < 0){
value += deadband;
}
return value;
}
float applyDeadbandFloat(float value, int16_t deadband) {
if(abs(value) < deadband) {
value = 0;
} else if(value > 0){
value -= deadband;
} else if(value < 0){
value += deadband;
}
return value;
}
float InvSqrt (float x){
union{
int32_t i;
float f;
} conv;
conv.f = x;
conv.i = 0x5f3759df - (conv.i >> 1);
return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);
}
int32_t isq(int32_t x){return x * x;}
Any idea where it really does something different (there is a lot of changes where the code was re-written, but to my untrained eye, seems to do the same).
go to the point one to balance your motors and props 
