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

Re: Altitude Hold improvement solution

Postby mahowik » Fri Sep 28, 2012 5:00 am

charbot wrote:hey guys,
I havent been following along on the alt hold progress so forgive me if Im missing something obvious in config or something...

Ive tried the last two new dev versions- 1129 & 1143, and everything is great except for alt hold (I have a bmp085, PID= default @ 1.6/ .015/ 7)- Starting at a stable hover, when I toggle baro, "on" my quad practically blasts off, vertically accelerating with all its got. Also (Not sure if this is important but ... ) since 2.1 ACC z axis reads 200 when the quad is static (using a nunchuk and motion + )
Is this a bug or does alt hold function differently now?


Sorry but as I remember nunchuk and motion+ are too noisy sensors for this alt hold solution... First check it in GUI as described some pages ago and also try special IMU_bmp085.ino...
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Postby mahowik » Fri Sep 28, 2012 5:59 am

Hi guys,

Here is MultiWii_2_1_b2 ;)

1) controlled ascend/descend mode based on the new Alt Hold routine. It's slowly increase/decrease AltHold altitude proportional to stick movement from point where AltHold activated ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms): solution from alexmos

2) /* Predefined initial throttle for AltHold will be calculated from MID (middle/hover) point of expo throttle from GUI (BUT not taken from current throttle value on AltHold activation).
I.e. pls. use GUI to set MID point of throttle expo as initial throttle.
Note: Value specified by define it's additional compensation for battery consumption */
//#define INITIAL_THROTTLE_HOLD_FROM_MID_EXPO_POINT 40

3) /* Correct throttle according to Z-axis inclination
Default is 100. Don't set above 200 */
//#define THROTTLE_ANGLE_CORRECTION 200

4) // set altitude to RTH. If Alt-hold activated during the RTH (or RTH during Alt-hold) it will keep specified altitude.
#define ALT_TO_RTH 1000 // in cm... = 10m

5) // set altitude after RTH, i.e. when copter reached to home position.
// DON'T USE VALUE LESS THEN 200 (2m) BECAUSE BARO ON ALTITUDES <2m WORKS UNSTABLE!!!
// Correct auto landing will be possible with sonar... soon ;)
#define ALT_TO_RTH_FINISH 350 // in cm... = 3.5m

6) // when roll or pitch more than specified value, PH (position hold) will be OFF and when roll/pitch stick released (i.e. in center) PH will be activated with new coordinates
// it will give possibility to fly in PH mode
#define GPSHOLD_DEADBAND 30

7) other changes from 2.1_b1 http://forum.rcdesign.ru/blogs/83206/blog15204.html
http://forum.rcdesign.ru/blogs/83206/blog15180.html

If you like this don't hesitate to buy me a beer by pressing donate button there ;)

thx-
Alex
Last edited by mahowik on Wed Dec 12, 2012 9:33 pm, edited 3 times in total.
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Postby Vaattari » Fri Sep 28, 2012 8:43 am

Mahovik, thanks for sharing the SW.
I tried b1 version with few flights in my Hex. I could not get Alt hold working at all even after playing with different PID values. Everytime the altitude hold worked worse than with MW2.1 (+-0.5m). During every test flight and after about 10 seconds the hex suddenly jumped up about 3 meters..
The hex does sudden small turns to left when only Level mode is used. Maybe that is caused by " #define GYR_CMPFM_FACTOR 400.0f" which you had in definitions, I have never used it before..
If everything works OK, what values should be seen in GUI debugs? Is there possible fixes in b2?
My setup:
Hex 6X (1.8kg AUW)
Flyduino Mega FC
Allinone +MS561101BA (BMA disabled from ALlinone)

Thanks
Vaattari
 
Posts: 14
Joined: Tue Mar 27, 2012 8:17 am

Re: Altitude Hold improvement solution

Postby shikra » Fri Sep 28, 2012 9:38 am

