Altitude Hold improvement solution

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
timecop
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Altitude Hold improvement solution

Post by timecop »

Yep GUI looked OK. The graphs for debug0/debug2 made sense according to your description.

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Post by mahowik »

can you see the right velocity on debug2 during the vertical movements?
e.g. start move up with ~1m/s and stop after 1s you should see positive spike about 80..120 on debug2

try to test this when motors OFF (and then when motors ON) without alt hold activated

Image
Attachments
velocity.png
(2.71 KiB) Not downloaded yet

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Post by mahowik »

Guys! Any news?

Also I see very strange behavior of some guys here (like topic starter). Two weeks ago they screamed: "altitude hold is the weakest function in MWC!!!" etc.
But now when real solution is provided, they even have not interest with this...
Veeeeeeeeeery strange... jealousy or something else? I do not understand...
Or probably they think that I steal their idea?! :))))))))

Only some few guys gave a feedback on that. Thanks to them! Also have some good beedbacks from russian forum... but I still haven't statistics for bmp085... only one post: viewtopic.php?f=8&t=2371&start=80#p22608

thx-
Alex

wilco1967
Posts: 156
Joined: Thu Aug 18, 2011 6:04 pm
Location: Winterswijk, Netherlands

Re: Altitude Hold improvement solution

Post by wilco1967 »

Hi Mahowik,

don't let a lack of feedback put you off.....I definitely think you're on the right path....

I just managed to integrate your solution in the current _shared version.... Have just test flown it (few minutes only due to poor weather) and it is pretty darn good ! Without any tuning, and quite windy weather, it holds within one meter.... Very strong response to pushing up or down (Z axis acc)....
With the old code, it would over time go up/down at least 5 meters......

If the weather becomes any better, I will try and play with the settings some more, to see if it can be even better than in is already.

This is with an old wooden Tricopter, Flyduino mega, MPU6050 and BMP-085 baro (whichs drifts by one meter already on the table...)
quite bad motor/prop vibrations also, which are not really helpful....
I used your last 5611 code version for this..... (not the 085).

I would definitely call this a keeper..... let's hope Alex (the other one, from france ;-) will include this in the next official release !



I think, most people, including myself, are not coders.... so implementing such code is quite a challenge.... It took me quite some time, by carefully comparing different versions to figure out was was important from your software, and what were other people's changes (unrelated to your code), before I could include your code
and all without actually 'understanding' what it was doing.
I guess most people will simply wait for it to be implemented in the 'official' versions before trying it out.....

But there would not be any progress without bright people like yourselve making things....

Thanks a lot !

Wilco

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Post by mahowik »

Many thanks for quick response!

wilco1967 wrote:Without any tuning, and quite windy weather, it holds within one meter...
....
MPU6050 and BMP-085 baro (whichs drifts by one meter already on the table...)


within one meter in windy weather - it's very good results for bmp085 (its precision about +/-1m) even AFTER tuning!

And sorry for emotions...

Alex
Last edited by mahowik on Fri Sep 14, 2012 5:38 pm, edited 2 times in total.

User avatar
jevermeister
Posts: 708
Joined: Wed Jul 20, 2011 8:56 am
Contact:

Re: Altitude Hold improvement solution

Post by jevermeister »

Hi,
I am unable to participate in any testing, because I am on vacation.

Rsgarding the trolls:Do not feed the trolls!
Ignprimg off topic post helps a lot ;-)

vpb
Posts: 231
Joined: Mon Jul 23, 2012 4:09 pm

Re: Altitude Hold improvement solution

Post by vpb »

Maybe the alternative code is better, and maybe it'll be used in the next stable release. But with me, the original 2.1 alt holding code is good enough after tuned, with calm weather, it holds perfect, the range is about 20-40cm, with poshold, it's glued to one position. In the windy condition, I release the throttle stick, just hold pitch&roll stick to keep it not flying with the wind :D, never goes up/down more than 1m, and with un-tuned poshold, it plays with the wind in the horizontal zone about 3-5m.

Btw, there's no reason to stop improving the good :D, keep going on guys, hope to see the next stable release with better hold-capability.

LenzGr
Posts: 166
Joined: Wed Nov 23, 2011 10:50 am
Location: Hamburg, Germany
Contact:

Re: Altitude Hold improvement solution

Post by LenzGr »

I also intend to check out this code once I find a minute to merge the changes into my branch and the weather actually allows going outside (it's raining cats & dogs at the moment).

Patience, my young padawan :)

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Post by mahowik »

one more video from forum ;)
http://www.youtube.com/watch?v=Y8bNG-pEXao

BMA180 & MS5611

Tazzy
Posts: 75
Joined: Sun Jun 19, 2011 4:45 pm
Location: Sweden

Re: Altitude Hold improvement solution

Post by Tazzy »

How about to put the code implemented in a ready to try zip file, then i think a lot of none coders also will try it out and give some feedback.

bill516
Posts: 334
Joined: Sun Aug 07, 2011 12:27 pm

Re: Altitude Hold improvement solution

Post by bill516 »

