Alt. Hold Ideas and discussion

ziss_dm · Post by **ziss_dm** » Tue Jun 07, 2011 5:20 am

Hi,
Looks like few people currently trying to improve Alt. Hold feature of MWC. I just want to try to consolidate knoweledge and probably start discussion in this thread. It would be nice, if pople share their research results, code snippets, sensors data, etc here.

To start ball rolling I would try to summarize what we need to archive good results:

1) Good altitude estimator
a) Precise enough to hold altitude with, let's say, +-0.5m
b) Fast enough to be able to stabilize the system, rough estimate would be 3-4Hz bandwidth would be enough.
e) Prefferably linear, atleast in holding range

2) Good PID controller
a) Properly mixing and waiting errors (in case we have multiple error sources)
b) With properly designed integrator, to resist estimator noise and taking into account low bandwidth of the estimator
c) Generating properly scaled and probably filtered output to the motor mixer
d) Handle assymmetric response to the throttle control (not equal system response time for up and down controls, something similar is happening in heaters control)

3) Good feed forward control
a) "Lock" algorithm
b) Algorithm to adjust height after it was locked

Links collection:
http://www.ijcas.com/admin/paper/files/ ... 17-127.pdf
http://www.protuner.com/app6.pdf
http://www.bgu.ac.il/chem_eng/pages/Cou ... ter_10.pdf
http://saba.kntu.ac.ir/eecd/ecourses/No ... oo/pid.pdf
http://www.idt.mdh.se/kurser/ct3340/ht0 ... ion_22.pdf

http://www.electroiq.com/index/display/ ... ation.html
http://blueflyvario.blogspot.com/

Filter design Software:
http://www.winfilter.20m.com/
http://www-users.cs.york.ac.uk/~fisher/ ... /trad.html

2DOF PID Controller:

http://books.google.com.au/books?id=GSy ... er&f=false
http://ntrs.nasa.gov/archive/nasa/casi. ... 021831.pdf

http://www.icee-con.org/papers/2006/pdf/EC2-08.pdf

to be continued..

regards,
ziss_dm

Alexinparis · Post by **Alexinparis** » Tue Jun 07, 2011 11:44 pm

ziss_dm wrote: a) Precise enough to hold altitude with, let's say, +-0.5m

I really would like such an altitude hold.
But from what I measured with the BMP085 sensors, it's impossible to achieve because there is a static drift of +/- 1m on my desk, whatever the smoothing/averaging strategy.
I noticed also this sensor has an important inertia, its reaction time could be up to 1 or 2s.

That's why I think there is also something to do with the accelerometers for short term holding.

ziss_dm · Post by **ziss_dm** » Wed Jun 08, 2011 1:47 am

Hi,
1-2s is too slow.

So, what a current behavior in Alt. hold mode (assuming you are not touching throttle)?
-Is it slowly starts oscilating?
-Is amplitude of oscilations stbilizes or always increasing?
-Is it going down deeper than going up?

regards,
ziss_dm

ouiski · Post by **ouiski** » Wed Jun 08, 2011 12:39 pm

Hi,

What about using MPX4115 as in the Mikrokopter ? He works great on this platform and he's at the same price of the BMP (~20€). The only drawback is that it's an analog sensor, so it could increase the Cycle Time due to analog conversion time, but with a 3-4hz refresh frequency, would it be so bad ? I'm sure we can achieve a great altitude management with that sensor :mrgreen:

.

ouiski.

ziss_dm · Post by **ziss_dm** » Wed Jun 08, 2011 1:29 pm

Hi ouiski,
Do you know place where to buy it? An what a price?

regards,
ziss_dm

ziss_dm · Post by **ziss_dm** » Wed Jun 08, 2011 2:03 pm

Hi,

Idea:
We need ACC Z data, but it is really hard to compensate bias. Idea is to use EstG vector from getEstimatedAttitude(). This vector is gravity vector, but due CF it has big delay (approx 2 sec). And because it is rotated by gyro this delay only in magnitude, not in direction. So we can use it as dynamic bias compensation. Extracting acceleration would be really simple:

Code: Select all

acc_z = (acc_z * 7 +  (accSmooth[YAW] - GEstG.V.Z)) / 8;

After that we can integrate it:

Code: Select all

  vel_z += acc_z ;
  vel_z = constrain(vel_z,-600000,+600000); //WindUp

It is probably good idea to reset integrator:

Code: Select all

    if (rcOptions & activateBaro8 && baroPresent == 1) {
      if (baroMode == 0) {
        baroMode = 1;
        altitudeHold = altitudeSmooth;
        vel_z = 0;
      }

And now, we can construct PID:
acc_z - D term
vel_z - P term
(altitudeSmooth-altitudeHold) - I Term

Code: Select all

  #if defined(BMP085)
    if (baroMode) {
       rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE]-(2*(altitudeSmooth-altitudeHold) + acc_z* 32/100 + vel_z * 26 /1000 ),max(rcCommand[THROTTLE]-   100,MINTHROTTLE),min(rcCommand[THROTTLE]+200,MAXTHROTTLE));
    } 
  #endif