Alex - very cool indeed. Looking forward to testing next week.
User avatar
shikra
 
Posts: 783
Joined: Wed Mar 30, 2011 7:58 pm

Re: Altitude Hold improvement solution

Postby mahowik » Fri Sep 28, 2012 3:52 pm

Vaattari wrote:I tried b1 version with few flights in my Hex. I could not get Alt hold working at all even after playing with different PID values. Everytime the altitude hold worked worse than with MW2.1 (+-0.5m). During every test flight and after about 10 seconds the hex suddenly jumped up about 3 meters..

At first pls. make sure:
- baro protected from direct sunlight (I saw 3-10m jumps in baro values after 2-3sec of direct sun!!!) and direct wind.
- all test must be made at an altitude higher than 1.5 meters to avoid ground effect.

Then if you have jumps, 99% you have too much vibrations on your board. Probably not balanced propellers etc. Actually algorithm not so sensitive to vibrations BUT much sensitive than LEVEL mode, because for level mode it's possible to use much stronger LPFs.
So you can do the following:
1) reduce vibrations if possible
2) I suppose you have BMA180 acc on your Allinone board. In this case set internal acc LPF from 20Hz to 10Hz in Sensors.ino
replace
Code: Select all
control = control | (0x01 << 4); // register: bw_tcs reg: bits 4-7 to set bw -- value: set low pass filter to 20Hz

to this
Code: Select all
control = control | (0x00 << 4); // set low pass filter to 10Hz (bits value = 0000xxxx)

3) change ACC_LPF_FOR_VELOCITY to 15 (max 20) from 10 in IMU.ino
4) then try to check how described here:
- viewtopic.php?f=8&t=2371&start=80#p22636
- viewtopic.php?f=8&t=2371&start=100#p22667
- viewtopic.php?f=8&t=2371&start=110#p22842

5) PIDs tuning: viewtopic.php?f=8&t=2371&start=90#p22649

6) still doesn't help?! :) go to the point one to balance your motors and props ;)

Vaattari wrote:The hex does sudden small turns to left when only Level mode is used. Maybe that is caused by " #define GYR_CMPFM_FACTOR 400.0f" which you had in definitions, I have never used it before..

High GYR_CMPFM_FACTOR is good protection for MAG noise and variable influence from power cords... BUT as I suppose you have a lot of vibrations on the board and your gyro can't be stabilized in CF with so high factor... just comment it back (by default it's 200)...

thx-
Alex
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

Altitude Hold improvement solution

Postby Termic1 » Fri Sep 28, 2012 4:46 pm

Wow! Alex you're fantastic! I haven't my quad with me but monday I will test your code and I'll let you know!

Thanks again!

Luciano
Termic1
 
Posts: 40
Joined: Tue Aug 21, 2012 11:14 am

Re: Altitude Hold improvement solution

Postby dramida » Sat Sep 29, 2012 9:01 pm

Mahowik, you are the Man, i'll be the first who buys you lots of beer cans for your work. Even i didn't had time to test your last development Sw, I am convinced that we will reach a solution. 100$ from me, right now...donation is now complete
User avatar
dramida
 
Posts: 473
Joined: Mon Feb 28, 2011 12:58 pm
Location: Bucharest

Re: Altitude Hold improvement solution

Postby mahowik » Sat Sep 29, 2012 9:23 pm

dramida wrote:Mahowik, you are the Man, i'll be the first who buys you lots of beer cans for your work. Even i didn't had time to test your last development Sw, I am convinced that we will reach a solution. 100$ from me, right now...donation is now complete

Thanks a lot! But this is too much for beer! :) So I'm little bit confused...
I would like to return your money back ;)
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Postby dramida » Sat Sep 29, 2012 9:41 pm

I made much more money by using Multiwii copters, take a look at http://www.fotografieaeriana.eu , you are not the only one whom i donated money. Actually you earned it. Please keep it.
User avatar
dramida
 