I second the zip idea, I have had lots of trouble lately trying to get the GPS code into the sketch without errors, a previously compiled zip makes life so much easier. With any luck I might be able to get to try this alt hold this weekend as well as the GPS and setup my new quad, clean the car and a host of other things.

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Post by mahowik »

Tazzy wrote:How about to put the code implemented in a ready to try zip file, then i think a lot of none coders also will try it out and give some feedback.

Thanks for help!
You can just replace IMU.ino or take full sketch here http://forum.rcdesign.ru/blogs/83206/blog15204.html

User avatar
dramida
Posts: 473
Joined: Mon Feb 28, 2011 12:58 pm
Location: Bucharest
Contact:

Re: Altitude Hold improvement solution

Post by dramida »

mahowik wrote:one more video from forum ;)
http://www.youtube.com/watch?v=Y8bNG-pEXao

BMA180 & MS5611

Very good alt hold Mr. Manhovik and i appologise i didn't gave you any feedback.
I like the new Alt Hold function, it is a real improvement. I admit i didn't test it as i am not quite comfortable in merging code inside MWC.
But i'll test it todaay, as i have all the infos gathered and a downloadable version also.
I sum up all the important things you wrote about alt-hold in this spammed topic:



- debug[0] - z axis acceleration +/- 2
- debug[1] - baro velocity
- debug[2] - velocity after CF (velocity from acc corrected by baro velocity through the CF) -5..15
- debug[3] - sum of the P+I+D


- cycle time should be about 3-4ms... otherwise you should re-tune LPFs and CF
- pls. make all test on altitude >=2m to avoid ground effect (on less altitudes measurement baro can give -1..2m and become very unstable)

- wait 10-15 sec after power on OR after ACC calibration... all LPFs, velocity integrator and CF need to be syncronized

- for mpu6050 pls turn on 42hz LPF (#define MPU6050_LPF_42HZ)...


PIDs tuning:
1) make shure that velocity calculation works... set P and I to zero (only D=30) and try to activate alt hold when copter goes up (with velocity 1..2m/s). It's should try to stop copter immediatelly (but possible to drift because P and I are zero)! If no effect, something wrong at all and next steps doesn't make sense...
If ok, play with D to find "best stop effect"... probably D=20 it's enough...
2) keep "I" about 0.01... It's should be enough according to baro precision to avoid slow oscillations produced by "I" part
3) set P to 1.0 and try to increase before it start oscillate, then reduce for 20-30%

I tried PIDs 5.0-0.030-30


All necessary changes for new baro+acc alt hold only in one file IMU.ino


Thank you for your efforts.

/Mihai

albertoLG
Posts: 57
Joined: Fri Sep 07, 2012 8:14 am

Re: Altitude Hold improvement solution

Post by albertoLG »

mahowik, I tested your code with a CRIUS AIO on a rcexplorer's tricopter, and I can say that it works very good!!! only today I could test it, windy, but the altitude was perfect, 20/50 cm. never had a so stable altitude hold!! thank you for your efforts!!

wilco1967
Posts: 156
Joined: Thu Aug 18, 2011 6:04 pm
Location: Winterswijk, Netherlands

Re: Altitude Hold improvement solution

Post by wilco1967 »

Ok.... tried some tuning.....