Effect is really interesting:
1) it is dampening stick input on quite big range
2) The integrator is drifting, approximately 2 sm per second
3) It is really easy to control height, you just need to compensate drift from time to time.
4) Not sure is Baro affecting it.

regards,
ziss_dm

ouiski · Post by **ouiski** » Wed Jun 08, 2011 2:04 pm

Hi !

You can buy it almost everywhere they sell MK parts as it is the FlightCtrl stock pressure sensor , on the official site :

https://www.mikrocontroller.com/index.p ... a885c52bad

It costs 20€ w/o shipping fees, probably around 30€ with shipping.

the datasheet is here : http://www.datasheetcatalog.com/datashe ... 115A.shtml

the response time is only 1ms on this sensor

.

sandmen · Post by **sandmen** » Thu Jun 09, 2011 5:34 am

One more good,precise Pressure Sensor is maybe
http://www.amsys.de/sheets/amsys.de.aan509.pdf

http://www.sander-electronic.de/be00067.html

Testing by
http://www.varesano.net/projects/hardware/FreeIMU#0.4 ?

ziss_dm · Post by **ziss_dm** » Mon Jun 13, 2011 12:21 pm

Hi,

This is the first experimental version, implementing this idea. It is not ideal, yet, but it looks like it is working. After adjusting parameters Alt. Hold PID was able to lock altitude with error +-1.5m without oscillations. The short overview of changes:

BMP085 driver:
1) Baro_update() changed to read temperature once per second.
2) Delay for temperature read reduced to 4.6 msec
3) Output filtered with 2nd order IIR with cut-off freq 1.5hz (floating point for now)
4) altitudeSmooth currently pressure
This was done to increase update frequency up to 33hz

IMU

Code: Select all

  #if BARO
    acc_z = (acc_z * 2 +  (accSmooth[YAW] - GEstG.V.Z)) / 3;
  #endif

And alt hold PID regulator:

Code: Select all

if (baroMode) {
      vel_z += acc_z;
      vel_z = constrain(vel_z,-60000,+60000); //WindUp
      pos_z += vel_z / 2;
      pos_z = constrain(pos_z,-60000,+60000); //WindUp
      
      PTerm = vel_z*P8[PIDALT]/1000;
      ITerm = pos_z*I8[PIDALT]/50000 + (altitudeSmooth-altitudeHold)*P8[PIDMAG]/10;
      DTerm = int32_t(acc_z) * D8[PIDALT]/10;
      rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE]-(PTerm + ITerm + DTerm),max(rcCommand[THROTTLE]-100,MINTHROTTLE),min(rcCommand[THROTTLE]+200,MAXTHROTTLE));
}

Configuration parameters:
Alt. P - first integral of ACC Z (vertical speed)
Alt. I - second integral of ACC Z (position?)
Alt. D - Acc Z (acceleration)
Mag. P - Proportional altitude error (actually pressure error)

Recommended setup procedure:
1) Set everything to zero.
2) Start increasing Alt. D. You should start feeling that quick throttle stick movements are dampened. Increase until really fast osculations starts in response on quick throttle stick movement. Than drop a little bit. My current value: 21
3) Start increasing Alt. P. Throttle response should be softer and softer. You are looking for combination of P and D which would dump throttle input on biggest range and with minimum drift. My current value 9.0
4) You can try to play with Alt. I, but looks like it does not do much. Same effect could be archived just increasing P.
5) Start increasing Mag. P (which is actually barometer error). The system should start stabilizing around set point. In case ACC part of the PID well adjusted, you can increase Mag.P really high. My current value 12 and I can probably make it higher. But this makes motors work harder and quad starts jumping following BMP085 noise.

Now the disadvantages, and things which require improvements:
1) The motors working hard, as the noise from BMP is amplified and presented in the output of the PID. In my case flying time is reduced by minute or two. Can be reduced by adjusting BMP filter, or by introducing I term for BMP.
2) It is always "sinks". As battery discharges it is slowly reducing altitude. Increasing Mag. P it is possible to reduce this effect, but this makes motors work harder. In future it can compensated by battery monitor, or implementing PID scheduling. Currently it is possible to "trim" it with throttle stick.
3) In case you flick switch having constant vertical speed, it is stabilizes higher or lower desired position. This effect probably could be reduced adjusting integral windups.

regards,
ziss_dm

Alexinparis · Post by **Alexinparis** » Wed Jun 15, 2011 1:05 pm

It's a major improvement !
I've not tested it yet, but that sounds very nice

I aslo noticed some other adaptations:
- the serial ISR is modified to insert a specific check, and secured with sei() call
- refresh rate of the GUI is increased. We can see better the attitude stabilization efficiency, but we have to check if it's friendly with low CPU setups
- there is no more a reference to altitudeZero
- some code space is saved via the neutralization of WMP code for non WMP users at the preprocessor step
- a bug corrected inside the pressure calculation function

acc_z = (acc_z * 2 + (accSmooth[YAW] - GEstG.V.Z)) / 3;
=> could be more precise with a +1 or +2 before dividing /3 (math integer approximation)

(altitudeSmooth-altitudeHold)*P8[PIDMAG]/10 is more something that should be in a PTerm