Posts: 473
Joined: Mon Feb 28, 2011 12:58 pm
Location: Bucharest

Re: Altitude Hold improvement solution

Postby jevermeister » Sun Sep 30, 2012 12:52 am

Wow!Nice one Dragu!
User avatar
jevermeister
 
Posts: 708
Joined: Wed Jul 20, 2011 8:56 am

Re: Altitude Hold improvement solution

Postby flyrobot » Sun Sep 30, 2012 11:31 am

mahowik wrote:Hi guys,

Here is MultiWii_2_1_b2 ;)

1) controlled ascend/descend mode based on the new Alt Hold routine. It's slowly increase/decrease AltHold altitude proportional to stick movement from point where AltHold activated ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms): solution from alexmos

2) /* Predefined initial throttle for AltHold will be calculated from MID (middle/hover) point of expo throttle from GUI (BUT not taken from current throttle value on AltHold activation).
I.e. pls. use GUI to set MID point of throttle expo as initial throttle.
Note: Value specified by define it's additional compensation for battery consumption */
//#define INITIAL_THROTTLE_HOLD_FROM_MID_EXPO_POINT 40

3) /* Correct throttle according to Z-axis inclination
Default is 100. Don't set above 200 */
//#define THROTTLE_ANGLE_CORRECTION 200

4) // set altitude to RTH. If Alt-hold activated during the RTH (or RTH during Alt-hold) it will keep specified altitude.
#define ALT_TO_RTH 1000 // in cm... = 10m

5) // set altitude after RTH, i.e. when copter reached to home position.
// DON'T USE VALUE LESS THEN 200 (2m) BECAUSE BARO ON ALTITUDES <2m WORKS UNSTABLE!!!
// Correct auto landing will be possible with sonar... soon ;)
#define ALT_TO_RTH_FINISH 350 // in cm... = 3.5m

6) // when roll or pitch more than specified value, PH (position hold) will be OFF and when roll/pitch stick released (i.e. in center) PH will be activated with new coordinates
// it will give possibility to fly in PH mode
#define GPSHOLD_DEADBAND 30

7) other changes from 2.1_b1 http://forum.rcdesign.ru/blogs/83206/blog15204.html
http://forum.rcdesign.ru/blogs/83206/blog15180.html

If you like this don't hesitate to buy me a beer by pressing donate button there ;)

thx-
Alex


Hi Alex,

Today i tried this software version 2.1_b2 only point 6. when i moved the roll and nick in PH mode and AH to other place and i release the stick (back to center) the quad move back to the position coordinate when its start PH on. I tried with limited battery power (i haven't charged the battery full) so i haven't tried tuning PID for AH yet. it still have a good AH (move vertical approx 1 meter) if the quad is flying on altitude more than 2 meter. Next i will try to tuning the PID as you mentioned above.

John
flyrobot
 
Posts: 73
Joined: Thu Apr 05, 2012 3:59 pm

Re: Altitude Hold improvement solution

Postby flyrobot » Sun Sep 30, 2012 3:52 pm

I just flied with 2.1 b2 with PID tuned I just made the video : http://m.youtube.com/watch?v=cOdZ3TDWQ60
You can see the quad flying back to coordinate when the PH activated after i move the roll to get the quad moving to other place then i put the stick on centre position. Any step i missed ?
The Alt hold is much much better during rth or pos hold, but i see the quad always start going down about 1 meter when the alt hold start and going up again than stable alt hold.

John
flyrobot
 
Posts: 73
Joined: Thu Apr 05, 2012 3:59 pm

Re: Altitude Hold improvement solution

Postby mahowik » Sun Sep 30, 2012 5:03 pm

flyrobot wrote:I just flied with 2.1 b2 with PID tuned I just made the video : http://m.youtube.com/watch?v=cOdZ3TDWQ60
You can see the quad flying back to coordinate when the PH activated after i move the roll to get the quad moving to other place then i put the stick on centre position. Any step i missed ?

thanks for test!

Strange, probably I missed something... I have tested this in GUI and also one time on the fly... in GUI if you have PH activated (with 3D fix), try to tilt roll/pitch and you should see that PH is OFF when roll/pitch tilted...
flyrobot wrote:The Alt hold is much much better during rth or pos hold, but i see the quad always start going down about 1 meter when the alt hold start and going up again than stable alt hold.

I suppose it depends on the PIDs... i.e. it's oscillation before stabilization in PID controller...

p.s. and for statistics: what acc and baro you are using?

thx-
Alex
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Postby mahowik » Sun Sep 30, 2012 6:09 pm

special release for Crius SE (atmega328) because it has only 30720 byte maximum, BUT GPS_FILTERING disabled there... but some guys mentioned that it works better w/o filter ;)
http://forum.rcdesign.ru/blogs/83206/blog15302.html
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Postby flyrobot » Sun Sep 30, 2012 7:10 pm