I could go as high as 7.0, 0.255, 70 (yes, that's 0.255.... not a 0 missing, about the highest MultiWii would allow), and it still would not oscillate.....
I guess my old tricopter is rather heavy....

I settled at 5.0, 0.100 and 50, as it is still very good, but the variations of the BMP-085 are less noticable then.... I get 1 meter baro variations just sitting idle at the kitchen table, without any fans or anything disturbing it (it's a wooden table ;) ).

So is a 5611 so much better than a 085 as some people claim ?.
Did someone compare them 1 to 1 on a copter (preferably with this great code), and how noticable was it ?
If it is really better, I might have to get a 5611....

I get around +/- 1 meter in flight (as reported from Multiwii -> Frsky telemetry, which is based on baro).
It also holds pretty well in fast forward flight (up to a limit of course).
In fast forward flight, It sometimes seemed to drop a bit (2..3 meter max), but a glance at the reported altitude in the telemetry showed is was actually holding altitude. I guess this could be the wind from fast forward actually gives the sensor some small offset.... At least the code is doing its job.

I'm very happy with these results !!!!!

Thanks a lot Mahowik !.... Great addition !.... :D

Now let's have it included in the next official version ! :!:

User avatar
Crashpilot1000
Posts: 631
Joined: Tue Apr 03, 2012 7:38 pm

Re: Altitude Hold improvement solution

Post by Crashpilot1000 »

Thank you very much Mahowik!

I also implemented your key features in my version of the Althold feature and it works great. My approach is a little different with reduced memory usage.
I only take really new baroaltitude values into account (about every 30ms) from slightly tuned and timing corrected sensors/baro read out routines for ms and bmp (both tested: they don't stall anymore - i let them run over 2h without problem). So that the pipeline is reduced from 40(16Bit) to 5(32Bit). The hight is also normalized to ground level at the start. (Future plan: Auto safety landing)
The PID controller is also completely changed:
P: Is a normal P, wich can produce oscillations. It "knows" the target hight.
I: Is the strength of a ACC/Variometer brake (using the key parts of your implementation). This brake supresses oscillations quiet well. The "I" doesn't know the target hight at all.
D: The D is now the throttle angle correction. This prevents a drop of the copter in forward flight.

Tuning: Set I and D to zero, increase P till solid JOJO appears, then increase I till jojo is done, try increasing both values further. Then do forwardflight and increase D till the hightdrop is compensated. My default values are set a little high on purpose (Alt PIDs P 10,0 // I 0,030 // D 80). So you can even start tuning with these defaults and just lower them to your needs.
As safety measure althold is disengaged if the copter flips on his back (when a prop breaks or something).

I also altered the way to set a new altitude. I measure the speed of throttlestickmovements. If the user stops fiddeling with the throttlestick for a predefined timeinterval a new Althold and a new deadband around throttlestick is defined. Now you can leave althold on.

The next thing i did is changing the positionHold behaviour. Position Hold override: When you fly in PH mode and fiddle with nick/roll to reach a new place, PH is disengaged and after a timeinterval when these sticks are back to center, a new PH is defined and PHmode is engaged again. Both functions combined work similar to loiter on arducopter.

Try out my version as well. You don't need eepromclear when an other 2.1 was preinstalled, do a acc calib. in the gui. The predefined althold values a listed above. You can even use your own config.h, then the default values are used and Positionholdoverride is set to OFF. Here is a listing of the config.h values you can change: http://fpv-community.de/showthread.php? ... post200133
I tried to comment the most codelines.
There are too many changes to publish the codebits here so i included the whole version below.
There is a german thread here: http://fpv-community.de/showthread.php? ... post200133

Please have a look at this version as well.

So long
Kraut Rob

EDIT: This version requires an accelerometer of some kind, like mpu (gyro&acc), bma180 (like on some freeimus), bma020 etc !! Only Baro and gyro can not and will and not work! I wonder, if the ancient nunchuk delivers some useful performance in this context?
Attachments
MultiWii_2_1_NewBaroPIDVario3Final.zip
(105.68 KiB) Downloaded 453 times

cswiger
Posts: 12
Joined: Wed Jun 20, 2012 11:43 pm

Re: Altitude Hold improvement solution

Post by cswiger »

just tried Mahowik's IMU.ino and see noticible improvement - just a quick test.
I could not get the crashpilot1000 sketch to work - got lots of i2c errors after setting my config.h defines. builds and loads but those bus errors made me back out.

User avatar
Crashpilot1000
Posts: 631
Joined: Tue Apr 03, 2012 7:38 pm

Re: Altitude Hold improvement solution

Post by Crashpilot1000 »

i2c errors??? Sorry, but my mod does not mess with your hardware!!! If the original 2.1 runs on your hardware, my mod should do it as well! There is no reason for that. If 2.1 doesn't run, my mod will not run either. Don't tell me 2.1 runs fine on your setup. In wich version did you copy the modified "imu" anyway ?

So long
Rob

User avatar
Hamburger
Posts: 2578
Joined: Tue Mar 01, 2011 2:14 pm
Location: air
Contact:

Re: Altitude Hold improvement solution

Post by Hamburger »

Hi Rob,
can you create a diff (unified diff, via 'diff -u') for your changes?
Then it would be easier for others to understand your changes and/or merge it into other branches.
It is good to see that at last Mahowik and you have pushed the limits of alt.hold ( and vel-z ?) further.

User avatar
Hamburger
Posts: 2578
Joined: Tue Mar 01, 2011 2:14 pm
Location: air
Contact:

Re: Altitude Hold improvement solution

Post by Hamburger »

@all baro users:
we currently have 3 implementations
- original v2.1
- mahowik
- crashpilot1000

I would like to see this cleaned up soon, if possible. ( even though I do not use alt.hold myself, the soon to come variometer will use the vel-z ).

Is it safe to say the original v2.1 is outdated now?
Can we safely replace it with one of two other implementations?
Or both for the user to choose per #define (not good in the long run)?

LenzGr
Posts: 166
Joined: Wed Nov 23, 2011 10:50 am
Location: Hamburg, Germany
Contact:

Re: Altitude Hold improvement solution

Post by LenzGr »

I've extracted mahovik's changes to IMU.ino (against the 2.1 version in trunk, not the _shared repo) and it compiles. I hope to be able to do some outdoor tests later today, once the wind has calmed down. For what it's worth, here's the diff:

Code: Select all

[lenz@labtop MultiWii]% bzr diff IMU.ino 
=== modified file 'IMU.ino'
--- IMU.ino   2012-07-15 17:12:21 +0000
+++ IMU.ino   2012-09-16 12:08:30 +0000
@@ -169,10 +169,15 @@
   v->Y += delta[PITCH] * v_tmp.Z + delta[YAW]   * v_tmp.X;
 }
 
+#define ACC_LPF_FOR_VELOCITY 10
+static float accLPFVel[3];
+
+static t_fp_vector EstG;
+
 void getEstimatedAttitude(){
   uint8_t axis;
   int32_t accMag = 0;
-  static t_fp_vector EstG;

 #if MAG
   static t_fp_vector EstM;
 #endif
@@ -194,7 +199,10 @@
     deltaGyroAngle[axis] = gyroADC[axis]  * scale;
     #if defined(ACC_LPF_FACTOR)
       accLPF[axis] = accLPF[axis] * (1.0f - (1.0f/ACC_LPF_FACTOR)) + accADC[axis] * (1.0f/ACC_LPF_FACTOR);
-      accSmooth[axis] = accLPF[axis];
+    
+     accLPFVel[axis] = accLPFVel[axis] * (1.0f - (1.0f/ACC_LPF_FOR_VELOCITY)) + accADC[axis] * (1.0f/ACC_LPF_FOR_VELOCITY);
+     
+     accSmooth[axis] = accLPF[axis];
       #define ACC_VALUE accSmooth[axis]
     #else 
       accSmooth[axis] = accADC[axis];
@@ -253,51 +261,108 @@
 
 #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 BARO_TAB_SIZE   21
+
+#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,BaroLow;
-  int32_t temp32;
-  int16_t last;
-
+  static int16_t baroHistTab[BARO_TAB_SIZE];
+  static int8_t baroHistIdx;
+  static int32_t baroHigh;

+
   if (abs(currentTime - deadLine) < UPDATE_INTERVAL) return;
-  deadLine = currentTime;
+  uint16_t dTime = currentTime - deadLine;
+  deadLine = currentTime;

 
   //**** Alt. Set Point stabilization PID ****
-  //calculate speed for D calculation
-  last = BaroHistTab[BaroHistIdx];
-  BaroHistTab[BaroHistIdx] = BaroAlt/10;
-  BaroHigh += BaroHistTab[BaroHistIdx];
-  index = (BaroHistIdx + (BARO_TAB_SIZE/2))%BARO_TAB_SIZE;
-  BaroHigh -= BaroHistTab[index];
-  BaroLow  += BaroHistTab[index];
-  BaroLow  -= last;
-
-  BaroHistIdx++;
-  if (BaroHistIdx == BARO_TAB_SIZE) BaroHistIdx = 0;
-
-  BaroPID = 0;
-  //D
-  temp32 = conf.D8[PIDALT]*(BaroHigh - BaroLow) / 40;
-  BaroPID-=temp32;
-
-  EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
+  baroHistTab[baroHistIdx] = BaroAlt/10;
+  baroHigh += baroHistTab[baroHistIdx];
+  baroHigh -= baroHistTab[(baroHistIdx + 1)%BARO_TAB_SIZE];
   
-  temp32 = AltHold - EstAlt;
-  if (abs(temp32) < 10 && abs(BaroPID) < 10) BaroPID = 0;  //remove small D parametr to reduce noise near zero position
+  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
-  BaroPID += conf.P8[PIDALT]*constrain(temp32,(-2)*conf.P8[PIDALT],2*conf.P8[PIDALT])/100;   
-  BaroPID = constrain(BaroPID,-150,+150); //sum of P and D should be in range 150
-
+  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 += temp32*conf.I8[PIDALT]/50;
+  errorAltitudeI += error * conf.I8[PIDALT]/50;
   errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
-  temp32 = errorAltitudeI / 500; //I in range +/-60
-  BaroPID+=temp32;
+  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;}

carlonb
Posts: 210
Joined: Sun Apr 03, 2011 6:29 pm

Re: Altitude Hold improvement solution

Post by carlonb »

mahowik wrote:......
PIDs tuning:
1) make shure that velocity calculation works... set P and I to zero (only D=30) and try to activate alt hold when copter goes up (with velocity 1..2m/s). It's should try to stop copter immediatelly (but possible to drift because P and I are zero)! If no effect, something wrong at all and next steps doesn't make sense...
If ok, play with D to find "best stop effect"... probably D=20 it's enough...
2) keep "I" about 0.01... It's should be enough according to baro precision to avoid slow oscillations produced by "I" part
3) set P to 1.0 and try to increase before it start oscillate, then reduce for 20-30%
......