I understand magP is currently used only for convenient purpose in order to tune a variable, but only if you don't have a mag that needs it.

I will test it today

PatrikE · Post by **PatrikE** » Thu Jun 16, 2011 3:15 pm

Hi Ziss_dm

I'we put my Quad together again.

I cant get my bmp 085 to work with this dev.
The Gui shows barovalue 300 All the time and the Led on Ardumini is flashing.
Cykletime is flickering between 6000 & 16000.

It's not possible to calibrate Acc or start engines.
If i disable bmp in the code all seems normal again. (not testflown)

I'm using MultiWiiConf_dev20110607.
Because i dont know how to compile the sourse code to your bmp-Dev.

/Patrik

Alexinparis · Post by **Alexinparis** » Thu Jun 16, 2011 10:31 pm

So, first results are not very concluding.
I noticed the sink behavior few seconds after the alt lock.
I think it's because of the vel_z term that tends to reach (and stay at) the windup limit.

ziss_dm · Post by **ziss_dm** » Sun Jun 19, 2011 9:25 am

Hi,

After a little bit of thinking, I have decided that it is not going to work. The problem is our control algorithm. We are controlling the motor thrust, that means we are controlling acceleration. To get from point A to B the control algorithm should produce something like this:
- Increase power to accelerate
- Reduce power to stop acceleration
- wait
- Reduce power again, to decelerate.
Simple PID controller just not able to produce something like this. The acceleration stabilization, helps but not dramatically. With small P and huge D, theoretically PD controller can stabilize this system but only in case pressure sensor fast and have low noise. To be able to use simple PID we need to stabilize and control vertical velocity. To be able to measure vertical velocity we need to integrate Acc data and "mix" it with Baro data to cancel integrator drift. So, classic Complimentary filter

.

Code: Select all

#define Kg 1.5f
#define Kp1 (0.0018f * Kg)
#define Kp2 (0.003f * Kg)
#define Ki  (0.0003f * Kg)

void getEstimatedAltitude(){
  static uint8_t inited = 0;
  static uint16_t PreviousTime;
  static float AltErrorI;
  uint16_t CurrentTime  = micros();
  float deltaTime = (CurrentTime - PreviousTime) * (1.0f/1000000.0f);
  PreviousTime = CurrentTime;
  // Soft start
  if (!inited && baroNewData) {
    inited = 1;
    EstAlt = BaroAlt;
    EstVelocity = 0.0;
    AltErrorI = 0.0;
  }  
  // Estimation Error
  float AltError = BaroAlt - EstAlt; 
  AltErrorI += AltError;
  // 
  float InstAcc = (accADC[YAW]  - acc_1G);
  InstAcc = InstAcc*(9.8 / acc_1G) * 1.155f + Ki*AltErrorI;
  // Integrators
  float OVVelocity = EstVelocity;
  EstVelocity += (InstAcc*deltaTime + Kp1*AltError);
  EstAlt += ((OVVelocity + EstVelocity)*deltaTime/2.0f + Kp2*AltError);
  // Debug
  magADC[0] = BaroAlt*100.0f;
  magADC[1] = EstVelocity*1000.0f;
  magADC[2] = EstAlt*100.0f;
}

After that, we can build PID controller to stabilize velocity:

Code: Select all

 //**** Velocity stabilization PID ****        
      int32_t VelError = EstVelocity*1000.0f - AltPID;
      delta = VelError - lastVelError;

      VelErrorI += VelError;
      VelErrorI = constrain(VelErrorI,-10000,10000);
      
      PTerm = VelError*P8[PIDVEL]/1000;
      ITerm = VelErrorI*I8[PIDVEL]/50000; 
      DTerm = delta * D8[PIDVEL]/100;

      lastVelError = VelError;

      rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE]-(PTerm + ITerm - DTerm),max(rcCommand[THROTTLE]-200,MINTHROTTLE),min(rcCommand[THROTTLE]+200,MAXTHROTTLE));

And control it with another PID for altitude set point:

Code: Select all

//**** Alt. Set Point stabilization PID ****
      error = (AltHold - EstAlt)*10.0f;
      delta = error - lastAltError;

      errorAltitudeI += error;
      errorAltitudeI = constrain(errorAltitudeI,-1400,1400);

      PTerm = error*P8[PIDALT]/10;
      ITerm = errorAltitudeI*I8[PIDALT]/1000;
      DTerm = delta * D8[PIDALT]/10;

      lastAltError = error;
      AltPID = PTerm + ITerm - DTerm;
     
      // Assymetric control
      if (AltPID < 0) AltPID = AltPID / 3;

The experimental version in the attachment. Small notes:
1) I have extended EEPROM parameters with new P/I/D for velocity PID. So it is better to re-check all your parameters again.
2) It is better to setup velocity PID first, than Alt PID
3) I was not able to test it outdoors, yet.
4) The CF currently requires 5-10 seconds to stabilize.

regards,
ziss_dm

PatrikE · Post by **PatrikE** » Mon Jun 20, 2011 7:16 am

Can someone please make a runable MultiWiiConf to this DEV?...

I don't know howto do it.

Is there any god guide for it?

/Patrik

