Altitude Hold improvement solution
Re: Altitude Hold improvement solution
Hi,
In a clean-up action, I updated my code to a 'fresh' download rev 1143, and then made the remaining SVN updates again upto 1152.... I was using a patched up base, from numerous SVN updates, which I was not sure were all correct, but flew perfectly.
However, with the code cleaned up, the alt hold does not seem to work anywhere near as good as it used to do after Mahowik's modifications from a few weeks ago....
I compared the current code with the earlier version, which was working perfectly.I noticed a number of relocations, and slighty different syntax, which seems to do the same.
However: One line seems to be 'reversed'
static float accVelScale = 9.80665f / 10000.0f / acc_1G ;
used to be
static float accVelScale = 9.80665f / acc_1G / 10000.0f;
Was this deliberate ?
I haven't had a change to test fly it yet to see if it makes any difference..... I'll report back once I get a chance....
In a clean-up action, I updated my code to a 'fresh' download rev 1143, and then made the remaining SVN updates again upto 1152.... I was using a patched up base, from numerous SVN updates, which I was not sure were all correct, but flew perfectly.
However, with the code cleaned up, the alt hold does not seem to work anywhere near as good as it used to do after Mahowik's modifications from a few weeks ago....
I compared the current code with the earlier version, which was working perfectly.I noticed a number of relocations, and slighty different syntax, which seems to do the same.
However: One line seems to be 'reversed'
static float accVelScale = 9.80665f / 10000.0f / acc_1G ;
used to be
static float accVelScale = 9.80665f / acc_1G / 10000.0f;
Was this deliberate ?
I haven't had a change to test fly it yet to see if it makes any difference..... I'll report back once I get a chance....
Re: Altitude Hold improvement solution
correct is:
static float accVelScale = 9.80665f / acc_1G / 10000.0f;
static float accVelScale = 9.80665f / acc_1G / 10000.0f;
Re: Altitude Hold improvement solution
x/y/z = x/z/y, but arguments order make some differencies in code size.
Re: Altitude Hold improvement solution
wilco1967 wrote:However: One line seems to be 'reversed'
static float accVelScale = 9.80665f / 10000.0f / acc_1G ;
used to be
static float accVelScale = 9.80665f / acc_1G / 10000.0f;
Checked in GUI for these two cases. Don't worry. Velocity is the same
Some details: 1st case more optimized and arduino compiler (cheked with 1.0.1 version) replace (9.80665f / 10000.0f) to one value. As result you have size of the compiled sketch less than 2nd variant...
BUT for some other compilers it can be an issue, if compiler start to parse from end to begin. E.g. for 2nd case (temp = acc_1G / 10000.0f) will be calculated at first and then 9.80665f / temp. As you see result will be fully incorrect...
((9.80665f / acc_1G) / 10000.0f) != (9.80665f / (acc_1G / 10000.0f))
@timecop: probably it's an issue with your port on stm32...
thx-
Alex
Re: Altitude Hold improvement solution
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)
And this code works perfect..... (with almost all possible PID parameters.... currently 5, 0.020, 30 (default).
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).
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).
Re: Altitude Hold improvement solution
Also could not find the issue here
probably here, when function for deadband was replaced to define
I will check at home...
probably here, when function for deadband was replaced to define
Code: Select all
#define applyDeadband(value, deadband) \
if(abs(value) < deadband) { \
value = 0; \
} else if(value > 0){ \
value -= deadband; \
} else if(value < 0){ \
value += deadband; \
}
I will check at home...
Re: Altitude Hold improvement solution
It seems I found it!
Try to comment applyDeadband(vel, 5)
In original version it was a function for deadband and "vel" variable in not overridden with deadband, but with define it's overridden! As result velocity integrator will re-zero in each iteration...
Try to comment applyDeadband(vel, 5)
Code: Select all
//D
// applyDeadband(vel, 5);
BaroPID -= constrain(conf.D8[PIDALT] * vel / 20, -150, 150);
debug[3] = BaroPID;
In original version it was a function for deadband and "vel" variable in not overridden with deadband, but with define it's overridden! As result velocity integrator will re-zero in each iteration...
Re: Altitude Hold improvement solution
mahowik wrote:Also could not find the issue here
probably here, when function for deadband was replaced to defineCode: Select all
#define applyDeadband(value, deadband) \
if(abs(value) < deadband) { \
value = 0; \
} else if(value > 0){ \
value -= deadband; \
} else if(value < 0){ \
value += deadband; \
}
I will check at home...
Thanks a lot for you help....
Small update:
I replaced the entire IMU.ino with the old one (rest of the code is latest dev), and it seems to work fine again....
(Only tried holding it in my hand, as it is raining outside, but it feels OK).
Re: Altitude Hold improvement solution
mahowik wrote:It seems I found it!
Try to comment applyDeadband(vel, 5)Code: Select all
//D
// applyDeadband(vel, 5);
BaroPID -= constrain(conf.D8[PIDALT] * vel / 20, -150, 150);
debug[3] = BaroPID;
In original version it was a function for deadband and "vel" variable in not overridden with deadband, but with define it's overridden! As result velocity integrator will re-zero in each iteration...
Succes!!!!!
that seems to solve it......
Still raining, so only tested inhouse, holding it.
Thanks a lot again !
-
- Posts: 1630
- Joined: Wed Jan 19, 2011 9:07 pm
Re: Altitude Hold improvement solution
mahowik wrote:It seems I found it!
Try to comment applyDeadband(vel, 5)Code: Select all
//D
// applyDeadband(vel, 5);
BaroPID -= constrain(conf.D8[PIDALT] * vel / 20, -150, 150);
debug[3] = BaroPID;
In original version it was a function for deadband and "vel" variable in not overridden with deadband, but with define it's overridden! As result velocity integrator will re-zero in each iteration...
Hi,
Sorry for the bug.
I've just fixed it this way:
Code: Select all
float vel_tmp = vel;
applyDeadband(vel_tmp, 5);
BaroPID -= constrain(conf.D8[PIDALT] * vel_tmp / 20, -150, 150);
and the code is a little bit shorter, mainly because calc on a static variable is a little bit longer
-
- Posts: 6
- Joined: Tue Sep 18, 2012 3:45 am
Re: Altitude Hold improvement solution
Hey guys,
I had a problem that whenever i activated the alt hold, the copter would drop.
so, debugging, i found my accelerometer was measuring 1g as 180, and the sensor's acc_1g was defined as 165.
so whenever the copter was standing still, the algorithm thought it was rising!
i changed the sensor acc_1g and it works, now! just need some PID tuning.
It's not actually a bug, since the code assumes acc_1g is correct, i'm just posting to let people with the same problem know what is wrong.
Thank you guys for the hard work =D
I had a problem that whenever i activated the alt hold, the copter would drop.
so, debugging, i found my accelerometer was measuring 1g as 180, and the sensor's acc_1g was defined as 165.
so whenever the copter was standing still, the algorithm thought it was rising!
i changed the sensor acc_1g and it works, now! just need some PID tuning.
It's not actually a bug, since the code assumes acc_1g is correct, i'm just posting to let people with the same problem know what is wrong.
Thank you guys for the hard work =D
Re: Altitude Hold improvement solution
We need a startup calibration subroutine for 1g value.
Re: Altitude Hold improvement solution
Is 165 the default in code 2.1?
Also what kind of accel do you have and was the 180 what showed in the GUI?
Just looking for more info so others can check theirs.
Also what kind of accel do you have and was the 180 what showed in the GUI?
Just looking for more info so others can check theirs.
-
- Posts: 6
- Joined: Tue Sep 18, 2012 3:45 am
Re: Altitude Hold improvement solution
I have a gy-80 chinese IMU, with an ADXL345.
Oops. the default value is 265, changed to 278(was in doubt between 278 and 280)
So, i have now checked out the r1155, had to recalibrate, and now the sensor actually reads 265.
I re-uploaded the version where i had this problem, and after calibration it also shows 265. I guess it was just a calibration problem after all.
Anyway, what happened is that when standing still, the debug[2](vel), instead of remaining constant around zero, remained constant at ~10(if i remember correctly). So whenever i activated the alt-hold, the motors would immediately slow down, trying to stop the copter ascension.
I wonder what caused the sensor to read 278....
Oops. the default value is 265, changed to 278(was in doubt between 278 and 280)
So, i have now checked out the r1155, had to recalibrate, and now the sensor actually reads 265.
I re-uploaded the version where i had this problem, and after calibration it also shows 265. I guess it was just a calibration problem after all.
Anyway, what happened is that when standing still, the debug[2](vel), instead of remaining constant around zero, remained constant at ~10(if i remember correctly). So whenever i activated the alt-hold, the motors would immediately slow down, trying to stop the copter ascension.
I wonder what caused the sensor to read 278....
Re: Altitude Hold improvement solution
Can we just make an average of the acc values read at startup, when also giro is calibrated. And then define that average as 1g. G force differs from spot to spot, not much, but maybe enough to have signifficant drift in algorythm.
Re: Altitude Hold improvement solution
No, because then you'd have to have the copter precisely level at power up.
Re: Altitude Hold improvement solution
ACC calibration -> The readout of my acc (ITG3200) is absolutely stable and reproducible over weeks, when the motors are not running.
Re: Altitude Hold improvement solution
ITG3200 is a gyro.
Re: Altitude Hold improvement solution
timecop wrote:ITG3200 is a gyro.
Sry, picked the wrong one... I meant BMA180
Re: Altitude Hold improvement solution
timecop wrote:No, because then you'd have to have the copter precisely level at power up.
Use sqrt(accx*accx + accy*accy + accz*accz) and you wont have to have it level.
A better solution would be to have a similar ACC (and gyro) calibration as with mag calibration where you turn around the copter to find each axis scale factor an bias and optionally any cross axis sensitivity. The MPU6050 is supposed to be factory calibrated in this regard (scale and sensitivity, not bias), but for other sensors this would likely improve things a lot
Re: Altitude Hold improvement solution
Use sqrt(accx*accx + accy*accy + accz*accz) and you wont have to have it level.
I dunno about that, some accels have different 1g values on XY and Z axes. Wouldn't that screw it up?
Re: Altitude Hold improvement solution
timecop wrote:No, because then you'd have to have the copter precisely level at power up.
Good find about startup calibration on a non-level surface.
Solution: We always can apply the per axis angle G corrections as we suppose that level is calibrated and copters stays still on ground. I am no more a software coder, i had my time 10 years ago but some principles are still in my brain.
Re: Altitude Hold improvement solution
timecop wrote:Use sqrt(accx*accx + accy*accy + accz*accz) and you wont have to have it level.
I dunno about that, some accels have different 1g values on XY and Z axes. Wouldn't that screw it up?
If that's the case, then all other algorithms used in MultiWii are not going to work correct, because they assume 1G is 256 (or 512 for MPU6050). One should not use accelerometers like that anymore
Re: Altitude Hold improvement solution
Sebbi wrote:timecop wrote:No, because then you'd have to have the copter precisely level at power up.
Use sqrt(accx*accx + accy*accy + accz*accz) and you wont have to have it level.
A better solution would be to have a similar ACC (and gyro) calibration as with mag calibration where you turn around the copter to find each axis scale factor an bias and optionally any cross axis sensitivity. The MPU6050 is supposed to be factory calibrated in this regard (scale and sensitivity, not bias), but for other sensors this would likely improve things a lot
Alexmos alredy done 3 axis ACC calibration to calculate each axis scale, but as for me it's very boring to do it each time... e.g. in autumn or winter when you need recalibrate ACC outside because of the temperature change inside and outside...
Also you can see in new alt hold solution I have already tried to use "dynamic" acc_1G=1/invG, but as I remember it has an big phase delay during the inclinations...
Code: Select all
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;
so we can use acc_1G calculated, just get it before motors armed... and acc 1G will be correct at least when copter in horizon...
Code: Select all
float invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
static int16_t acc_1G_calculated;
if (!f.ARMED) {
acc_1G_calculated = 1/invG;
}
int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G_calculated;
UPDATE: Tested this in GUI. It takes into account ACC 1G inaccuracy... so make sense to include to current alt-hold implementation.
I.e. if ACC not calibrated enough or has inaccuracy because of the temperature drift or not scaled on Z axis (e.g. range from -496 to +520), it will be taken into account at the time when motors arms and will keep accZ near zero on start...
thx-
Alex
Re: Altitude Hold improvement solution
Hi,
I was sure I posted this question but it seems that I didn't. If it ends up on the board twice I apologise.
I have a problem I am running MultiWii_2_1_b2 on a CRIUS AIO Pro with Serial GPS on S2 at 52600 bds. The whole lot guiding a 2.7 Kg Hex built from Frame Wheel arms and plate. It flies very well indeed when not in Level or Althold. In fact its a little on the docile side. It is a different beast when switched to Level of Althold. It climbs very fast and can only be slowed down by almost closing the throttle, this gives a sever YOYO motion as the Hex Climbs and dives, absolutely uncontrollable. Further to this problem if I disarm, on the ground obviously, then rearm with either or Level and Althold selected the motors will not arm, seems like the fc goes to a high throttle setting when ever Leval or Althold is selected regardless of the actual throttle setting.
Any suggestions other than RTFM welcome. The directive RTFM will be followed If you point me to the correct bit to read
I was sure I posted this question but it seems that I didn't. If it ends up on the board twice I apologise.
I have a problem I am running MultiWii_2_1_b2 on a CRIUS AIO Pro with Serial GPS on S2 at 52600 bds. The whole lot guiding a 2.7 Kg Hex built from Frame Wheel arms and plate. It flies very well indeed when not in Level or Althold. In fact its a little on the docile side. It is a different beast when switched to Level of Althold. It climbs very fast and can only be slowed down by almost closing the throttle, this gives a sever YOYO motion as the Hex Climbs and dives, absolutely uncontrollable. Further to this problem if I disarm, on the ground obviously, then rearm with either or Level and Althold selected the motors will not arm, seems like the fc goes to a high throttle setting when ever Leval or Althold is selected regardless of the actual throttle setting.
Any suggestions other than RTFM welcome. The directive RTFM will be followed If you point me to the correct bit to read
Re: Altitude Hold improvement solution
Loaded old dev r1129 (alt hold bug free) and ALT HOLD now works great again!
Multiwii on ATMEGA328 and BMP085 with PID 3 - 0,020 - 60
Today I've done a couple of flights using throttle only for takeoff and landing!
ALT Hold is great!
Next tests on ATMega 2560 and MS5611 using r1143 modified to remove the bug
Luciano
Multiwii on ATMEGA328 and BMP085 with PID 3 - 0,020 - 60
Today I've done a couple of flights using throttle only for takeoff and landing!
ALT Hold is great!
Next tests on ATMega 2560 and MS5611 using r1143 modified to remove the bug
Luciano
Re: Altitude Hold improvement solution
Here you have a flight with more than 17 min of alt hold and pos hold flying with pushes up and down and sideways. I used Multiwii R1143 on Crius AIOP.
http://www.youtube.com/watch?v=CoCKWUhDqjU
Thank you community for this achievement.
I am looking forward to have an controlled ascend/descent with failsafe landing on baro and use of X-Y acceleration to dampen the pos hold moves.
http://www.youtube.com/watch?v=CoCKWUhDqjU
Thank you community for this achievement.
I am looking forward to have an controlled ascend/descent with failsafe landing on baro and use of X-Y acceleration to dampen the pos hold moves.
Re: Altitude Hold improvement solution
dramida wrote:Here you have a flight with more than 17 min of alt hold and pos hold flying with pushes up and down and sideways. I used Multiwii R1143 on Crius AIOP.
http://www.youtube.com/watch?v=CoCKWUhDqjU
Thank you community for this achievement.
I am looking forward to have an controlled ascend/descent with failsafe landing on baro and use of X-Y acceleration to dampen the pos hold moves.
Dragu, are you using r1443 as is or have you modified imu.ino? With r1443, even with the alexinparis mod, I'm not able to have a good Alt Hold-Pos Hold (CRIUS AIO Pro).
Loaded "old" r1129 on Crius AIO Pro and now I can see an Alt Hold-Pos Hold pretty like your video with pid ALT 2.5-0.010-60
Luciano
Re: Altitude Hold improvement solution
yes, for r1143 you should apply this fix viewtopic.php?f=8&t=2371&start=210#p24276
Re: Altitude Hold improvement solution
Hi mahowik, in r1143 version, there's no additional config options you added in 2.1b2, isn't it? if it's in the different branch, will it be merged to the main trunk?
Re: Altitude Hold improvement solution
guys,
which is a bug free version for mahowik's alt hold for crius se (bmp085)??
MultiWii_2_1_b1?
MultiWii_2_1_b2??
MultiWii_2_1_b2_mega328???
r1129????
r1143 version with Alex fix (viewtopic.php?f=8&t=2371&start=210#p24276)????
can someone make this more clear?? Also i have noticed 2 things:
1) Why IMU_BMP085.ino does not exist after MultiWii_2_1_b2?
2) mahowik uses LPF_42Hz in all MultiWii_2_1_b(x) config.h. Is this optional or a must???
Thanks in advance
which is a bug free version for mahowik's alt hold for crius se (bmp085)??
MultiWii_2_1_b1?
MultiWii_2_1_b2??
MultiWii_2_1_b2_mega328???
r1129????
r1143 version with Alex fix (viewtopic.php?f=8&t=2371&start=210#p24276)????
can someone make this more clear?? Also i have noticed 2 things:
1) Why IMU_BMP085.ino does not exist after MultiWii_2_1_b2?
2) mahowik uses LPF_42Hz in all MultiWii_2_1_b(x) config.h. Is this optional or a must???
Thanks in advance
Re: Altitude Hold improvement solution
if you want to try alt hold you can use any version specified above...
If you can't to get it working, see this viewtopic.php?f=8&t=2371&p=23923#p23819
If you can't to get it working, see this viewtopic.php?f=8&t=2371&p=23923#p23819
Re: Altitude Hold improvement solution
New alt hold routines work great...but you have to be careful not to activate in config.h the new AP_MODE as it deactivates BARO when you move the sticks...and that has driven me mad!!!
If you want to use AP_MODE see viewtopic.php?f=16&t=1944&start=40#p24596
Luciano
If you want to use AP_MODE see viewtopic.php?f=16&t=1944&start=40#p24596
Luciano
Re: Altitude Hold improvement solution
Hi guys,
On these weekend I have fixed some bugs in b2 and made RTH video with ALT_TO_RTH (If Alt-hold activated during the RTH or RTH during Alt-hold, it will keep specified altitude) and ALT_TO_RTH_FINISH (altitude after RTH, i.e. when copter reached to home position).
as I wrote before: but there +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms... I checked with full throttle in GUI (where alt hold activated in middle point) it gives ~2m/s... probably for fast flights it's not enough though...
after set of packs flying I can say that is acceptable solution but nice to have beeper (I haven't) on the board and during the ascend/descend altitude you should hear this
...where with default ascend/descend solution (from 2.1) now it's not possible to control altitude at all, because velocity from new alt hold dampens any change of initial throttle...
fixed this issue...
I'm going to post it with b3 soon...
thx-
Alex
On these weekend I have fixed some bugs in b2 and made RTH video with ALT_TO_RTH (If Alt-hold activated during the RTH or RTH during Alt-hold, it will keep specified altitude) and ALT_TO_RTH_FINISH (altitude after RTH, i.e. when copter reached to home position).
dramida wrote:2- Altitude control (up and down) is to "soft" , i crashed into ground two times and it tends to lose altitude after a controled climb (verry slow). 0.5 m/s vertical speed is too low.
as I wrote before: but there +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms... I checked with full throttle in GUI (where alt hold activated in middle point) it gives ~2m/s... probably for fast flights it's not enough though...
after set of packs flying I can say that is acceptable solution but nice to have beeper (I haven't) on the board and during the ascend/descend altitude you should hear this
...where with default ascend/descend solution (from 2.1) now it's not possible to control altitude at all, because velocity from new alt hold dampens any change of initial throttle...
dramida wrote: Also i noticed that in altitude control mode, if you put the stick to minimum, the control is lost and immediat crash happens as motors enters in IDLE mode.
fixed this issue...
I'm going to post it with b3 soon...
thx-
Alex
Re: Altitude Hold improvement solution
I think that calibrating at startup 1G acceleration is a must for successful repeatable behavior of all functions using accelerometer.(temperature variations, fabrications marginal errors, even altitude and latitude does count) Please consider a non leveled surface at startup.
Re: Altitude Hold improvement solution
mahowik, I tested your baro code (the part that is in dev _shared) today and made some observations (using default alt PIDs of 5.0 0.020 30):
1) as soon as I activate alt hold the copter drops 1-2 meters
2) it did hold the altitude somewhat by not going below that initial drop, but gained altitude quiet often and than almost killed the throttle to get back to the target altitude (a sign of wrong PIDs I guess)
3) even though you use the time since the last time the code ran it is highly timing sensitive. I changed the frequency of the function call from 40 Hz to 10 Hz to test this and the copter dropped from the sky ... i didn't test by how much it dropped (came down from about 4 meters high), but acc_z certainly didn't help here ...
Am I doing something wrong?
I checked the baro readings from my smartphone and it looked like the copter "knew" it was climbing/descending (change of altitude in GUI). I will redo the test and log the data to confirm.
1) as soon as I activate alt hold the copter drops 1-2 meters
2) it did hold the altitude somewhat by not going below that initial drop, but gained altitude quiet often and than almost killed the throttle to get back to the target altitude (a sign of wrong PIDs I guess)
3) even though you use the time since the last time the code ran it is highly timing sensitive. I changed the frequency of the function call from 40 Hz to 10 Hz to test this and the copter dropped from the sky ... i didn't test by how much it dropped (came down from about 4 meters high), but acc_z certainly didn't help here ...
Am I doing something wrong?
I checked the baro readings from my smartphone and it looked like the copter "knew" it was climbing/descending (change of altitude in GUI). I will redo the test and log the data to confirm.
Re: Altitude Hold improvement solution
Sebbi wrote:mahowik, I tested your baro code (the part that is in dev _shared) today and made some observations (using default alt PIDs of 5.0 0.020 30):
1) as soon as I activate alt hold the copter drops 1-2 meters
2) it did hold the altitude somewhat by not going below that initial drop, but gained altitude quiet often and than almost killed the throttle to get back to the target altitude (a sign of wrong PIDs I guess)
3) even though you use the time since the last time the code ran it is highly timing sensitive. I changed the frequency of the function call from 40 Hz to 10 Hz to test this and the copter dropped from the sky ... i didn't test by how much it dropped (came down from about 4 meters high), but acc_z certainly didn't help here ...
Am I doing something wrong?
I checked the baro readings from my smartphone and it looked like the copter "knew" it was climbing/descending (change of altitude in GUI). I will redo the test and log the data to confirm.
Hi Sebbi,
1) yes, in case you activate althold during up/down movement it can change the alt hold point slightly, because EstAlt variable has some inertion because of LPFs there...
2) not sure that I understood you correctly (I'm far from native speaker ), but if you mean that is not possible to change altitude by using throttle stick, yes as I specified here "where with default ascend/descend solution (from 2.1) now it's not possible to control altitude at all, because velocity from new alt hold dampens any change of initial throttle..."
In b2 I'm using other solution to change altitude on althold:
Code: Select all
if (abs(rcCommand[THROTTLE]-initialThrottleHold) > ALT_HOLD_THROTTLE_NEUTRAL_ZONE) {
// Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
AltHoldCorr+= rcCommand[THROTTLE] - initialThrottleHold;
if(abs(AltHoldCorr) > 500) {
AltHold+= AltHoldCorr/500;
AltHoldCorr%= 500;
}
#if defined(BUZZER)
buzzerFreq = 1; // beep buzzer 1Hz
#endif
errorAltitudeI = 0;
}
3) yes. it's because mwii uses dependent on cycle time LPFs (for best performance). If you want time independent code just use LPF with specified cut frequency, e.g. from GPS.ino
Code: Select all
/// Low pass filter cut frequency for derivative calculation.
///
static const float _filter = 7.9577e-3; // Set to "1 / ( 2 * PI * f_cut )";
// Examples for _filter:
// f_cut = 10 Hz -> _filter = 15.9155e-3
// f_cut = 15 Hz -> _filter = 10.6103e-3
// f_cut = 20 Hz -> _filter = 7.9577e-3
// f_cut = 25 Hz -> _filter = 6.3662e-3
// f_cut = 30 Hz -> _filter = 5.3052e-3
.......
_derivative = _last_derivative + (*dt / ( _filter + *dt)) * (_derivative - _last_derivative);
thx-
Alex
Re: Altitude Hold improvement solution
mahowik wrote:I'm going to post it with b3 soon...
Hi guys,
Here is b3...
Also RTH video with ALT_TO_RTH (If Alt-hold activated during the RTH or RTH during Alt-hold, it will keep specified altitude... =10m in config) and ALT_TO_RTH_FINISH (altitude after RTH, i.e. when copter reached to home position... =3m in config).
https://www.youtube.com/watch?v=3gDocCKRQCU
thx-
Alex
Last edited by mahowik on Wed Dec 12, 2012 9:33 pm, edited 1 time in total.
Re: Altitude Hold improvement solution
Hi Alex,
1) could very well be, have to confirm is EstAlt was lagging behind ... but the copter was hovering for a few seconds before I activated alt hold. Will try to log that behaviour tomorrow to confirm.
2) and yeah, i noticed in another flight that the code counteracts whatever I do with the trottle, but that's not what i meant (not a native speaker either). I didn't touch the throttle stick, it was rising/cutting throttle on its own. Maybe a result of my changed baro sensor code and certain lag in EstAlt update rate (there is additional LPF there)
3) I understand. That should probably be mentioned somewhere and maybe it would be good to split getEstimatedAltitude into 2 functions, one for actual altitude estimation and one that calculates the PIDs at a much faster rate (because it now relies on the acc values). What's your take on that?
1) could very well be, have to confirm is EstAlt was lagging behind ... but the copter was hovering for a few seconds before I activated alt hold. Will try to log that behaviour tomorrow to confirm.
2) and yeah, i noticed in another flight that the code counteracts whatever I do with the trottle, but that's not what i meant (not a native speaker either). I didn't touch the throttle stick, it was rising/cutting throttle on its own. Maybe a result of my changed baro sensor code and certain lag in EstAlt update rate (there is additional LPF there)
3) I understand. That should probably be mentioned somewhere and maybe it would be good to split getEstimatedAltitude into 2 functions, one for actual altitude estimation and one that calculates the PIDs at a much faster rate (because it now relies on the acc values). What's your take on that?
Re: Altitude Hold improvement solution
Here is a video from the drop and a few minutes of altitude hold with only pitch/roll correction from my side: http://www.youtube.com/watch?v=i0dX4NYVpk4
Re: Altitude Hold improvement solution
Tested today GPS HOLD & ALT HOLD using r1129 with AP_MODE BARO bug corrected and I shoud say that now the altitude and position control of the multicopter is really good. I'd like to thank again all the developers of these functions. Can't wait now to test new ALT TO RTH routines ...but I have to wait for a version integrated in the new dev as I can't use 2.1 (2.1 does not support GPS auto config)
Here a short video of my last happy flight ...hands off from the RC transmitter! http://www.youtube.com/watch?v=T3htaJ53Z7E
Luciano
Here a short video of my last happy flight ...hands off from the RC transmitter! http://www.youtube.com/watch?v=T3htaJ53Z7E
Luciano
Re: Altitude Hold improvement solution
does it hold well with more winters?
Re: Altitude Hold improvement solution
Sebbi wrote:Hi Alex,
1) could very well be, have to confirm is EstAlt was lagging behind ... but the copter was hovering for a few seconds before I activated alt hold. Will try to log that behaviour tomorrow to confirm.
2) and yeah, i noticed in another flight that the code counteracts whatever I do with the trottle, but that's not what i meant (not a native speaker either). I didn't touch the throttle stick, it was rising/cutting throttle on its own. Maybe a result of my changed baro sensor code and certain lag in EstAlt update rate (there is additional LPF there)
3) I understand. That should probably be mentioned somewhere and maybe it would be good to split getEstimatedAltitude into 2 functions, one for actual altitude estimation and one that calculates the PIDs at a much faster rate (because it now relies on the acc values). What's your take on that?
1) probably it's related to your changes... make sense to debug/profile from IN to OUT...
also have an idea to integrate the calculated velocity (and keep it by applying BaroAlt via CF) to get EstAlt w/o (or less) delay... BUT more than one integrator from noisy input data it's double chance to lost control of stable routine
2) yes, if EstAlt has another lag/delay, baro velocity (baroVel) is also "other"and it means that you need at least to re-tune vel/baroVel CF... also you can check vel/baroVel vars in GUI and baroVel should approximately repeat vel, during no so fast vertical movements BUT of course with delay...
3) agree, it make sense to split alt estimator and pid controller but I was lazy with this and just applied to 2.1 code... Also you know some kind of Old school IT practice: if it works good just don't touch it!
thx-
Alex
Re: Altitude Hold improvement solution
Hi ......
Is there any issue about copter going to up, when navigating like RTH ?, like speed nav to fast or any effect from ALT_HOLD ?
Is there any issue about copter going to up, when navigating like RTH ?, like speed nav to fast or any effect from ALT_HOLD ?
Re: Altitude Hold improvement solution
Sebbi.
I think, but not sure that altitude drop after switch althold on can be caused by 1G constants difference from real 1G reading.
I make some code changes for runtime 1G measuring. Measuring is done within 1 second after gyro calibration ends.
Additionally i do one more modification for gyro calibration. If the copter is moving during gyro calibration, the procedure is stopped until gyro readings is stable. From now no false gyro calibrations if we move the copter during power-up.
Here is the changes.
Sensors.ino:
IMU.ino:
This is only tested on the desk, not during flight.
I think, but not sure that altitude drop after switch althold on can be caused by 1G constants difference from real 1G reading.
I make some code changes for runtime 1G measuring. Measuring is done within 1 second after gyro calibration ends.
Additionally i do one more modification for gyro calibration. If the copter is moving during gyro calibration, the procedure is stopped until gyro readings is stable. From now no false gyro calibrations if we move the copter during power-up.
Here is the changes.
Sensors.ino:
Code: Select all
void GYRO_Common() {
static int16_t previousGyroADC[3] = {0,0,0};
static int32_t g[3];
uint8_t axis, tilt=0;
#if defined MMGYRO
// Moving Average Gyros by Magnetron1
//---------------------------------------------------
static int16_t mediaMobileGyroADC[3][MMGYROVECTORLENGHT];
static int32_t mediaMobileGyroADCSum[3];
static uint8_t mediaMobileGyroIDX;
//---------------------------------------------------
#endif
if (calibratingG>0) {
for (axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (calibratingG == 400) {
g[axis]=0;
previousGyroADC[axis] = gyroADC[axis];
}
if (calibratingG % 10 == 0) {
if(abs(gyroADC[axis] - previousGyroADC[axis]) > 8) tilt=1;
previousGyroADC[axis] = gyroADC[axis];
}
// Sum up 400 readings
g[axis] +=gyroADC[axis];
// Clear global variables for next reading
gyroADC[axis]=0;
gyroZero[axis]=0;
if (calibratingG == 1) {
gyroZero[axis]=g[axis]/400;
blinkLED(10,15,1);
}
}
if(tilt) {
calibratingG=1000;
LEDPIN_ON;
} else {
calibratingG--;
LEDPIN_OFF;
}
return;
}
...
IMU.ino:
Code: Select all
...
// projection of ACC vector to global Z
// Math: accZ = A * G / |G|
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;
// int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
// runtime measuring 1G after any gyro calibration
static int16_t measured1G = 8*acc_1G; // filter init
static uint8_t count;
if(calibratingG) count=40; // restart measuring after gyro calibration
if(f.ARMED) count=0; // stop measuring if armed
if(!calibratingG && count) { // within 1 sec after end of gyro calibration (sure that is stable)
measured1G -= measured1G/8;
measured1G += accZ; // filter ACC readings
accZ = 0;
count--;
} else {
accZ -= measured1G/8; // substraction of measured 1G from calculated accZ
}
applyDeadband(accZ, ACC_Z_DEADBAND);
...
This is only tested on the desk, not during flight.
Re: Altitude Hold improvement solution
Hi Mahowik,
I just try alt hold on B3 version. i have problem when the alt hold is on and i tried to move the nick and roll stick and the quad become very hard to control and my quad crash, broke one of prop.
I tried B2 version exactly same behavior as Sebbi's video.
Tomorrow i am going to try again the B3 with different PID setting.
Thanks,
John
I just try alt hold on B3 version. i have problem when the alt hold is on and i tried to move the nick and roll stick and the quad become very hard to control and my quad crash, broke one of prop.
I tried B2 version exactly same behavior as Sebbi's video.
Tomorrow i am going to try again the B3 with different PID setting.
Thanks,
John
Re: Altitude Hold improvement solution
Good move Mis. I'll test your new calibration routine. On what MWC release should i make changes? B3 from Mahowik or some main trunk dev?
/Mihai
/Mihai
Mis wrote:Sebbi.
I think, but not sure that altitude drop after switch althold on can be caused by 1G constants difference from real 1G reading.
I make some code changes for runtime 1G measuring. Measuring is done within 1 second after gyro calibration ends.
Additionally i do one more modification for gyro calibration. If the copter is moving during gyro calibration, the procedure is stopped until gyro readings is stable. From now no false gyro calibrations if we move the copter during power-up.
Here is the changes.
Sensors.ino:Code: Select all
void GYRO_Common() {
static int16_t previousGyroADC[3] = {0,0,0};
static int32_t g[3];
uint8_t axis, tilt=0;
#if defined MMGYRO
// Moving Average Gyros by Magnetron1
//---------------------------------------------------
static int16_t mediaMobileGyroADC[3][MMGYROVECTORLENGHT];
static int32_t mediaMobileGyroADCSum[3];
static uint8_t mediaMobileGyroIDX;
//---------------------------------------------------
#endif
if (calibratingG>0) {
for (axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (calibratingG == 400) {
g[axis]=0;
previousGyroADC[axis] = gyroADC[axis];
}
if (calibratingG % 10 == 0) {
if(abs(gyroADC[axis] - previousGyroADC[axis]) > 8) tilt=1;
previousGyroADC[axis] = gyroADC[axis];
}
// Sum up 400 readings
g[axis] +=gyroADC[axis];
// Clear global variables for next reading
gyroADC[axis]=0;
gyroZero[axis]=0;
if (calibratingG == 1) {
gyroZero[axis]=g[axis]/400;
blinkLED(10,15,1);
}
}
if(tilt) {
calibratingG=1000;
LEDPIN_ON;
} else {
calibratingG--;
LEDPIN_OFF;
}
return;
}
...
IMU.ino:Code: Select all
...
// projection of ACC vector to global Z
// Math: accZ = A * G / |G|
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;
// int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
// runtime measuring 1G after any gyro calibration
static int16_t measured1G = 8*acc_1G; // filter init
static uint8_t count;
if(calibratingG) count=40; // restart measuring after gyro calibration
if(f.ARMED) count=0; // stop measuring if armed
if(!calibratingG && count) { // within 1 sec after end of gyro calibration (sure that is stable)
measured1G -= measured1G/8;
measured1G += accZ; // filter ACC readings
accZ = 0;
count--;
} else {
accZ -= measured1G/8; // substraction of measured 1G from calculated accZ
}
applyDeadband(accZ, ACC_Z_DEADBAND);
...
This is only tested on the desk, not during flight.
Re: Altitude Hold improvement solution
I have acces to repository, but before I commit this idea, i need some positive feedback.
Too many my ideas have been nuked by other programmers.
Too many my ideas have been nuked by other programmers.
Re: Altitude Hold improvement solution
flyrobot wrote:Hi Mahowik,
I just try alt hold on B3 version. i have problem when the alt hold is on and i tried to move the nick and roll stick and the quad become very hard to control and my quad crash, broke one of prop.
I tried B2 version exactly same behavior as Sebbi's video.
Tomorrow i am going to try again the B3 with different PID setting.
Thanks,
John
It seems the problem above is just the stock PID setting need tobe changed. This morning i tried again, have one problem.
When the alt hold switch active the pos hold is not active. Alt hold is better but still the drop 50-1 meter after the alt hold activated.
John