Hi mahowik,
I'm trying your code and I need to be addressed better.
Relating to point 1) when my hexacopter is rising and I engage AH it try to stop, this means that code is calculating correctly the velocity and I can continue with PID setup.
Then I've a dificulty to set I & P
1) What do you mean when say "play with D to find "best stop effect" ?
2) What do you mean when say "to avoid slow oscillations produced by "I" part ", slow oscillation like yo-yo or roll/pitch vibrations ?
3) What do you mean when say "before it start oscillate" ? before or after ? oscillate like yo-yo or vibrations ?

I hope you can answer my questions, and later I can try again.
Thanks for your effort ! :)

Ciao, Carlo

User avatar
Crashpilot1000
Posts: 631
Joined: Tue Apr 03, 2012 7:38 pm

Re: Altitude Hold improvement solution

Post by Crashpilot1000 »

@Hamburger:

Hi!
I tried WinMerge-2.12.4 on both Versions. I hope it helps. But i will do a complete writeup of the changes soon.

So long

Rob
Attachments
MultiWii_2_1_VS_MultiWii_2_1_NewBaroPIDVario3Final_Diff.zip
(4.44 KiB) Downloaded 443 times

cswiger
Posts: 12
Joined: Wed Jun 20, 2012 11:43 pm

Re: Altitude Hold improvement solution

