Hi guys!
This weekend I became restless again that we are worse than others!
I have already tried and spent too much time 6-8 month ago without real success, but today I have collected all my knowledge again and made second attempt.
In two words! Now I can say "I got real results with baro+acc Alt Hold!"
http://www.youtube.com/watch?v=nePjkz_leR8It's:
- take into acount inclinations
- doesn't see horizontal accelerations
- keeps altitude on the wind (tried with 15-20km/h)
- has very good reaction for pushing
- has accuracy +/-20..40cm in calm weather according to baro accuracy
- has accuracy about +/-1m during the flights... flying 4 packs with initial PIDs without any tuning because I was too satisfied with results
Code: Select all
#define ACC_LPF_FOR_VELOCITY 10
static float accLPFVel[3];
static t_fp_vector EstG;
void getEstimatedAttitude(){
.....................................
.....................................
#if defined(ACC_LPF_FACTOR)
accLPF[axis] = accLPF[axis] * (1.0f - (1.0f/ACC_LPF_FACTOR)) + accADC[axis] * (1.0f/ACC_LPF_FACTOR);
accLPFVel[axis] = accLPFVel[axis] * (1.0f - (1.0f/ACC_LPF_FOR_VELOCITY)) + accADC[axis] * (1.0f/ACC_LPF_FOR_VELOCITY);
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 40
#define ACC_Z_DEADBAND (acc_1G/50)
void getEstimatedAltitude(){
uint8_t index;
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];
index = (BaroHistIdx + (BARO_TAB_SIZE/2))%BARO_TAB_SIZE;
BaroHigh -= BaroHistTab[index];
BaroHistIdx++;
if (BaroHistIdx == BARO_TAB_SIZE) BaroHistIdx = 0;
//EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
EstAlt = EstAlt*0.6f + (BaroHigh*10.0f/(BARO_TAB_SIZE/2))*0.4f; // additional LPF to reduce baro noise
//P
int16_t error = applyDeadband16(AltHold - EstAlt, 10); //remove small P parametr to reduce noise near zero position
BaroPID = constrain((conf.P8[PIDALT] * error / 100), -100, +100);
//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 = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - acc_1G;
//int16_t accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - 1/invG;
int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
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 near zero caluculated velocity based on baro velocity
vel = vel * 0.985f + baroVel * 0.015f;
//vel = constrain(vel, -300, 300); // constrain velocity +/- 300cm/s
debug[2] = vel;
//debug[3] = applyDeadbandFloat(vel, 5);
//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;}
How it works:
1. new array for acc values (accLPFvel) was created to prepare specialy filtered ACC data for alt hold, where fast acceleration still visible but not noise/vibrations. Because accADC is too noisy raw data even with MPU6050_LPF_42HZ filter, but accLPF has too big delay and shift of phase...
2. was used BaroHistTab and additional LPF to reduce baro noise (LPF too soft to doesn't lost the phase, i.e. to avoid increase of the delay )
3. then "P" calculated based on baro with applied deadband (=10) to reduce noise near zero position
4. then "I" calculated as before... nothing special here
5. accZ calculted as projection of ACC vector to global Z, with 1G subtructed (accZ = A * G / |G| - 1G) where (EstG vector taken from IMU). And again apply deababnd for 2% of acc_1G (acc_1G/50).
6. then vertical velocity calculated from accZ in integrator (cm/sec)
- and here we got MAIN issue: HOW TO KEEP VELOCITY NEAR ZERO BY USING INTEGRATOR?!
...answers in points below
7. baro velocity calculated from filtered baro values, BUT it still very noisy... so applying of deadband (=10) little bit solve the problem
8. !!! then we apply Complimentary Filter (CF) 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
9. So then calculated velocity from CF (with applyed deadband = 5) used for D
Code is still required some performance optimizations (e.g. replace float calculations to integer where it's possible) but in general it's good I think...
In debug you can see:
- debug[0] - accZ
- debug[1] - baro velocity
- debug[2] - velocity after CF (velocity from acc corrected by baro velocity through the CF)
- debug[3] - sum of the P+I+D
I tried PIDs
5.0-0.030-30 from first attempt and was satisfied with 4 packs of flying... so I hadn't tried to tune that, but it make sense
Note: I'm used mpu6050+ms5611 (AIO from rctimer) with PCB just screwed on the board, BUT i'm not sure about other accelerometers like ADXL345 (too sensitive to the noise) or bma020 with low 1G resolution (64 only) for 8G range... bma180 should be good I suppose.
thx-
Alex
, I decrease some steps downto 22, it looks fine. But I couldnot continue due to late evening and the black cloud was going to cover my head. I'll continue to tune tomorrow with your instructions.
( 
.