mahowik wrote:
flyrobot wrote:I just flied with 2.1 b2 with PID tuned I just made the video : http://m.youtube.com/watch?v=cOdZ3TDWQ60
You can see the quad flying back to coordinate when the PH activated after i move the roll to get the quad moving to other place then i put the stick on centre position. Any step i missed ?

thanks for test!

Strange, probably I missed something... I have tested this in GUI and also one time on the fly... in GUI if you have PH activated (with 3D fix), try to tilt roll/pitch and you should see that PH is OFF when roll/pitch tilted...
flyrobot wrote:The Alt hold is much much better during rth or pos hold, but i see the quad always start going down about 1 meter when the alt hold start and going up again than stable alt hold.

I suppose it depends on the PIDs... i.e. it's oscillation before stabilization in PID controller...

p.s. and for statistics: what acc and baro you are using?

thx-
Alex


Hi Alex,

Im using Flyduino Mega (mega board) and 0.3.5 MS (ITG3200, BMP180 and MS5611).

I think, I need more times to tune the PID. Its really takes quite alot of time and battery.

I will check on my tx telemetry for pos hold on and off if i move the roll and nick.

Many thanks,

John
flyrobot
 
Posts: 73
Joined: Thu Apr 05, 2012 3:59 pm

Re: Altitude Hold improvement solution

Postby mahowik » Sun Sep 30, 2012 7:46 pm

Also for bma180 make sense to set 10hz internal LPF as described here viewtopic.php?f=8&t=2371&start=190#p23819
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Postby Termic1 » Mon Oct 01, 2012 11:16 am

mahowik wrote:Hi guys,

Here is MultiWii_2_1_b2 ;)

.....


Alex, I loaded your MultiWii_2_1_b2 but I'm not able to test it as it is based on 2.1 and not on the new DEV. The problem is that I'm using RCTimer UBLOX GPS and EOSBandi has made some mod to GPS.ino in order to configure this popular GPS at startup. These mods hase been integrated in the new dev. Loading your version I've lost all EOSBandi modifications and RCTimer GPS is not working anymore. I can't insert the GPS mod in your code as EOSBandi modified GPS.ino that contains your modification as well....too bad... :cry:

I was really happy to test new ALT to RTH functions but I have to wait a code that integrate your and EOS mods.

Thanks anyway for your effort...

Luciano
Termic1
 
Posts: 40
Joined: Tue Aug 21, 2012 11:14 am

Re: Altitude Hold improvement solution

Postby dramida » Mon Oct 01, 2012 6:30 pm

Today i tested B2 version of Alex.

1- Can't use GPS based function as this dev version has no support for Ublox GPS
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.
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.


To further test gps function i need to mount an Nmea Medietek 3329 gps or Alex add a define fro ublox :)

my two cents ideea:

Alex needs to work on current shared trunk to have more testers.
User avatar
dramida
 