Post by cswiger »

[quote="Crashpilot1000"]i2c errors??? Sorry, but my mod does not mess with your hardware!!! If the original 2.1 runs on your hardware, my mod should do it as well! There is no reason for that. If 2.1 doesn't run, my mod will not run either. Don't tell me 2.1 runs fine on your setup. In wich version did you copy the modified "imu" anyway ?

Rob - Sorry, likely some n00b issue, this is way over my head - just the facts tho. Today, I've erased eeprom, commented out #define I2C_GPS, even unplugged the Navigatron. Loaded the sketch and get the thosands of i2c errors (attached). The sketch I'm using is latest 2.1 for Quadrino Zoom d/l from flyingeinstein.com (for the board w/ a seperate acc). To that, I just replaced the IMU.ino from Mohawik and do not get the errors.
Attachments
i2c errors
i2c errors

User avatar
Crashpilot1000
Posts: 631
Joined: Tue Apr 03, 2012 7:38 pm

Re: Altitude Hold improvement solution

Post by Crashpilot1000 »

@cswiger: Thank you for your feedback.
I can not reproduce GPS problems here because i do not have the req. hardware. I can assure you, i didn't alter the core gps code/hardwareread out at all. It is just another variable in the mainprogram to deactivate the gps influence temporarily. It must be some flyingeinstein specific issue. Perhaps they use an altered address? I get the same crazy errorcount with no gps connected. This is why changing just the "IMU" works for you as long as the rest is still "flyingeinstein".
Can you tell me the specific flyingstein file you use? Perhaps i can find out what they changed.
Sorry for my impolite post before !!!!

So long
Rob

Edit: I looked at a flyingeinstein version. It is like i guessed they altered some I2C adresses for their hardware. Thats why the errorcount goes up - because no hardware found on "normal" addresses. They even introduced an option to reduce the PWM output from 490Hz to 250Hz.
What you can do: Unzip my MultiWii_2_1_NewBaroPIDVario3Final and replace these files with the ones from your flyingeinstein version:
def.h
Output.ino
config.h

Then start arduino 1.0.1 with the NewBaroPIDVario3Final and edit the config.h by pasting in these lines somewhere:

Code: Select all

    #define AltHoldBlindTime 800000

    #define PositionHoldOverride                          // Uncomment to activate @cswiger i activated it for you here
      /*Parameters for Position hold Override You see here the default values. Uncomment and change if you want */
    #define PosHoldBlindTime 300000                       // Time in microseconds (def. 0.3 sec) before PH is re-engaged after returning to Stickcenter
    #define BlindZonePitch 30                             // Defines the Blindzone around Pitchaxiscenter
    #define BlindZoneRoll 30                              // Defines the Blindzone around Rollaxiscenter


That should do the trick.

So long
Rob

User avatar
dramida
Posts: 473
Joined: Mon Feb 28, 2011 12:58 pm
Location: Bucharest
Contact:

Re: Altitude Hold improvement solution

Post by dramida »

Guys, i tested the Manhowik's Altitude Hold code and it works amazingly. It has an accuracy of about 20-30 cm and it reacts immediately after a down push or up push and most important , it keeps its altitude in forward flight as well. There are some issues with an initial jump proportional with D term after witch the copter comes down to initial AH level and it keeps that level. My best P-I-D settings were 4-0.01-10 on a Crius AIO quadcopter

I think the alt hold could be even better but for now, the improvement goal is reached and it deserves to enter in the shared trunk.


Well done Mr. Manhowik and The Team!
/Mihai

penpen77
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

Re: Altitude Hold improvement solution

Post by penpen77 »

tried new althold code with baro+sonar this weekend, works fine.

Sonar alone is a little bit dampeling, but sonar+baro fusion interval has a good behavior with same pid setting than baro only

User avatar
dramida
Posts: 473
Joined: Mon Feb 28, 2011 12:58 pm
Location: Bucharest
Contact:

Re: Altitude Hold improvement solution

Post by dramida »

My friend posted a clip with me having a beer and playing with Position Hold and Altitude Hold in MWC 2.1 (before Alt hold upgrade) Now the alt hold works even better.

http://www.youtube.com/watch?v=orsYq4lGbQk

Enjoy

copterrichie
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Altitude Hold improvement solution

Post by copterrichie »

dramida wrote:My friend posted a clip with me having a beer and playing with Position Hold and Altitude Hold in MWC 2.1 (before Alt hold upgrade) Now the alt hold works even better.

http://www.youtube.com/watch?v=orsYq4lGbQk

Enjoy


This is BEFORE? Well, I think the developers should be congratulated and not bashed. From what I see in this video, Alt-HLD Works very well. Hmmm.

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Post by mahowik »

Hi guys,

Thanks a lot to all for feedback! As for now i have a vacation in Mexico an I'm far from my toys :)
Also i have internet only on reception with my android so it's not so useful to write a lot... sorry...