ziss_dm · Post by **ziss_dm** » Mon Jun 20, 2011 9:12 am

Hi PatrikE,
Sorry, it was too big to attach it to the post. But gompf has made nice instructions how to compile configurator: Compile MultiWiiConf-Basic HowTo

regards,
ziss_dm

PatrikE · Post by **PatrikE** » Mon Jun 20, 2011 3:39 pm

Thanks Ziss...

After some struggle i got it to work.

The guide was god but the path for the library is not correct for me...
It gave me some gray hair...

My path have to be..
..\processing-1.5.1\modes\java\libraries\controlP5

/ PatrikE

ziss_dm · Post by **ziss_dm** » Wed Jun 22, 2011 1:58 pm

Hi,
Looks like idea is working!!!! :shock:

I have managed to hover full battery in the room, not touching throttle. It is fluctuating +- 0.7m and slightly sinking during discharge, but definitely holding set point!
The interesting thing, that BMP085 produces less noise if you do have pauses between measurements. In this version it reads pressure only 5 times per second.

regards,
ziss_dm

arnaldo · Post by **arnaldo** » Wed Jun 22, 2011 5:06 pm

can anyone make an executable dev with this upgrade ?!............alex?!?!?!?!?

PatrikE · Post by **PatrikE** » Wed Jun 22, 2011 7:12 pm

can anyone make an executable dev with this upgrade ?!............alex?!?!?!?!?

Install Processing and you can DIY.
Use this guide.
http://wbb.multiwii.com/viewtopic.php?f=18&t=190&p=860&hilit=processing#p860

It works almost like Arduino Software.

Open the pde-file with Processing and press RUN button.

Or do this
File/Export Application.

/Patrik

Alexinparis · Post by **Alexinparis** » Wed Jun 22, 2011 10:49 pm

Hi,

I tried to merge all the recent developments in a new dev (0622). (with executable also)
Live repository is also up to date.
ziss_dm, this version is for you as I integrated properly a new VEL PID in the gui.

I didn't have time to test your last dev, but I'm sure it will ease the next experiment.

ziss_dm · Post by **ziss_dm** » Thu Jun 23, 2011 2:46 am

Hi,
Tnx, Alex you made all dirty work for me..

I will try latest version today.

BTW: Do we want additional feature "Throttele assistant"?

Using only VEL PID alone gives really interesting behavior: It makes throttle adjustments really precise and soft and in theory it also should work without BMP085.

regards,
Dmitry Kaukov

arnaldo · Post by **arnaldo** » Thu Jun 23, 2011 11:30 am

ziss_dm wrote:Hi,
Tnx, Alex you made all dirty work for me..
I will try latest version today.

BTW: Do we want additional feature "Throttele assistant"? Using only VEL PID alone gives really interesting behavior: It makes throttle adjustments really precise and soft and in theory it also should work without BMP085.

regards,
Dmitry Kaukov

ziss and alex, can you explain how new dev works?! how we can tune new pid values and what do ALTpid and VELpid.
thank you

ziss_dm · Post by **ziss_dm** » Thu Jun 23, 2011 5:05 pm

Hi Alex,
This is my recent changes. Functionality looks like OK, but velocity stabilization not behaving.

Report will be tomorrow.
@arnaldo: I will try to explain tomorrow.

regards,
ziss_dm

ziss_dm · Post by **ziss_dm** » Fri Jun 24, 2011 12:46 am

Hi Alex,
Just a quick one. Looks like we have some sort of problem with mixers. Whan I replaced mixer from old version:

Code: Select all

  #ifdef QUADX
    motor[0] = rcCommand[THROTTLE] - axisPID[ROLL] + axisPID[PITCH] - YAW_DIRECTION * axisPID[YAW]; //REAR_R
    motor[1] = rcCommand[THROTTLE] - axisPID[ROLL] - axisPID[PITCH] + YAW_DIRECTION * axisPID[YAW]; //FRONT_R
    motor[2] = rcCommand[THROTTLE] + axisPID[ROLL] + axisPID[PITCH] + YAW_DIRECTION * axisPID[YAW]; //REAR_L
    motor[3] = rcCommand[THROTTLE] + axisPID[ROLL] - axisPID[PITCH] - YAW_DIRECTION * axisPID[YAW]; //FRONT_L
  #endif

it started behaving.

regards,
ziss_dm

Hamburger · Post by **Hamburger** » Fri Jun 24, 2011 3:45 pm

Hi ziss_dm and Alex,

for the Alt functionality a new PID triple was introduced to the paramstruct for LCD config. Unfortunately this does break powermeter handling via LCD in the config loop, because I tried to make the extra data and code in LCD.pde to be only included if needed. To achieve this I use defines in MultiWii.pde

Code: Select all

#if defined(POWERMETER)
  #ifndef VBAT
   #error "to use powermeter, you must also define and configure VBAT"
  #endif
  #define PARAMMAX 29
  #define PARAMMOTORSTART  20
  #define PARAMMOTOREND    28
  #define PARAMMOTOROFFSET 21
#else
  #define PARAMMAX 20
#endif

