Baseflight improved althold

crazyal · Post by **crazyal** » Fri Aug 30, 2013 4:22 pm

I recently adjusted the althold code on baseflight to improve the altitude hold function, as it wasn't working.
Anybody with a naze32 is welcome to try it out. Only the Acc_type has to be changed to MPU-6050. The default config options work pretty well for me and code change is allready in Timecops svn

Here are some vids I made with my Quad

http://www.youtube.com/watch?v=1_AiJnSMhmQ
http://www.youtube.com/watch?v=eMIJNSV96j4

timecop · Post by **timecop** » Sat Aug 31, 2013 2:57 am

The hex in today's svn includes all the changes from this, so careful testing can commence by those who can't build their own firmware. Defaults should be OK to try.

Torque66 · Post by **Torque66** » Sat Aug 31, 2013 3:01 pm

Hey thx... just mounted a gps on my Tarot 650 and fiddling with alt and pos hold....

Torque66 · Post by **Torque66** » Sun Sep 01, 2013 1:14 am

No acc_type in CLI so where is the option?

timecop · Post by **timecop** » Sun Sep 01, 2013 1:22 am

acc_hardware or whatever.

crazyal · Post by **crazyal** » Sun Sep 01, 2013 1:37 am

right, you have to set acc_hardware=2 for MPU-6050

brm · Post by **brm** » Sun Sep 01, 2013 2:08 pm

crazyal wrote:right, you have to set acc_hardware=2 for MPU-6050

i gave it a try.
no Tuning at all and i have bumps of 2 meters.
maybe you can present your pid Settings and the motors used.

i went through some tests - Need to apply the results to the code.
might be also interesting

crazyal · Post by **crazyal** » Sun Sep 01, 2013 2:23 pm

2m doesn't sound too bad. Do you get oscillations or does it just slowly drift within 2m?
For tuning you could play with the baro_cf parameters and the pid settings. If you get oscillations it might be a vibration issue and you could try increasing the deadband. With strong wind I noticed, that the motors are twitching a bit, but the Althold still seems pretty stable.

My PID settings are the defaults and I use Sunnysky x2212 motors with 1045 props on 3s with a ~900g auw frame.

The changes I made are based on one of the code sniplets you dumped a while ago

If you have any ideas or improvements that would be great

Probably the next step to improve althold further would be to integrate some form of autotuning.

brm · Post by **brm** » Sun Sep 01, 2013 2:35 pm

yes, there are more interesting findings.
i am downsampling acc measurements down to 20 hz.
These samples are amazing stable.
can be the base for a new/optimized calculation.

anyway - smoothing the acc values is ok.
but you might use a pt1 element before using the accZ value in the ned Frame.
the cutoff frequency is 20hz.
this will dampen the acc readings.

my bumps are quite slow.
but are clearly osiclations - no slow drift.

i have some vibrations with my frame - which is normal for me because i cannot fly ...
will add the pt1 element and drop the code snippet for further testing

crazyal · Post by **crazyal** » Sun Sep 01, 2013 2:45 pm

yeah oscillations are probably an issue then with acc integration.
I'm looking forward to your results

brm · Post by **brm** » Sun Sep 01, 2013 3:36 pm

flight tested,
the bumps are smaller than 1 meter.
what i would expect - and still without Tuning.

Code: Select all

#define   F_CUT_ACCZ     20.0f
static const float fc_acc = 0.5f / (M_PI * F_CUT_ACCZ);