Posts: 473
Joined: Mon Feb 28, 2011 12:58 pm
Location: Bucharest

Re: Altitude Hold improvement solution

Postby jevermeister » Mon Oct 01, 2012 6:45 pm

Dragu, if you put throttle to minimum the control algorithm stops completely.If trhottle<mincheck there is no governor active.

Nils
Ps.: nice footage
User avatar
jevermeister
 
Posts: 708
Joined: Wed Jul 20, 2011 8:56 am

Re: Altitude Hold improvement solution

Postby mahowik » Mon Oct 01, 2012 9:27 pm

Sorry guys for issues/bugs! :(
I hope your crashes are light...

I forgot to mention that features from b2 was initially tested only with one flight and in GUI...

dramida wrote:1- Can't use GPS based function as this dev version has no support for Ublox GPS

sorry, but I haven't ublox to check in case of add support for this...

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.

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...
Also with current solution possible additional delay because it changes only alt-hold point (i.e. altitude to hold) and PID controller tries to keep that new altitude point according to current PID values. So maybe it's not acceptable solution for the fast flights... i.e. only for correcting/changing altitude in hover...

dramida wrote:my two cents ideea:
Alex needs to work on current shared trunk to have more testers.

make sense :)
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Postby wilco1967 » Tue Oct 02, 2012 9:30 pm

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....
wilco1967
 
Posts: 156
Joined: Thu Aug 18, 2011 6:04 pm
Location: Winterswijk, Netherlands

Re: Altitude Hold improvement solution

Postby Magnetron » Tue Oct 02, 2012 10:23 pm

correct is:
static float accVelScale = 9.80665f / acc_1G / 10000.0f;
Magnetron
 
Posts: 124
Joined: Tue Jul 05, 2011 4:32 pm

Re: Altitude Hold improvement solution

Postby Mis » Tue Oct 02, 2012 10:57 pm

x/y/z = x/z/y, but arguments order make some differencies in code size.
Mis
 
Posts: 203
Joined: Fri Apr 01, 2011 12:23 am

Re: Altitude Hold improvement solution

Postby mahowik » Wed Oct 03, 2012 4:37 pm

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
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Postby wilco1967 » Wed Oct 03, 2012 5:00 pm

just tried it with
accVelScale = 9.80665f / acc_1G / 10000.0f;

still the same...... hardly able to hold altitude..... as bad as the old code (before your modifications)....
It DOES hold, just very bad..... PID tuning does not seem to make it any better.... had to reduce Alt_P to below 1 to stop it from yoyo-ing.... it will drop like 10 meters or more... Alt D tried everyting from 10 to 150...

This is the code I'm running (in the latest dev from SVN (1152), and which is not performing well. (it seems to do everything else just fine)
Code: Select all
#define UPDATE_INTERVAL 25000    // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY      4000000  // 4 sec initialization delay
#define BARO_TAB_SIZE   21

#define ACC_Z_DEADBAND (acc_1G/50)

#define applyDeadband(value, deadband)  \
  if(abs(value) < deadband) {           \
    value = 0;                          \
  } else if(value > 0){                 \
    value -= deadband;                  \
  } else if(value < 0){                 \
    value += deadband;                  \
  }