I think if you are going to include this to next release, it make sense to add it as experemental function (with define) because we still have not enough statistic with bmp085.

@Crashpilot1000: Good points described in prev page!
Throttle angle correction was in my plans also... :-D good tested solution already implemented by alexmos, so i just was going to take it from there. I have not chance to look at your code now but from description it seems your implementation have not classic "I" part at all or it's there just as constant? You need to have this at least as constant because it's classic part of pid controller and helps to keep slow altitude correction during the battery power consumption...

Thx-
Alex

nicog
Posts: 88
Joined: Tue Aug 21, 2012 2:21 pm

Re: Altitude Hold improvement solution

Post by nicog »

Hi Mahowick,
I did some tests last week with the Timecop implementation of your code....

The accZ stop works. I tested it with D 20 and to 50 and P=I=0. It really stop the ascension.

But then, I could never got a real hold after tuning the PID. With Ms baro I get more than 1 meter variation while geting like 20 to 30 cm on od baro code.

I setted the looptime at 3000 µs to be at the same values as 8bits.

Should the quad keep altitude while moving? That didn't happend. On movement the quad falls like in old baro computation.

But you are going the right way I think.

wilco1967
Posts: 156
Joined: Thu Aug 18, 2011 6:04 pm
Location: Winterswijk, Netherlands

Re: Altitude Hold improvement solution

Post by wilco1967 »

mahowik wrote:Hi guys,

I think if you are going to include this to next release, it make sense to add it as experemental function (with define) because we still have not enough statistic with bmp085.

Alex


Don't worry...... BMP085 works just fine with your code.... much better than the original code anyway....
perhaps not as good as a 5611, but still very good.

User avatar
shikra
Posts: 783
Joined: Wed Mar 30, 2011 7:58 pm

Re: Altitude Hold improvement solution

Post by shikra »

cswiger wrote:just tried Mahowik's IMU.ino and see noticible improvement - just a quick test.
I could not get the crashpilot1000 sketch to work - got lots of i2c errors after setting my config.h defines. builds and loads but those bus errors made me back out.


On your GUI, there is no baro data - suggest you look at baro config - is correct one activated, and on the right address

User avatar
shikra
Posts: 783
Joined: Wed Mar 30, 2011 7:58 pm

Re: Altitude Hold improvement solution

Post by shikra »

Mahowik - awesome. Tested this morning and it was real good.
There was a little wandering at first (both gps and baro) and then it settled and just sat in the sky not moving. Unreal.

It was perfect time to test - early morning - no wind. Will be interesting to see what its like in the wind.

For me its by far the best Baro implementation so far.
RTH worked very well. I also tried it with FPV goggles on - unnerving the first time giving over control and trusting it!


Tricopter running a PARIS/SIRIUS combo ( BMP085 + BMA180 / ITG3200 / HMC5883 ) + multiwiicopter.com I2C GPS
Default GPS and BARO PIDS

User avatar
Hamburger
Posts: 2578
Joined: Tue Mar 01, 2011 2:14 pm
Location: air
Contact:

Re: Altitude Hold improvement solution

Post by Hamburger »

Is anything stopping us to replace the v2.1 alt.hold code with both or any of the two mahowik & crashpilot1000 versions?
Baro alt.hold experts: what do you say?

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: Altitude Hold improvement solution

Post by Alexinparis »

Hi,

I think we have some very good feedbacks about mahowik changes.
It should be a great improvement for multiwii as this is currently a weak point.
I will take some time to integrate it in the _shared and will also make a snapshot version so that we can have more feedbacks.

On this basis, I suggest to study how this code could be improved with Crashpilot1000 code and idea.

User avatar
Hamburger
Posts: 2578
Joined: Tue Mar 01, 2011 2:14 pm
Location: air
Contact:

Re: Altitude Hold improvement solution

Post by Hamburger »

very good. Thanks for that decision.

Can we try and prepare separation of
1. baro sensor reading
2. different optional uses of baro info - selectable via defines
a. alt.hold
b. variometer
c. ...

It would be helpful for memory footprint if having a baro sensor would not automatically include all code for all possible baro value uses.

vistauser
Posts: 14
Joined: Mon Jul 25, 2011 7:58 pm
Location: Germering, Southern Germany

Re: Altitude Hold improvement solution

Post by vistauser »

@ROB
tested ALTHOLD today with your V3 final software.
PID setting 10.0//0.020//60

We had various wind conditions:
at no wind AH was very good +- 20cm smooth up and down,
with gusty winds altitude changes up to +- 1m, but always safe and smooth.

During flying the MC around AH still worked as expected.

With increasing flying time and reduced battery power the initial set altitude for hold could´nt be maintained
and the MC slowly came lower and lower. (its an additional indication to stop flying, hi).

The 640 gr MC X used is equiped with:

Flydubution power distribution board
Flyduino Mega FC,
ATAVRSBIN1 Sensorboard und BMP085 on Flydusense board,
FMP04 GPS on Flyduino FMP04 GPS Bob.

TX is a Spektrum DX7 with Satellit RX