int getEstimatedAltitude(void)
{
   static float dAccZ = 0.0f;

you see what i added as variables for alt. hold.
now:

Code: Select all

    // pt1 element for accZ in the ned frame
    float dt = dTime * 1e-6;
    dAccZ = dAccZ + (dt / (fc_acc + dt)) * (accSum[2] - dAccZ);
    accSum[2] = dAccZ;

    // Integrator - velocity, cm/sec

the last comment shows where to add the calculation.
as i said - better then the prev. code with my bumpy quad.

there is a 3d integrator in the work without baro.
not just there where i would be ...
maybe i post it on April 1st on the openpilot Forum

Torque66 · Post by **Torque66** » Mon Sep 02, 2013 7:06 am

Meh.... didn't see much of difference to be honest.... maybe a slightly better alt lock..... at least it didn't sledgehammer....

crazyal · Post by **crazyal** » Mon Sep 02, 2013 12:46 pm

Torque66 wrote:Meh.... didn't see much of difference to be honest.... maybe a slightly better alt lock..... at least it didn't sledgehammer....

Your post is about as helpful as saying nothing

How about you tell me what your setup is? any vibration issues ? did you cover your baro with some foam ?
Please note that there are a few tuneable parameters which should be adapted, as every setup differs.
Such as baro_cf_vel and baro_cf_alt which determines how much weight is put on the acc readings for velocity/altitude estimation.
Also the althold PID settings will probably be different if tuned correctly.

@brm
I didn't have time to test your changes yet, but will do so soon.
Instead of using deltaT as a Timebase for your filter I would suggest using accTimeSum as this value is closer to the time of the actual acc readings.

brm · Post by **brm** » Mon Sep 02, 2013 6:34 pm

Torque66 wrote:Meh.... didn't see much of difference to be honest.... maybe a slightly better alt lock..... at least it didn't sledgehammer....

hovering in generel means no fast flying, isn't it?
the pt1 element dampens the acc changes when vibrations are present.
a sledgehammer would be a programming bug.

brm · Post by **brm** » Mon Sep 02, 2013 6:44 pm

crazyal wrote:
Torque66 wrote:Meh.... didn't see much of difference to be honest.... maybe a slightly better alt lock..... at least it didn't sledgehammer....

Your post is about as helpful as saying nothing
How about you tell me what your setup is? any vibration issues ? did you cover your baro with some foam ?
Please note that there are a few tuneable parameters which should be adapted, as every setup differs.
Such as baro_cf_vel and baro_cf_alt which determines how much weight is put on the acc readings for velocity/altitude estimation.
Also the althold PID settings will probably be different if tuned correctly.

@brm
I didn't have time to test your changes yet, but will do so soon.
Instead of using deltaT as a Timebase for your filter I would suggest using accTimeSum as this value is closer to the time of the actual acc readings.

the accelsum time should be the Delta (dTime = currentT - previousT;)
it differs a bit because of the consumed cpu cycles until getEstimatedAltitude() is being called.
but in essence it is the same time slice.
if not, then it is bad Software.

crazyal · Post by **crazyal** » Mon Sep 02, 2013 8:00 pm

sure, in the end the time should basically be the same. I just think it's a better practice to take the time when the value is actually updated and not with some random small delay in between. After the Acc is read multiwii goes trough the whole code in the main loop until it reaches getestimated Altitude.
And yes multiwii isn't good software, but it works

brm · Post by **brm** » Mon Sep 02, 2013 8:30 pm

crazyal wrote:sure, in the end the time should basically be the same. I just think it's a better practice to take the time when the value is actually updated and not with some random small delay in between. After the Acc is read multiwii goes trough the whole code in the main loop until it reaches getestimated Altitude.
And yes multiwii isn't good software, but it works

you CANNOT cut out a timeslice on the time axe.
it is a continous System.
maybe you should make a drawing.
and then you calculate the area for each time slice.

and there is no 'white' area between the time slices

in the end you compensite this 'error' with the p part of the Controller.
the more you have to compenstate the worser the System gets. :idea:

brm · Post by **brm** » Mon Sep 02, 2013 8:39 pm

crazyal wrote:sure, in the end the time should basically be the same. I just think it's a better practice to take the time when the value is actually updated and not with some random small delay in between. After the Acc is read multiwii goes trough the whole code in the main loop until it reaches getestimated Altitude.
And yes multiwii isn't good software, but it works

Code: Select all

#if 1
    debug[0] = accSum[2] / accSumCount; // acceleration
    //debug[1] = vel;                     // velocity
    //debug[2] = accAlt;                  // height
    debug[1] = accAlt;                  // height
    debug[2] = accTimeSum;
    debug[3] = dTime;
#endif

pls. explain the difference between debug[2] and debug[3] when running

i see no difference which should be taken into account.

crazyal · Post by **crazyal** » Tue Sep 03, 2013 9:12 pm

I just said it's better practice imho, not that it makes a noticeable differnce in the result.

Just think about a normal adc, there dt is 1/samplefreq and not the time between your calculations.
In my opinion the whole sensor readout should be redone to have a constant sampling rate and not some dirty hacking solution like in multiwii..
Timecop allready adapted the layout of the boards, so the sensor readout could be triggered via the dataready lines of the sensors.
I just found a bug in my integration calculation. Actually I noticed it when implementing that stuff and didn't bother further as the complementary filter still kinda fixed the wrong acc integration... The fixed Version is on my github page: https://github.com/luggi/baseflight/com ... d1ea768557 I will flight test it tomorrow morning.

Torque66 · Post by **Torque66** » Wed Sep 04, 2013 5:30 am

I got it working at least to my needs... no bumps when engaging during descending or ascending flight... smooth transition to level flight no more jackrabbit.

Baro is in a seal blacked out delrin case.

P 2.5
I .045
D 20

Have a nice day...

brm · Post by **brm** » Wed Sep 04, 2013 6:14 pm

crazyal wrote:I just said it's better practice imho, not that it makes a noticeable differnce in the result.
Just think about a normal adc, there dt is 1/samplefreq and not the time between your calculations.
In my opinion the whole sensor readout should be redone to have a constant sampling rate and not some dirty hacking solution like in multiwii..
Timecop allready adapted the layout of the boards, so the sensor readout could be triggered via the dataready lines of the sensors.
I just found a bug in my integration calculation. Actually I noticed it when implementing that stuff and didn't bother further as the complementary filter still kinda fixed the wrong acc integration... The fixed Version is on my github page: https://github.com/luggi/baseflight/com ... d1ea768557 I will flight test it tomorrow morning.

but but but ...
you can literally calculate the time spend every few cpu cycles.
no one hinders you - just another messy place.
the baro is slower ...

i already have seperated sensor reading from the Fusion.
means sampling ar 1ms Intervalls - the attitude calculation takes 1.2ms - slightly optimized.
i am somewhat tired to adjust this every few days...

crazyal · Post by **crazyal** » Wed Sep 04, 2013 7:37 pm

to be honest I have a bit trouble figuring out what you actually mean..
would you care to share that code ?

I tried your lpf implementation and somehow the graph in the gui with it on looked a bit crippled.
I implemented a different lpf that smoothes the accz readings and not the sum.
additionally I also introduced a new way of setting the low pass filters via config. It's just the first quick implementation, but imho it seems quite sane.
Now you can directly enter the cut-off frequency you want for that filer. The end result may be a bit off due to sample times differing a bit, but it flies well.
Here is my latest flight-tested change to the code: https://github.com/luggi/baseflight/com ... 2efe7e617f

brm · Post by **brm** » Wed Sep 04, 2013 8:02 pm

you Need to be correct on the time axe with regards to the Integrator.
the sensor Fusion does also work with the pre-defined looptime.

Code: Select all

    accZ_smooth = accZ_smooth * lpf.accZ + accel_ned.V.Z * (1 - lpf.accZ);


    if (cfg.acc_unarmedcal == 1) {
        if (!f.ARMED) {
            accZoffset -= accZoffset / 64;
            accZoffset += accZ_smooth;
        }
        accZ_smooth -= accZoffset / 64;  // compensate for gravitation on z-axis
    } else
        accZ_smooth -= acc_1G;


    // apply Deadband to reduce integration drift and vibration influence
    accZ_smooth = applyDeadband(accZ_smooth, cfg.accz_deadband);
    accel_ned.V.X = applyDeadband(accel_ned.V.X, cfg.accxy_deadband);
    accel_ned.V.Y = applyDeadband(accel_ned.V.Y, cfg.accxy_deadband);

first a filter and then the deadband.
to me not good.
at the end you Need to adjust the complementary filter.
not filter a, filter b plus complementary at the very end.

i could share the code when you clean up the code :mrgreen:

crazyal · Post by **crazyal** » Wed Sep 04, 2013 8:30 pm

yeah, in general I probably could get rid of the deadband.. It's only there, because I just copied it from previous implementations.

sure, I'll definetly look trough it. =)

brm · Post by **brm** » Thu Sep 05, 2013 4:58 pm

you create a wiki describing all new variables accessable by cli?

crazyal · Post by **crazyal** » Fri Sep 06, 2013 5:08 pm

for your new code ?
with current baseflight everything is allready documented in mw.h

brm · Post by **brm** » Fri Sep 06, 2013 8:08 pm

crazyal wrote:for your new code ?
with current baseflight everything is allready documented in mw.h

yes, i see it.
but

// apply CF to use ACC for height estimation

what range?
how to tune?
what should i look for when Tuning....
what is CF?

crazyal · Post by **crazyal** » Fri Sep 06, 2013 9:42 pm

good questions

basically there are 2 complementary filters which mix baro velocity with integrated acc velocity and double integrated acc altitude with baro altitude.
The tuning parameters are baro_cf_alt and baro_cf_vel those factors go from 0 to 1.0f.
Setting those factors to 0.9 means 90% acc reading is used and 10% baro readings are used for the end result.
It basically acts as a low-pass for baro readings and as a high pass for acc readings depending on how you set that tuning factor.
more on that topic here: https://sites.google.com/site/myimuesti ... ary-filter
I hope that kinda answeres your question.

brm · Post by **brm** » Sun Sep 08, 2013 9:11 am

crazyal wrote:good questions
basically there are 2 complementary filters which mix baro velocity with integrated acc velocity and double integrated acc altitude with baro altitude.
The tuning parameters are baro_cf_alt and baro_cf_vel those factors go from 0 to 1.0f.
Setting those factors to 0.9 means 90% acc reading is used and 10% baro readings are used for the end result.
It basically acts as a low-pass for baro readings and as a high pass for acc readings depending on how you set that tuning factor.
more on that topic here: https://sites.google.com/site/myimuesti ... ary-filter
I hope that kinda answeres your question.

exactly that belongs to a wiki page.

Code: Select all

    // the accel values have to be rotated into the earth frame
    rpy[0] = -(float) angle[ROLL] * RADX10;
    rpy[1] = -(float) angle[PITCH] * RADX10;
    rpy[2] = -(float) heading * RADX10 * 10;

the angle value is not really good for the double Integration step.
as it is cut off using:

Code: Select all

    return (int16_t)(atan2f(y, x) * (180.0f / M_PI * 10.0f));

with the incorrect round ing.
you may save this into a seperate structure to increase the numerical stability.
will test this later on.
it is .... raining.

brm · Post by **brm** » Sun Sep 08, 2013 11:33 am

Torque66 wrote:I got it working at least to my needs... no bumps when engaging during descending or ascending flight... smooth transition to level flight no more jackrabbit.

Baro is in a seal blacked out delrin case.

P 2.5
I .045
D 20

Have a nice day...

i would Support These Settings as Defaults.

btw, what Motors do you use (kv rating)?

brm · Post by **brm** » Sun Sep 08, 2013 5:38 pm

crazyal wrote:for your new code ?
with current baseflight everything is allready documented in mw.h

you may check the sign of the baro velocity.
to me you should reverse the sign.

brm · Post by **brm** » Tue Sep 10, 2013 8:13 pm

i went through a few tests.
as usual - some where good others ... ups ... simply bad.

ecnlosed you find a hex file for testing.
i suggest to reset the config and applying the changed defaults.
i found the alternative pid calculations best.

P 1.5
I .045
D 20

where the Settings for alt. hold i used in my last test.
way to dark for my gopro.

crazyal · Post by **crazyal** » Tue Sep 10, 2013 8:32 pm

brm wrote:
crazyal wrote:for your new code ?
with current baseflight everything is allready documented in mw.h

you may check the sign of the baro velocity.
to me you should reverse the sign.

It is correct as in positive vel means going up and neg vel means going down. I didn't change that part coming from multiwii.
explaining why the sign is incorrect might help.. do you mean it should be compatible with the ned coordinates ? where positive z axis means going down ?
Could you please include the code to your .hex. I'm not really a fan of blackboxes regarding opensource stuff and I don't care how the code is formatted

, as long as it isn't as bad as that harakiri stuff

brm · Post by **brm** » Wed Sep 11, 2013 5:12 pm

the file with the code snippets has a Copyright at the very beginning.
you obviously did not copy them.

i am not a fan doing so.

here it is just a Placebo test.
no Feedback no source code - that simple.

crazyal · Post by **crazyal** » Wed Sep 11, 2013 9:34 pm

brm wrote:the file with the code snippets has a Copyright at the very beginning.
you obviously did not copy them.

i am not a fan doing so.

here it is just a Placebo test.
no Feedback no source code - that simple.

whatever.. your post makes no sense again.

crazyal · Post by **crazyal** » Sat Feb 01, 2014 6:39 pm

latest progress: not my video, but it's using the latest velocity based althold https://www.youtube.com/watch?v=A0XaMP3 ... ata_player
my video: http://www.youtube.com/watch?v=owIIuswYv98 sry for the bad quality

scrat · Post by **scrat** » Mon Feb 03, 2014 6:55 am

Better then naza

. I have almost the same results. I have set just P=3.8 I=0 D=0 for ALT_HOLD. IS that ok? What value for acc_lpf_factor you're using?

hinkel · Post by **hinkel** » Mon Feb 03, 2014 2:21 pm

Hi, I test this with defaults values and it is OK, but this parameter are not prog in CLI are this just constant ?

Code: Select all

cfg.P8[PIDVEL] = 120;
cfg.I8[PIDVEL] = 45;
cfg.D8[PIDVEL] = 1;

Vario for althold is missing !

Also (Taboo :mrgreen:

) GPS PH + RTH is " working again

" on Naze32 rev4 with "Afro32 CLI version 2.2 Jan 31 2014 / 10:01:02"
Thanks to Disq

PS: the "Cut in the stomach" code is just fine

Regards
hinkel

crazyal · Post by **crazyal** » Mon Feb 03, 2014 2:36 pm

hinkel wrote:Hi, I test this with defaults values and it is OK, but this parameter are not prog in CLI are this just constant ?
Code: Select all
cfg.P8[PIDVEL] = 120;
cfg.I8[PIDVEL] = 45;
cfg.D8[PIDVEL] = 1;
Vario for althold is missing !

Also (Taboo ) GPS PH + RTH is " working again " on Naze32 rev4 with "Afro32 CLI version 2.2 Jan 31 2014 / 10:01:02"
Thanks to Disq

PS: the "Cut in the stomach" code is just fine

Regards
hinkel

the values can be tuned in the development version of cTns Baseflight-Configurator. It will be in the the Chrome store once Chrome 33 is out.
When I have time I might make a patch, so these settings are programmable via CLI.

msev · Post by **msev** » Mon Feb 03, 2014 3:58 pm

hinkel wrote:Hi, I test this with defaults values and it is OK, but this parameter are not prog in CLI are this just constant ?
Code: Select all
cfg.P8[PIDVEL] = 120;
cfg.I8[PIDVEL] = 45;
cfg.D8[PIDVEL] = 1;
Vario for althold is missing !

Also (Taboo ) GPS PH + RTH is " working again " on Naze32 rev4 with "Afro32 CLI version 2.2 Jan 31 2014 / 10:01:02"
Thanks to Disq

PS: the "Cut in the stomach" code is just fine

Regards
hinkel

Hinkel hmmm "thanks to disq", would you mind explaining a bit more? I'm curious

hinkel · Post by **hinkel** » Mon Feb 03, 2014 4:24 pm

@msev
Some issues with GPS, Mr Disq solve this !

viewtopic.php?f=23&t=1947&start=640#p44626
viewtopic.php?f=23&t=1947&start=640#p44644