void getEstimatedAltitude(){
  static uint32_t deadLine = INIT_DELAY;

  static int16_t baroHistTab[BARO_TAB_SIZE];
  static int8_t baroHistIdx;
  static int32_t baroHigh;
 
 
  if (abs(currentTime - deadLine) < UPDATE_INTERVAL) return;
  uint16_t dTime = currentTime - deadLine;
  deadLine = currentTime;
 

  //**** Alt. Set Point stabilization PID ****
  baroHistTab[baroHistIdx] = BaroAlt/10;
  baroHigh += baroHistTab[baroHistIdx];
  baroHigh -= baroHistTab[(baroHistIdx + 1)%BARO_TAB_SIZE];
 
  baroHistIdx++;
  if (baroHistIdx == BARO_TAB_SIZE) baroHistIdx = 0;


  //EstAlt = baroHigh*10/(BARO_TAB_SIZE-1);
  EstAlt = EstAlt*0.6f + (baroHigh*10.0f/(BARO_TAB_SIZE - 1))*0.4f; // additional LPF to reduce baro noise
 
  #ifndef SUPPRESS_BARO_ALTHOLD

  //P
  int16_t error = constrain(AltHold - EstAlt, -300, 300);
  applyDeadband(error, 10); //remove small P parametr to reduce noise near zero position
  BaroPID = constrain((conf.P8[PIDALT] * error / 100), -150, +150);
 
  //I
  errorAltitudeI += error * conf.I8[PIDALT]/50;
  errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  BaroPID += (errorAltitudeI / 500); //I in range +/-60
 
 
  // projection of ACC vector to global Z, with 1G subtructed
  // Math: accZ = A * G / |G| - 1G
  float invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
  int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
  //int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - 1/invG;
  applyDeadband(accZ, ACC_Z_DEADBAND);
  debug[0] = accZ;
 
  static float vel = 0.0f;
  //static float accVelScale = 9.80665f / 10000.0f / acc_1G ;
  static float accVelScale = 9.80665f / acc_1G / 10000.0f; //wilco
 
  // Integrator - velocity, cm/sec
  vel+= accZ * accVelScale * dTime;
 
  static int32_t lastBaroAlt;
  float baroVel = (EstAlt - lastBaroAlt) * 1000000.0f / dTime;
  lastBaroAlt = EstAlt;

  baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
  applyDeadband(baroVel, 10); // to reduce noise near zero 
  debug[1] = baroVel;
 
  // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
  // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
  vel = vel * 0.985f + baroVel * 0.015f;
  //vel = constrain(vel, -300, 300); // constrain velocity +/- 300cm/s
  debug[2] = vel;
 
  //D
  applyDeadband(vel, 5);
  BaroPID -= constrain(conf.D8[PIDALT] * vel / 20, -150, 150);
  debug[3] = BaroPID;
 
  #endif
}