These values have to be altered (+3 to each) if you insert additional values to the paramstruct param[].
Sorry for the inconvenience. How should we solve this case and make it more failsafe for future changes to come? I like the concept of keeping the param[] and the configLCD code as small as possible depending on activated features. But this seems to contradict the ease of maintenance.
Maybe it is acceptable to enlarge the param[] unconditional with the powermeter extensions and only make the configLCD code feature-dependent, so it does not break at compile time nor run time? Then we could make the above defines superfluous and replace with relative reference to PARAM_BLOCK_SIZE.
What do you think? Hamburger

ziss_dm · Post by **ziss_dm** » Fri Jun 24, 2011 5:04 pm

Hi Hamburger,
I have slightly re-factored LCD.pde to avoid this in future. It would be nice if you could verify that PM functionality if ok. I have briefly tested it myself, but maybe missed something.

regards,
ziss_dm

Hamburger · Post by **Hamburger** » Fri Jun 24, 2011 7:10 pm

Hey ziss_dm,

very cool. It basically works. I found 2 1/2 minor itches
- powermeter sum for hardware sensor was pressed through calculation twice - fixed via cpp directive
- all powermeter values are uint32_t, - fixed? (following __u16Fmt expects pointer to uint16_t; is explicit cast required?)
- I need smaller increment for I-values :-)

please see my attached diff.
Thanks for the solution. Hamburger

ziss_dm · Post by **ziss_dm** » Fri Jun 24, 2011 11:20 pm

Hi Hamburger,
Tnx!!

regards,
ziss_dm

Alexinparis · Post by **Alexinparis** » Sat Jun 25, 2011 12:42 am

Hi,

I've just updated both your last dev

ziss_dm, what is exactly the problem with the PIDMIX ?
(I kept the same syntax, but came back to the original formula)

ziss_dm · Post by **ziss_dm** » Mon Jun 27, 2011 3:26 am

Hi Alex,
Sory I had no time to investigate algorithm itself, but I can describe some symptoms, maybe it would help.

So my configuration is Quad-X and I have notices the following behavior diring "hand test"
- Tilting Pitch and Roll has good resistance and feels normal
- But tilting diagonally has pretty much no ressistance at all.

In acro mode it feels more twitchy (same RC rate and expo), and seems like not holding angle, like I have zero I in PIDs
In level mode I had to reduce P from 8 to 4.

regards,
ziss_dm

ziss_dm · Post by **ziss_dm** » Tue Jun 28, 2011 12:37 am

Hi,
Looks like it is working with latest dev changes and because my quad going to service and bad weather, it would be interesting to see if somebody could repeat results with other setups. So, I would try to explain latest changes in soft. and GUI and setup precedure.

GUI changes:
1) The baro currently displaying altitude above see level in meters.
2) The baro graph automatically scaled with range +-1 m, so you can see alt. variations quite preciselly
3) The Estimated altitude is displayed on top of baro graph, it uses the same scale as baro graph. So it is possible to see how baro data is smoothed.
4) The additional PIDs added to configuration area.

How it works:
Complimentary filter: Mixing data from baro and accelerometers, to get estimation of vertical speed and altitude.
Velocity PID: Uses estimated vertical speed and adjusts throttle to hold desired vertical speed.
Altitude PID: Uses estimated altitude and adjusts vertical speed (Outpud is feeded to the Velocity PID)

Setup procedure:
1) Set Vel. and Alt. PID values to the zero
2) Start increasing Vel. D. You should start feeling that quick throttle stick movements are dampened. Increase until really fast osculations starts in response on quick throttle stick movement. Than drop a little bit. My current value: 30
3) Start increasing Vel. P. Throttle response should be softer and softer and quad should start holding altitude. The tricki part here, that CF using baro data for vertical speed and it is fluctuating, but you can roughly assest amount from GUI. The good values for Vel. PID should give you the following behavior: After flicking switch the quad should stop and slowly drift.
4) Start increasing Alt. P. The quad should stabilize around set point. Increase until it start oscilating, the value could be really high: I have 20
5) Start increasing Alt. I. The stabilization should become more precise.
6) experiment..

It would be nice to collect statistics of working PID values, as we need to decide do we really need full PIDs for both Alt. and Vel. and probably re-adjust PID ranges.

regards,
ziss_dm

ziss_dm · Post by **ziss_dm** » Sat Jul 02, 2011 8:52 am

Hi,

First outdoor test:
http://youtu.be/TQE07uFLO68

Conditions:
wind spd: 15Km/h
wind gust: 31Km/h

Mode: Level+Baro

regards,
ziss_dm

ziss_dm · Post by **ziss_dm** » Mon Jul 04, 2011 1:03 am

Hi Alex,

In the latest dev, the auto-repeat functionality was removed from LCD configurator. Was there some problem with it or just accident

? I thought it is nice feature..

regards,
ziss_dm

Alexinparis · Post by **Alexinparis** » Mon Jul 04, 2011 9:47 am

Hi,

I suppressed the auto repeat functionality because it caused sometimes an immediate exit without save from the LCD configuration loop.
But I’m not against this functionality if it’s restricted to only param switch & param modification

About the baro:
Your video is quite demonstrative, it seems to work well on your setup.