Props are 9" ( cuted EPP1045 )
Engines: Turnigy2205 1500Kv
ESC: Turnigy Plush 18A,
powered by a 3S1P 2200 mA battery.

Summery:
for me this Alt Hold implementation is a good one, also if i compare it with the AH performance of my MK.

Thank you very much for your and Mahowiks hard work.

vistauser

User avatar
Crashpilot1000
Posts: 631
Joined: Tue Apr 03, 2012 7:38 pm

Re: Altitude Hold improvement solution

Post by Crashpilot1000 »

@Alexinparis: Thank you very much for considering of implementing bits of my work as well !!
I can do a complete writeup with all changes. Currently i am reducing the bytesize by using some 16Bit instead of 32Bit calculations when possible and optimizing speed (bitshifts instead of div or mul when possible).

@mahowik: Thank you for looking into the mod as well!!
You are right, it has no real "I" to compensate a draining battery. So it isn't anything like a real PID controller, but it is so easy to tune for everybody, because changing a value shows a direct and comprehensible effect.
But i did several other things:
- I do not use an additional LPF or deadband on the acc to increase reactiontime. In the last step i do a simple average with the last BaroPID to smooth things out.
- The constant "accVelScale" is now calculated only once at startup when the groundalt is gathered ("getGroundAlt") for 4,5 sec.
I found out that when you set P higher it can compensate for a draining battery for the most of the time. And the increased speed of the acc/baro variometer brake ("I") kills the oscillations quiet well. So in my opinion it's sufficient to set both values high enough. Normally you would increase P till a solid jojo and then reduce it (like factor 0.6 or so) but now i would increase the P further (knowing i am in range now) and iron out the oscillations later with the variometerbrake ("I"). So the "P" gives enough "power" to compensate for a draining lipo. It's perhaps against every law, but it is pragmatic, easy to understand and works quiet well. The throttle angle correction is currently just a simple linear relation between the tiltangle and a throttlefactor ("D").

@vistauser
Thank you for posting your results here. After upgrading to Freeimu 0.3.5MS it's hard to test with bmp085. So thank you very much for your report. The draining battery is compensated only by the "P" over the time because there is no real "I". But they all go down sooner or later with or without "I".

@Hamburger
A different codepath is necessary for gyro and baro combination only. That's what i did before mahowik's great work. I think this combination should be really rare. At the moment I2C GPS/althold and posholdoverride still fits into a promini.

So long
Kraut Rob

LenzGr
Posts: 166
Joined: Wed Nov 23, 2011 10:50 am
Location: Hamburg, Germany
Contact:

Re: Altitude Hold improvement solution

Post by LenzGr »

FIY, I made some tests with mahowik's implementation yesterday (added on top of the stock 2.1 release, see my previous post for the patch). So far I could not get it fully stable (slow oscillations), I need to toy around with the PID values some more (using a Flyduino MicroWii with MPU-6050 and MS5611 baro). Any hints on how to best perform the PID tuning for fast results?

flyman777
Posts: 55
Joined: Mon Sep 19, 2011 1:44 pm

Re: Altitude Hold improvement solution

Post by flyman777 »

Hi,

many thanks to Mahowik for his work.
I tried the IMU_BMP085.ino (MW2.1) yesterday on my tri (900g) and after 10 flights I can say that it is a huge improvement.
Never the altitude holding was so reliable and reproducible.
My settings ended at PID: 8; 0.010; 50.
The altitude hold is very strong in fast climbing or descending and then switching BARO ON, like nailed in the sky.
In slow transition, the AH is about +- 20 cm even when the battery is near to be empty.
In fast flight, the loss of height will be about 1 meter and the recovery to the initial altitude begins when the speed decreases.
I wish that this mod would be taken in the next release if possible.

Thanks
Regards
Claude

User avatar
Hamburger
Posts: 2578
Joined: Tue Mar 01, 2011 2:14 pm
Location: air
Contact:

Re: Altitude Hold improvement solution

Post by Hamburger »

Crashpilot1000 wrote:@Hamburger
A different codepath is necessary for gyro and baro combination only. That's what i did before mahowik's great work. I think this combination should be really rare. At the moment I2C GPS/althold and posholdoverride still fits into a promini.

??
I do not know about gyro&baro only combinations, sorry.
But contrary to your experience I constantly struggle to fit 10DOF code and LCD conf&telemetry into promini, without any gps activated.
What I want to see is this:
From baro sensor we get some data that can be used for lots of different features
- alt hold
- variometer
- limited velocity rise/fall
- etc
So it looks appropriate to only enable the desired features (via define as always) and not have it all only because a baro sensor is defined.

User avatar
Crashpilot1000
Posts: 631
Joined: Tue Apr 03, 2012 7:38 pm

Re: Altitude Hold improvement solution

Post by Crashpilot1000 »

Hi, i saved 150 Bytes off already.

Compiled size with freeimu 0.3.5 MS and simple serial LCD and posholdoverride and i2C GPS the Byte size is:
Orig 2.1: 26.690 (no pos hold override)
MultiWii_2_1_NewBaroPIDVario3Final: 28.182 (+1492)
MultiWii_2_1_NewBaroPIDVario3Final "Diet": 28.032 (+1342)