And this code works perfect..... (with almost all possible PID parameters.... currently 5, 0.020, 30 (default).
Code: Select all
#define UPDATE_INTERVAL 25000    // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY      4000000  // 4 sec initialization delay
#define BARO_TAB_SIZE   21

#define ACC_Z_DEADBAND (acc_1G/50)

void getEstimatedAltitude(){
  static uint32_t deadLine = INIT_DELAY;

  static int16_t baroHistTab[BARO_TAB_SIZE];
  static int8_t baroHistIdx;
  static int32_t baroHigh;
 
 
  if (abs(currentTime - deadLine) < UPDATE_INTERVAL) return;
  uint16_t dTime = currentTime - deadLine;
  deadLine = currentTime;
 

  //**** Alt. Set Point stabilization PID ****
  baroHistTab[baroHistIdx] = BaroAlt/10;
  baroHigh += baroHistTab[baroHistIdx];
  baroHigh -= baroHistTab[(baroHistIdx + 1)%BARO_TAB_SIZE];
 
  baroHistIdx++;
  if (baroHistIdx == BARO_TAB_SIZE) baroHistIdx = 0;


  //EstAlt = baroHigh*10/(BARO_TAB_SIZE-1);
  EstAlt = EstAlt*0.6f + (baroHigh*10.0f/(BARO_TAB_SIZE - 1))*0.4f; // additional LPF to reduce baro noise
 
  //P
  int16_t error = constrain(AltHold - EstAlt, -300, 300);
  error = applyDeadband16(error, 10); //remove small P parametr to reduce noise near zero position
  BaroPID = constrain((conf.P8[PIDALT] * error / 100), -150, +150);
 
  //I
  errorAltitudeI += error * conf.I8[PIDALT]/50;
  errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  BaroPID += (errorAltitudeI / 500); //I in range +/-60
 
 
  // projection of ACC vector to global Z, with 1G subtructed
  // Math: accZ = A * G / |G| - 1G
  float invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
  int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
  //int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - 1/invG;
  accZ = applyDeadband16(accZ, ACC_Z_DEADBAND);
  debug[0] = accZ;
 
  static float vel = 0.0f;
  static float accVelScale = 9.80665f / acc_1G / 10000.0f;
 
  // Integrator - velocity, cm/sec
  vel+= accZ * accVelScale * dTime;
 
  static int32_t lastBaroAlt = EstAlt;
  float baroVel = (EstAlt - lastBaroAlt) / (dTime/1000000.0f);
  baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
  baroVel = applyDeadbandFloat(baroVel, 10); // to reduce noise near zero 
  lastBaroAlt = EstAlt;
  debug[1] = baroVel;
 
  // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
  // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
  vel = vel * 0.985f + baroVel * 0.015f;
  //vel = constrain(vel, -300, 300); // constrain velocity +/- 300cm/s
  debug[2] = vel;
 
  //D
  BaroPID -= constrain(conf.D8[PIDALT] * applyDeadbandFloat(vel, 5) / 20, -150, 150);
  debug[3] = BaroPID;
}

int16_t applyDeadband16(int16_t value, int16_t deadband) {
  if(abs(value) < deadband) {
    value = 0;
  } else if(value > 0){
    value -= deadband;
  } else if(value < 0){
    value += deadband;
  }
  return value;
}

float applyDeadbandFloat(float value, int16_t deadband) {
  if(abs(value) < deadband) {
    value = 0;
  } else if(value > 0){
    value -= deadband;
  } else if(value < 0){
    value += deadband;
  }
  return value;
}

float InvSqrt (float x){
  union{ 
    int32_t i; 
    float   f;
  } conv;
  conv.f = x;
  conv.i = 0x5f3759df - (conv.i >> 1);
  return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);
}

int32_t isq(int32_t x){return x * x;}

Any idea where it really does something different (there is a lot of changes where the code was re-written, but to my untrained eye, seems to do the same).
wilco1967
 
Posts: 156
Joined: Thu Aug 18, 2011 6:04 pm
Location: Winterswijk, Netherlands

Re: Altitude Hold improvement solution

Postby mahowik » Wed Oct 03, 2012 5:28 pm

Also could not find the issue here :)
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...
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Postby mahowik » Wed Oct 03, 2012 5:57 pm

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...
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Postby wilco1967 » Wed Oct 03, 2012 6:27 pm

mahowik wrote:Also could not find the issue here :)
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...


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).
wilco1967
 
Posts: 156
Joined: Thu Aug 18, 2011 6:04 pm
Location: Winterswijk, Netherlands

Re: Altitude Hold improvement solution

Postby wilco1967 » Wed Oct 03, 2012 6:39 pm

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 !
wilco1967
 
Posts: 156
Joined: Thu Aug 18, 2011 6:04 pm
Location: Winterswijk, Netherlands

Re: Altitude Hold improvement solution

Postby Alexinparis » Wed Oct 03, 2012 9:19 pm

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 :)
Alexinparis
 
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: Altitude Hold improvement solution

Postby Willian_II » Wed Oct 03, 2012 10:59 pm

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
Willian_II
 
Posts: 6
Joined: Tue Sep 18, 2012 3:45 am

Re: Altitude Hold improvement solution

Postby dramida » Thu Oct 04, 2012 1:27 am

We need a startup calibration subroutine for 1g value.
User avatar
dramida
 
Posts: 473
Joined: Mon Feb 28, 2011 12:58 pm
Location: Bucharest

Re: Altitude Hold improvement solution

Postby capt » Thu Oct 04, 2012 2:37 am

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.
capt
 