I tried yesterday and here is what I noticed:
- it worked well (very well) for 10-20s and the multi began to rise without limit progressively, I had to swith to a non baro mode to keep the control.
- when I activate the baro mode in flight, I noticed a sudden variation of the motor speed => the init conditions when the baro mode is activated should be refined

=> there is still something wrong somewhere, the most annoying part is the rising with no limit effect.

ziss_dm · Post by **ziss_dm** » Mon Jul 04, 2011 1:39 pm

Hi Alex,
As you are the first one, who actually tried this I have lot of questions for you

:
1) What PID's you had?
2) Have you managed to setup velocity PID separately from Alt. PID?
3) How it was flying with only velocity PID?
4) When it started raising, it was looking like something overflowing or it started oscillation? I had something similar than I had too big Alt. P, it is starting oscillation with amplitude, I would say 50m. And it is really fast.
5) Is CF working well in GUI?
6) I have also noticed, that CF require some time to stabilize after quad stay on the ground inclinated. Was your attempt was immediately after takeoff or after some time in the air?

But you are right, there are lot of work to do..

regards,
ziss_dm

Alexinparis · Post by **Alexinparis** » Mon Jul 04, 2011 11:57 pm

Hi,

1)
I have:
VEL: P 9.0 I 0.0 D 19
ALT: P: 9.0 I 0.0 D: 10

2)
yes, more or less, it works as expected

3)
I tested it only inside a room, not in open space.
and the test with ALT PID was done outside

4)
it was looking like something overflowing

5)
yes perfect
I modified some code (after my test)
I move baro est to expected baro value
I move baro raw value to a debug variable
(by default it is not graphed, you have to click on debug1 to see the graph)

6)
I modifed some code (after my test) to cancel the CF with ZACC when the quad is tilted.
My attempt was after some time in the air.
But anyway, I think it's not normal to have a sudden motor variation at the alt hold activation.

ziss_dm · Post by **ziss_dm** » Tue Jul 05, 2011 1:59 am

Hi Alex,

I think, that ALT: P: 9.0 I 0.0 D: 10, could actually start oscilation. I think ALT pid should have I term. In my latest tests I was using only PI term for ALT stabilization.
The D term in ALT pid could be usefull, but only in case setpoint is far from the current position (i.e. autonomous flight).

I have also noticed, that you switched back to relative alt. Is it because of OSD?

I think, it is better to leave sensors to report "raw" absolute altitude and for OSD we could have separate adjustment, which we could initialize on motors arming. There are couple of reasons for this:
1) Easier to debug sensor data (I had cases, than relative altitude was "hiding" mistakes in calculations)
2) Will be easier in future to fuse GPS alt to the CF.
3) Code will be smaller

regards,
ziss_dm

Alexinparis · Post by **Alexinparis** » Tue Jul 05, 2011 7:33 pm

you're wise, I switched back to absolute reading.
I will retry a baro hold experience latter.

ziss_dm · Post by **ziss_dm** » Tue Jul 12, 2011 4:26 am

Hi Alex,
Did you have a chance to test Alt. Hold? I have made some adjustments to the CF and PID regulators, but it also involves changes to the current Acc trimming procedure (current one changes magnitude of the ACC vector). So it would be nice to have second opinion.

regards,
ziss_dm

Alexinparis · Post by **Alexinparis** » Tue Jul 12, 2011 4:18 pm

ziss_dm wrote:Hi Alex,
Did you have a chance to test Alt. Hold? I have made some adjustments to the CF and PID regulators, but it also involves changes to the current Acc trimming procedure (current one changes magnitude of the ACC vector). So it would be nice to have second opinion.

regards,
ziss_dm

Hi,
I've not the chance to test it recently.
There is no problem to modify the current ACC trimming as the trim accuracy depends on the acc_1G value. So it's currently not perfect and should be adjusted to be ACC agnostic.

dynai · Post by **dynai** » Sat Jul 16, 2011 11:42 am

Hi,

i have tried 8" and 10" props... on the 10" i had that "rise without limit" effect... (default values)

then i followed the setup procedure.. could raise Vel. D over 60 without any signs of oscillation (1kg Quad) didn't go further...
as soon as Vel. P went over 6 "rise without limit"

that was the point when i switched from 10 to 8"... now no problems with a Vel. P of 10

next i rose Alt. P and i reached the LIMIT without major signs of oscillation

but as soon as i introduced a bit (0.01) Alt. I oscillation began... till i reached a I of 0.1 (YES).... i even went for 0.2

everything seems fine during the first 10-30 seconds as alex described but then quad drops/rises for approx 2-3 meters then stops climbs and after around 1 sinus it stabilises.. just to go 30secs later through the same procedure

my "findings" (if one wants to call them that way) Alt. D lead to worse results whereas Vel. D seemed to make things smoother.
Alt. P of 10-20 seems reasonable and delivered good results (reaching limits of functions always gives me some headache although 25 without Alt. I looked good)

but sometimes i wished i had a sonar

Vel. P makes me realy wonder... why does the quad start to climb that much... and why if you lower throttle does the stabilization stop before the quad stops climbing...

and last but not least could one of the experts have a look at this video of my gui... i wonder if my baro works as expected... thanks