Compiled size with freeimu 0.3.5 MS and simple serial LCD the Byte size is:
MultiWii_2_1: 24.512
MultiWii_2_1_NewBaroPIDVario3Final: 25.678 (+1166)
MultiWii_2_1_NewBaroPIDVario3Final "Diet": 25.528 (+1016)

Btw:
Though you do not know about gyro&baro only combinations - they are possible.
"..So it looks appropriate to only enable the desired features .. "
Castrating althold to free a few bytes for gps (like SUPPRESS_BARO_ALTHOLD) seems unlogical, because gps alone is not able to maintain an altitude.
Activating 90% of the code just for an osd output is hardly comprehensible to me.
Like always it's a point of view.

So long
Kraut Rob

User avatar
Hamburger
Posts: 2578
Joined: Tue Mar 01, 2011 2:14 pm
Location: air
Contact:

Re: Altitude Hold improvement solution

Post by Hamburger »

Crashpilot1000 wrote:Castrating althold to free a few bytes for gps (like SUPPRESS_BARO_ALTHOLD) seems unlogical, because gps alone is not able to maintain an altitude.
Activating 90% of the code just for an osd output is hardly comprehensible to me.


Let me explain: I have no gps, I am not interested in alt.hold. it is not about 'just an osd output'. It is about several possible features, all based on baro readings. The currently implemented altitude output is just the beginning - see my earlier posts for other uses.

User avatar
mgros
Posts: 90
Joined: Thu Jan 20, 2011 12:32 am

Re: Altitude Hold improvement solution

Post by mgros »

Thats could be our solution?
http://www.youtube.com/watch?v=4ijo_821TQI&feature=em-uploademail
a good job from Fabio.

User avatar
jevermeister
Posts: 708
Joined: Wed Jul 20, 2011 8:56 am
Contact:

Re: Altitude Hold improvement solution

Post by jevermeister »

if you have a free imu

User avatar
Crashpilot1000
Posts: 631
Joined: Tue Apr 03, 2012 7:38 pm

Re: Altitude Hold improvement solution

Post by Crashpilot1000 »

I have done a summary of all my changes here in human readable form:

http://fpv-community.de/showthread.php? ... post189820

@Hamburger: My uploaded version includes a pure Baroclimbrateoutput. It is an average of the last 8 readings, so it is a little jerky. Currently i am working on something like a "Nazx style" (don't like the word, but everybody knows) so i added a slightly slower acc/baro clean climbrateoutput with 10cm/s resolution without floatpoint stuff. Still work in progress. I look into your matter and will reduce it to the minimum by #define. Just estalt and climbrate (is 10cm/s res ok?) Althold and "Pid" disabled.

P.s.: Nice to see Fabio is also on it! I think the air gets thinner for some overpriced closed source products. I am sure they never looked during development for a second at a already working code like in this great mwii project!!
Cheers
Rob

copterrichie
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Altitude Hold improvement solution

Post by copterrichie »

Rabbit?

User avatar
Hamburger
Posts: 2578
Joined: Tue Mar 01, 2011 2:14 pm
Location: air
Contact:

Re: Altitude Hold improvement solution

Post by Hamburger »

Crashpilot1000 wrote:@Hamburger: My uploaded version includes a pure Baroclimbrateoutput. It is an average of the last 8 readings, so it is a little jerky. Currently i am working on something like a "Nazx style" (don't like the word, but everybody knows) so i added a slightly slower acc/baro clean climbrateoutput with 10cm/s resolution without floatpoint stuff. Still work in progress. I look into your matter and will reduce it to the minimum by #define. Just estalt and climbrate (is 10cm/s res ok?) Althold and "Pid" disabled.

Rob,
sure, 10cm/s res should do easily. For the next use of baro in the variometer feature, I think the res will not be critical. It is really about signaling tendency - constant alt, slowly rising, faster rising, fast rising and same for falling. The vel-z has not to be overly acurate but the value should be stable for constant conditions and the tendency not bounce.
If your throttle is set for a slooow rise of the copter, the vario should always say 'slow rise'. Intermittent wrong 'slow fall' or 'fast rise' signals would be counterproductive.

I will need some time to clean up the Vbat issue and from there delve into the variometer thing.
Thanks, Hamburger

User avatar
shikra
Posts: 783
Joined: Wed Mar 30, 2011 7:58 pm

Re: Altitude Hold improvement solution

Post by shikra »

I did another test with mahowiks code last night.

Tough conditions - swirling gusty wind and spots of rain.
The altitude held very very well. Much better that the GPS position actually. Both are on default PIDS.
Considering the wind and having an open sensor (no foam surround) I was very impressed.

And the best test - leave it up in position hold and flatten battery....
It worked brilliant. Held it up until it had not enough power for the motors at which point it started to descend.
Switch over to manual - take off and would last maybe 3 seconds before it started to descend.
Old battery is knackered - but it shows that battery state is not an issue.

Crash1000 I'll take a look at your when get chance. But I lost my programmer somewhere....

Post Reply