Posts: 54
Joined: Wed Jan 19, 2011 10:54 pm

Re: Altitude Hold improvement solution

Postby Willian_II » Thu Oct 04, 2012 3:26 am

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....
Willian_II
 
Posts: 6
Joined: Tue Sep 18, 2012 3:45 am

Re: Altitude Hold improvement solution

Postby dramida » Thu Oct 04, 2012 11:42 am

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.
User avatar
dramida
 
Posts: 473
Joined: Mon Feb 28, 2011 12:58 pm
Location: Bucharest

Re: Altitude Hold improvement solution

Postby timecop » Thu Oct 04, 2012 11:48 am

No, because then you'd have to have the copter precisely level at power up.
timecop
 
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Altitude Hold improvement solution

Postby pm1 » Thu Oct 04, 2012 12:13 pm

ACC calibration -> The readout of my acc (ITG3200) is absolutely stable and reproducible over weeks, when the motors are not running.
pm1
 
Posts: 136
Joined: Sun Jan 22, 2012 7:26 pm

Re: Altitude Hold improvement solution

Postby timecop » Thu Oct 04, 2012 12:19 pm

ITG3200 is a gyro.
timecop
 
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Altitude Hold improvement solution

Postby pm1 » Thu Oct 04, 2012 12:23 pm

timecop wrote:ITG3200 is a gyro.

Sry, picked the wrong one... I meant BMA180
pm1
 
Posts: 136
Joined: Sun Jan 22, 2012 7:26 pm

Re: Altitude Hold improvement solution

Postby Sebbi » Thu Oct 04, 2012 12:35 pm

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 ;-)
Sebbi
 
Posts: 478
Joined: Sun Jul 08, 2012 1:08 am
Location: Germany

Re: Altitude Hold improvement solution

Postby timecop » Thu Oct 04, 2012 1:01 pm

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?
timecop
 
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Altitude Hold improvement solution

Postby dramida » Thu Oct 04, 2012 2:00 pm

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.
User avatar
dramida
 
Posts: 473
Joined: Mon Feb 28, 2011 12:58 pm
Location: Bucharest

Re: Altitude Hold improvement solution

Postby Sebbi » Thu Oct 04, 2012 2:28 pm

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 ;-)
Sebbi
 
Posts: 478
Joined: Sun Jul 08, 2012 1:08 am
Location: Germany

Re: Altitude Hold improvement solution

Postby mahowik » Thu Oct 04, 2012 4:12 pm

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
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

Re: Altitude Hold improvement solution

Postby Mac9 » Fri Oct 05, 2012 4:39 pm

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 :)
Mac9
 
Posts: 45
Joined: Thu Oct 20, 2011 2:47 pm

Re: Altitude Hold improvement solution

Postby mahowik » Fri Oct 05, 2012 5:21 pm

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

Re: Altitude Hold improvement solution

Postby Termic1 » Sat Oct 06, 2012 4:52 pm

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
Termic1
 
Posts: 40
Joined: Tue Aug 21, 2012 11:14 am

Re: Altitude Hold improvement solution

Postby dramida » Mon Oct 08, 2012 9:13 am

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.
User avatar
dramida
 
Posts: 473
Joined: Mon Feb 28, 2011 12:58 pm
Location: Bucharest

Re: Altitude Hold improvement solution

Postby Termic1 » Mon Oct 08, 2012 2:16 pm

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
Termic1
 
Posts: 40
Joined: Tue Aug 21, 2012 11:14 am

Re: Altitude Hold improvement solution

Postby mahowik » Mon Oct 08, 2012 5:16 pm

yes, for r1143 you should apply this fix viewtopic.php?f=8&t=2371&start=210#p24276
mahowik
 
Posts: 331
Joined: Sun Apr 10, 2011 6:26 pm

PreviousNext

Return to Software development

Who is online

Users browsing this forum: No registered users and 20 guests

cron