http://youtu.be/9ZoRj-AdruY

the video was taken with the quad only powered over usb.. so it is resting on the table.. no movement, door to the room closed.

kind regards

Christoph

iacei2 · Post by **iacei2** » Sat Jul 16, 2011 6:34 pm

Hi, I have the same behaviour I hope those are not meters because it fluctuates from 1 to 2m when the quad is standing still.

Even worse if I throttle up forcing stable altitude the values jump to 2-3 m difference.

I did protect my board and the baro with a shell to avoid wind blowing on it but it does not change this behaviour.

Maybe my arms are too short: 47cm motor to motor with 10 inch props.

I did not try it during flight atm because I fear it will fly away. On hand I already felt that it wanted to rise or drop.

ziss_dm · Post by **ziss_dm** » Tue Jul 19, 2011 3:50 am

Hi dynai,
Thank you for detailed report. Currently I'm trying to re-scale PID's to have more usable ranges, so your report just in time.

According to your graph, you should be able to stabilize it inside 1m range (whole window is +-1m from center point). Ofcorse you need to protect sensor from the wind and props, but also make sure nothing touching the metal case of the sensor (according to the datasheet the minimum distance 2-3mm). So, you was not able to setup Alt. PID to avoid overshoots?

regards,
ziss_dm

Alexinparis · Post by **Alexinparis** » Tue Jul 19, 2011 10:28 am

Hi,

some brainstorming:

The current altitude estimator is very good. With the help of ACC integration, we regain the latency we lost using only data from the baro.
We can see clearly the effect of the filter with the first debug box in the 1.8 GUI (hide/show raw baro value, hidden by default). We can click on it for those you don't know

We have now something more closer to an ultra sonic sensor regarding response time.

So, one first thought:
- I know that in other project the altitude stabilization is very good using an ultra sonic sensor with a simple PID. Don't you think it can't be achieved here by a single PID using the current alt estimator ?

- the current code relie only on Z_ACC, and I corrected it to relie only on this under 10deg inclination (last dev). It is better to use the real magnitude, but the code&CPU time is longer

Code: Select all

  InstAcc = (sqrt((int32_t)accADC[YAW]*accADC[YAW]+(int32_t)accADC[ROLL]*accADC[ROLL]+(int32_t)accADC[PITCH]*accADC[PITCH]) - acc_1G) * AccScale;

- the reason why we encounter a sudden jump of altitude loss when we switch the baro mode is due to EstVelocity. It might be better to zero it at the switch activation (assuming the mode is engaged only when the multi is nearly alt table)

- there is nothing to constrain EstVelocity.
int32_t VelError = EstVelocity*1000.0f – AltPID;
PTerm = VelError*P8[PIDVEL]/1000;
I think it's the explanaition of the “rise without limit effect”. I didn't find out why such a divergence could occur, because it should be regulated by (Kp1 * Kt) * AltError , but it's the only explanaition for me.
I think there is a simple way to prevent this by adding a long term 0 convergence in the IMU loop. Something like

Code: Select all

  EstVelocity *= 0.99;

Alexinparis · Post by **Alexinparis** » Tue Jul 19, 2011 10:32 am

@dynai,
you baro works very well.
you can see it staying in a quite narrow range around -6.3m.
don't be afraid about the huge curve magnitude as there is an "autozoom" function in the gui for the altitude.

EOSBandi · Post by **EOSBandi** » Tue Jul 19, 2011 5:56 pm

Alexinparis wrote:Hi,

some brainstorming:

The current altitude estimator is very good. With the help of ACC integration, we regain the latency we lost using only data from the baro.
We can see clearly the effect of the filter with the first debug box in the 1.8 GUI (hide/show raw baro value, hidden by default). We can click on it for those you don't know
We have now something more closer to an ultra sonic sensor regarding response time.

So, one first thought:
- I know that in other project the altitude stabilization is very good using an ultra sonic sensor with a simple PID. Don't you think it can't be achieved here by a single PID using the current alt estimator ?

- the current code relie only on Z_ACC, and I corrected it to relie only on this under 10deg inclination (last dev). It is better to use the real magnitude, but the code&CPU time is longer
Code: Select all
  InstAcc = (sqrt((int32_t)accADC[YAW]*accADC[YAW]+(int32_t)accADC[ROLL]*accADC[ROLL]+(int32_t)accADC[PITCH]*accADC[PITCH]) - acc_1G) * AccScale;
- the reason why we encounter a sudden jump of altitude loss when we switch the baro mode is due to EstVelocity. It might be better to zero it at the switch activation (assuming the mode is engaged only when the multi is nearly alt table)

- there is nothing to constrain EstVelocity.
int32_t VelError = EstVelocity*1000.0f – AltPID;
PTerm = VelError*P8[PIDVEL]/1000;
I think it's the explanaition of the “rise without limit effect”. I didn't find out why such a divergence could occur, because it should be regulated by (Kp1 * Kt) * AltError , but it's the only explanaition for me.
I think there is a simple way to prevent this by adding a long term 0 convergence in the IMU loop. Something like
Code: Select all
  EstVelocity *= 0.99;

Hi Alex,
The idea to zero out EstVelocity seems to work. Perviously I got "shoot to the sky" or "drop like stone" when activating alt hold, now it activates smooth and flawless. Still need to adjust PID's but it's definitely better. Change for Z_ACC to use full magnitude of inclination seems to work also, however I did not noticed any sigificant behavior change.
Regards,
EOSBandi

ziss_dm · Post by **ziss_dm** » Wed Jul 20, 2011 9:17 am

Hi Alex,

So, one first thought:
- I know that in other project the altitude stabilization is very good using an ultra sonic sensor with a simple PID. Don't you think it can't be achieved here by a single PID using the current alt estimator ?

I think, it is not really possible to archive good control with one PID here, for the same reason(s) as it is not possible for level mode: We are controlling acceleration, measiring absolute displacement. The single PID are not able to generate acceleration profiles.

the current code relie only on Z_ACC, and I corrected it to relie only on this under 10deg inclination (last dev). It is better to use the real magnitude, but the code&CPU time is longer

I'm investigating it, but it ais also involves changes in existing Level Mode trimming.

- the reason why we encounter a sudden jump of altitude loss when we switch the baro mode is due to EstVelocity. It might be better to zero it at the switch activation (assuming the mode is engaged only when the multi is nearly alt table)

It is most logical explanation, but nobody yet reported jumps with Vel. PID only. (yes there is short bump while PID stabilizes, but no uncontrolled rises or drops). The jumps usually starts with Alt. PID. Which currently makes me to belive, that something wrong with PID controller itself.

The idea to zero out EstVelocity seems to work. Perviously I got "shoot to the sky" or "drop like stone" when activating alt hold, now it activates smooth and flawless. Still need to adjust PID's but it's definitely better. Change for Z_ACC to use full magnitude of inclination seems to work also, however I did not noticed any sigificant behavior change.

Be carefull, in case of big difference the CF could start scillating.

regards,
ziss_dm

ziss_dm · Post by **ziss_dm** » Thu Jul 21, 2011 12:31 pm

Hi Alex,

I think, we need to start merge changes ;(

Short overview of changes:
1) Level trim - separate from accZero, has constant step 0.1 deg
2) LCD - slightly optimized + Auto repeat
3) PDF for level mode (good for indoor tests

)
4) getEstimatedAltitude() - adjusted gains and uses Acc vector magnitude projected on local Z (.99 removed so far). Looks better in GUI and looks like have better velocity tracking. (with Vel. PID only has quite good transition to the Baro mode)
5) Velocity PID changed to PD scaled to the 16bit and constrained to avoid overflows.
6) Alt.PID - unfinished yet.
7) In Sensors.pde last changes reverted for BMAs ;(

regards,
ziss_dm

dynai · Post by **dynai** » Thu Jul 21, 2011 12:38 pm

hi Ziss_dm, hi Alex,

i had that rise - effect with only Vel. PID set up (Alt PID zeroed out) and it only occured when P reached a certain "limit", depending largely on the Props i had mounted.
the velocity fade-out could be a solution as it seemed that the software was thinking the quad is allready mooving when it wasn't

the baro is shielded inside a hull (large bottom opening) and has aroud 4mm space to all sides

i tested around 8 charges (around 50 changes of the pid) and could'nt get a better result than having it in a window of around 1m and after a certain time starting slowly starting to rise/drop for 3-4m before reaction catching itself after total up/down cycle (5-10m)....

with your confirmation of the baro-"resting"-curve i can't await to do some testing on the weekend.

kind regards

Christoph

edit: removed picture to shorten entry

Alexinparis · Post by **Alexinparis** » Thu Jul 21, 2011 11:16 pm

I've done some merges in R214

1) excellent
2) Auto repeat is now ok because the first blink sequence prevent from exiting immediately with the same stick sequence used to enter in conf loop. The autorepeat behavior is very convenient.
3) added as define
4) still ok, it's difficult to notice a difference in the GUI.
One thing I noticed more since last time: it's possible to fool the estimator by shaking rapidly the copter up/down.
It may explain sudden divergence in flight due to ACC vibration. probably one thing to investigate
5) ok
6) added some int32_t cast to avoid overflows
7) ok, it worked fine before. and sensor eeprom stuffs need more investigation.

8) I noticed a deadband function. added as a #define. I'm personally not a fan of this because it prevent the use of fine TX trim, but I understand the need.
9) no more MAX_CORRECTION

ziss_dm wrote:Hi Alex,

I think, we need to start merge changes ;(

Short overview of changes:
1) Level trim - separate from accZero, has constant step 0.1 deg
2) LCD - slightly optimized + Auto repeat
3) PDF for level mode (good for indoor tests )
4) getEstimatedAltitude() - adjusted gains and uses Acc vector magnitude projected on local Z (.99 removed so far). Looks better in GUI and looks like have better velocity tracking. (with Vel. PID only has quite good transition to the Baro mode)
5) Velocity PID changed to PD scaled to the 16bit and constrained to avoid overflows.
6) Alt.PID - unfinished yet.
7) In Sensors.pde last changes reverted for BMAs ;(

regards,
ziss_dm