MW2.4 IMU.cpp bugs?

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
Post Reply
Dennis D
Posts: 16
Joined: Wed Jun 24, 2015 1:48 am

MW2.4 IMU.cpp bugs?

Post by Dennis D »

I have Crius MW SE 2.5 on TL250 quad frame running MW2.4
I have 2 items that I think need review in the IMU.cpp file.

Item 1:
The existing applyDeadband() macro affects the values that are outside of the deadband region. This affects the calibration/linearity of the system.
This is not a standard deadband function and I can't see how this is the intended function.
Please review and comment.

The existing macro (approx line # 290)

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

The replacement macro that does not affect values outside of the deadband region is below:

#define applyDeadband(value, deadband) {value = (abs(value) < deadband ? 0 : value);}

The graph (with example data and deadband = 5) illustrates the effect of the existing MW deadband function vs. the effect of a typical deadband.

Image


Item 2
in getEstimatedAltitude() at approx line # 330

There seems to be a typo in the following line. The '6' should be a '7'. The existing code will never settle to a stable value other than 0 (zero).

alt.EstAlt = (alt.EstAlt * 6 + BaroAlt) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)

The corrected line appears below

alt.EstAlt = (alt.EstAlt * 7 + BaroAlt) >> 3; // additional LPF to reduce baro noise (faster by 30 µs) // corrected

Please review and comment



I have applied both of these fixes to my FC. The altitude hold is better.

Dennis
Attachments
deadband.png
(8.72 KiB) Not downloaded yet

ravid824
Posts: 26
Joined: Wed Jul 29, 2015 3:59 am
Location: California

Re: MW2.4 IMU.cpp bugs?

Post by ravid824 »

Dennis
Thanks for sharing. The ALT Hold on Multiwii is broken since a while.
I was reviewing previous Multiwii releases and the alt.EstAlt * 7 was always set to 6.
Do you want to say that it didn't work also on the older releases?
Can you also explain more on what the Deadband is used for and how it affects the ALT HOLD?
David

User avatar
Plüschi
Posts: 433
Joined: Thu Feb 21, 2013 6:09 am

Re: MW2.4 IMU.cpp bugs?

Post by Plüschi »

In 2.2 it was

Code: Select all

EstAlt = (EstAlt * 6 + BaroAlt * 2) >> 3;

which unecessarily ugly but correct.

in 2.3 i have

Code: Select all

alt.EstAlt = (alt.EstAlt * 6 + BaroAlt ) >> 3; 

which is simply wrong.

I did never bother to get 2.4 ...

ravid824
Posts: 26
Joined: Wed Jul 29, 2015 3:59 am
Location: California

Re: MW2.4 IMU.cpp bugs?

Post by ravid824 »

If you pay attention in version 2.3 and 2.4 the following was changed compared to 2.2.
if(calibratingB > 0) {
logBaroGroundPressureSum = log(baroPressureSum);
baroGroundTemperatureScale = ((int32_t)baroTemperature + 27315) * (2 * 29.271267f); // 2 * is included here => no need for * 2 on BaroAlt in additional LPF
calibratingB--;
}
I don't know how it affects, but they say that the '"2 * "added above should compensate the missing "*2" from the alt.EstAlt = (alt.EstAlt * 6 + BaroAlt ) >> 3;

Dennis D
Posts: 16
Joined: Wed Jun 24, 2015 1:48 am

Re: MW2.4 IMU.cpp bugs?

Post by Dennis D »

All,

Regarding deadband.....

The idea of a deadband is to quiet "chatter" (noise) around a particular value (value = 0 for the IMU.cpp cases) and not affect other values outside of the deadband.
Identical deadband functions can be cascaded (in series) and still produce the same output. e.g. in-deadband values = 0, non-deadband values = value.
Cascading several identical existing MW deadband functions won't produce the same output e.g. in-deadband values = 0, non-deadband values = value +/- (deadband * #_of_deadband_functions).
The althold is based on the barometer and ACC(z) which are noisy data sources.
The author was trying to quiet this down mainly when in a steady state condition such as hovering.

IMO: replace the MW deadband function with standard type deadband function.


Dennis

Lecostarius
Posts: 46
Joined: Mon Oct 20, 2014 11:29 pm

Re: MW2.4 IMU.cpp bugs?

Post by Lecostarius »

Many thanks for sharing your observations!

I guess the code with the alt estimation is correct as it is currently in 2.4. If you modified the 6 by a 7, what happens is that the relative impact of the new noisy barometer reading is reduced (a bit), and in general, your results are too big by a factor of 9/8 or 12.5%. Since we do not hold altitude at an absolute level, that 12.5% deviation did not matter to you in your flight experiments. If you saw that your alt hold is improved, it means that your barometer readings are more noisy than the ones of the original code developer, since you get better results using more smoothing. It is quite possible that the settings for alt hold are sub-optimal, at least I read a lot of negative comments about alt hold, so it might well be that more smoothing of the baro readings is the way to go.--

Regarding the deadband, I think it has been introduced for owners of cheap Transmitters that do not return to zero if the sticks are released. Therefore, this pilots face the situation that in neutral stick position their copter flies somewhere instead of hovering over a fixed position. Deadband avoids that. Now the question is, how to deal with the owner of a cheap transmitter that *wants* to move the copter somewhere and moves the stick away from his not-so-neutral position. First, nothing happens - the stick is still within the deadband range. If the user moves the stick more, in your solution, the slope of the control is very strong and the copter will essentially "jump" from e.g. 0 degree tilt directly to, say, 5 degree tilt. It is close to impossible to achieve a 2 degree tilt, since your suggested deadband function is extremely steep in this region; that might be possible for the owner of a high precision transmitter, but for the owner of a cheap transmitter, it is outright impossible.

So my feeling is that the current design of the deadband is done with owners of cheap equipment in mind. We dont typically cascade deadbands, so there is no major drawback of the current implementation. My vote would be for keeping it as it is.

Cheers, Leco

Dennis D
Posts: 16
Joined: Wed Jun 24, 2015 1:48 am

Re: MW2.4 IMU.cpp bugs?

Post by Dennis D »

I found this link that pertains to the IMU.cpp code:
https://git.ece.iastate.edu/microcart/microcart/blob/570004ca5732a7d0a81adbd719c66d07b9d465c7/misc/Multiwii/MultiWii/IMU.cpp

The following is an excerpt from the link:

if(calibratingB > 0) {
logBaroGroundPressureSum = log(baroPressureSum);
baroGroundTemperatureScale = (baroTemperature + 27315) * 29.271267f;
calibratingB--;
}

// baroGroundPressureSum is not supposed to be 0 here
// see: https://code.google.com/p/ardupilot-meg ... P_Baro.cpp
BaroAlt = ( logBaroGroundPressureSum - log(baroPressureSum) ) * baroGroundTemperatureScale;

alt.EstAlt = (alt.EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)

end of excerpt...

Notice the purple highlighted lines.
baroGroundTemperatureScale is computed differently for the excerpt vs. the 2.4 MW code!
This will cause BaroAlt to be computed differently!

I think the excerpt (from the above link) is correct. Do you agree?

Dennis

Lecostarius
Posts: 46
Joined: Mon Oct 20, 2014 11:29 pm

Re: MW2.4 IMU.cpp bugs?

Post by Lecostarius »

Here is the MW2.4 code:

Code: Select all

 if(calibratingB > 0) {
    logBaroGroundPressureSum = log(baroPressureSum);
    baroGroundTemperatureScale = ((int32_t)baroTemperature + 27315) * (2 * 29.271267f); // 2 *  is included here => no need for * 2  on BaroAlt in additional LPF
    calibratingB--;
  }

  // baroGroundPressureSum is not supposed to be 0 here
  // see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp
  BaroAlt = ( logBaroGroundPressureSum - log(baroPressureSum) ) * baroGroundTemperatureScale;

  alt.EstAlt = (alt.EstAlt * 6 + BaroAlt ) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)

Differences in barometric pressure (computed by the term

Code: Select all

( logBaroGroundPressureSum - log(baroPressureSum) )

are scaled by the sensitivity of the sensor ("baroGroundTemperatureScale") to arrive at the altitude estimate.
The sensitivity is multiplied by 2 in MW2.4 code, so that we always estimate twice the true altitude value. However, that is compensated by omitting the factor 2 in the application of the LP filter / smoother in

Code: Select all

 alt.EstAlt = (alt.EstAlt * 6 + BaroAlt ) >> 3;


So I think that this part is okay in MW2.4.

Cheers, Leco

Dennis D
Posts: 16
Joined: Wed Jun 24, 2015 1:48 am

Re: MW2.4 IMU.cpp bugs?

Post by Dennis D »

So...
What was the reason for writing the code like that?
Why not include comments that explain this?
It is open source and other people will look at it.

ravid824
Posts: 26
Joined: Wed Jul 29, 2015 3:59 am
Location: California

Re: MW2.4 IMU.cpp bugs?

Post by ravid824 »

Thank you all for the excellent discussion.
My understanding from the conversation is that the "deadband" should not be touched (thanks Leco for the explanation).
However if one has noisy Baro trying to change the values to 7 at "alt.EstAlt = (alt.EstAlt * 6 + BaroAlt ) >> 3;" might be a good direction.
I have standard Baro used by the Crius AIO Pro V2 (I think MS5611). I read that some has success of ALT HOLD with the same board.
My problem is that the ALT Hold is very sensitive and very difficult to tune the PID (when I set the PID, P=0 I=0 I can't go with the D to more than 2-3. Higher values cause the copter jump like a rocket to the sky.
There is a way that my board is exposed to a lot of vibrations which is the cause of the problems?
I want to try dumping these vibrations with better rubbers, do you think it might help?
David

ravid824
Posts: 26
Joined: Wed Jul 29, 2015 3:59 am
Location: California

Re: MW2.4 IMU.cpp bugs?

Post by ravid824 »

Another question I have.
Do you know if the MW team are considering officially adding SONAR support in the next release?
This might help a lot at the lower levels.
David

User avatar
Plüschi
Posts: 433
Joined: Thu Feb 21, 2013 6:09 am

Re: MW2.4 IMU.cpp bugs?

Post by Plüschi »

Dennis D wrote:What was the reason for writing the code like that?


Omitting that factor 2 in the filter and moving it up some lines sure makes it unreadable. And it makes Baro_Alt have the wrong value. Someone fucked up the code to save 30us. Consider it to be Hanlon's Razor.

Using

Code: Select all

alt.EstAlt = (alt.EstAlt * 3 + BaroAlt ) >> 2;
instead of

Code: Select all

alt.EstAlt = (alt.EstAlt * 6 + BaroAlt * 2 ) >> 3;
nobody can steal the 2 and we still save those essential 30us :twisted:

Dennis D
Posts: 16
Joined: Wed Jun 24, 2015 1:48 am

Re: MW2.4 IMU.cpp bugs?

Post by Dennis D »

Thank you for answering the question....
It is what I thought...
Corrects a bug and also eliminates 1 runtime multiplication...

Dennis

Dennis D
Posts: 16
Joined: Wed Jun 24, 2015 1:48 am

Re: MW2.4 IMU.cpp bugs?

Post by Dennis D »

I just found out what 'Hanlon's Razor' means....
hehe...
And...yes I agree.
Thanks for that....

Dennis

Lecostarius
Posts: 46
Joined: Mon Oct 20, 2014 11:29 pm

Re: MW2.4 IMU.cpp bugs?

Post by Lecostarius »

@Plüschi: thanks for the suggestion. It is much better readable like this.
@ravid: I have a CRIUS AIOP and ALT HOLD is working. It is stupid, of course - I fly in hilly terrain, and it holds altitude over sea level, which means that it does not hold altitude above ground if going uphill or downhill... so yes, Sonar would come in handy and I am planning to implement Sonar to cure that behaviour :-)

That said, I would suggest the following:

1. Cover the hole in the barometer with something that breathes air, but is pitch black. You absolutely do not want light to enter through the hole, or else you will experience lots and lots of drift and noise from the baro.
2. Cover the hole in the barometer with something that prevents that wind goes in. Of course it must not be airtight. But its OK if it is not so easy to blow air through it, like thick foam.
3. absolutely, re-check item #1, seriously.
4. I am not sure since for me it worked out of the box, but setting both I and P to zero looks strange. D is the value that is most noise sensitive. Why dont you put P up a bit and then fiddle around with I and P? Dont use D at all on noisy sensors.
5. Vibrations are not terribly bad for ALT HOLD. I dont think that your current difficulties are due to vibrations...

Please keep us updated.

Cheers, Leco

ravid824
Posts: 26
Joined: Wed Jul 29, 2015 3:59 am
Location: California

Re: MW2.4 IMU.cpp bugs?

Post by ravid824 »

Leco,
I followed the ALT Hold PID calibration process. While P & I = 0 , play with D until I you Alt Hold for short period.
And then play with P until it holds for longer time. The I value as it mention is targeted to compensate for battery drop. It is still a valid process for version 2.4?
My problem is that the D is so sensitive that I can't go forward with the PID tuning.
When I set the default PID values it is also behave crazy.

Dennis.
Regarding the conversation above, at the end I am slightly confused. Beside of the code more readable, should I make any changes to the 2.4 code in regard to the lines below
baroGroundTemperatureScale = ((int32_t)baroTemperature + 27315) * (2 * 29.271267f); //
alt.EstAlt = (alt.EstAlt * 6 + BaroAlt ) >> 3;

Should I revert it back to the previous code eg.
baroGroundTemperatureScale = ((int32_t)baroTemperature + 27315) * 29.271267f); //
alt.EstAlt = (alt.EstAlt * 6 + BaroAlt * 2) >> 3;

David
David

Dennis D
Posts: 16
Joined: Wed Jun 24, 2015 1:48 am

Re: MW2.4 IMU.cpp bugs?

Post by Dennis D »

David

The code should be reverted to:

Code: Select all

baroGroundTemperatureScale = ((int32_t)baroTemperature + 27315) * 29.271267f;

...and...

Code: Select all

alt.EstAlt = (alt.EstAlt * 6 + 2 * BaroAlt) >> 3;

The reverted code corrects the calculation of 'baroGroundTemperatureScale' : this fixes a bug.
It should be noted that the correct calculation of 'baroGroundTemperatureScale' causes the subsequent calculation of the value of 'BaroAlt' to be correct.
The reverted code keeps the same damping affect (depth of damping) as the original code (which is correct).
Note: incorrect comments have been removed from reverted code.

Dennis

Dennis D
Posts: 16
Joined: Wed Jun 24, 2015 1:48 am

Re: MW2.4 IMU.cpp bugs?

Post by Dennis D »

David

Just an additional note....

Code: Select all

alt.EstAlt = (alt.EstAlt * 6 + BaroAlt * 2) >> 3;

can be simplified to:

Code: Select all

alt.EstAlt = (alt.EstAlt * 3 + BaroAlt) >> 2;

This yields correct calculation and identical damping effect and eliminates an un-neccessary runtime multiplication operation. (faster is always better)
I think this also helps readability of the code and eliminates some 'ugly'.

Dennis

Lecostarius
Posts: 46
Joined: Mon Oct 20, 2014 11:29 pm

Re: MW2.4 IMU.cpp bugs?

Post by Lecostarius »

David,

I am not the best person to comment on the tuning process for Alt Hold PID since I did not have to tune it, it worked out of the box.

I hope someone reads this who can help... anyway, if I find the time I will try myself the tuning and see how it behaves. If I do so, I will keep you updated.

Did you make sure that your baro is shielded from light and from wind?

Best, Leco

ravid824
Posts: 26
Joined: Wed Jul 29, 2015 3:59 am
Location: California

Re: MW2.4 IMU.cpp bugs?

Post by ravid824 »

thanks Leco
Yes I did cover the Baro at all of the experiments I did.
David

Lecostarius
Posts: 46
Joined: Mon Oct 20, 2014 11:29 pm

Re: MW2.4 IMU.cpp bugs?

Post by Lecostarius »

@ravid

can you send me the URL of the description how to tune the ALT hold PID?

I tried that myself today. All I can say is that D is required, which is probably due to a huge amount of smoothing that is applied to the Baro readings, and hence a large time delay. For me, large values of D (30 to 40) worked best. I ended up 0.025 and P fairly large, in the 12..15 range.

That was my result. With too little D, the reaction was so delayed that I got a crash landing once ...

Hope that helps.

Cheers, Leco

Lecostarius
Posts: 46
Joined: Mon Oct 20, 2014 11:29 pm

Re: MW2.4 IMU.cpp bugs?

Post by Lecostarius »

From your description I assume you might have looked at a version of the following:

https://dl.dropboxusercontent.com/u/805 ... 0Hold.html

I checked the code in 2.4 in IMU.cpp and it does not look as if the description in that document is accurate at all. In particular, the relative importance of accelerometer versus barometer is fixed at 1.5%/98.5% (accel is used to estimate speed up/down) and you cannot modify that.

The meaning of P,I,D is pretty much what you would expect and is by no means what is described in the document I posted above.

So, if you follow the document and put P and I to zero you are likely going to crash. I would expect completely crazy up/down swings - similar to what you saw. That document might refer to a different implementation, not the one we currently have.

Just my two cents.

Cheers, Leco

ravid824
Posts: 26
Joined: Wed Jul 29, 2015 3:59 am
Location: California

Re: MW2.4 IMU.cpp bugs?

Post by ravid824 »

Leo
Yes this is the link I was referring too. And this is why I wondered and asked the questions at the other post
http://www.multiwii.com/forum/viewtopic.php?f=8&t=6660
after Mr. happul3 explained more in details how the Baro PID works.

I agree with you that what is written in this post https://dl.dropboxusercontent.com/u/80566205/not%20remove/tutorials/PID%20Alt%20Hold.html
is wrong and might be related to other implementation. I failed bad to calibrated the PID with this procedure.

BTW, in the code changes you did, do you need the *2
alt.EstAlt = (alt.EstAlt * 3 + BaroAlt *2 ) >> 2; vs. alt.EstAlt = (alt.EstAlt * 3 + BaroAlt ) >> 2;
David

Lecostarius
Posts: 46
Joined: Mon Oct 20, 2014 11:29 pm

Re: MW2.4 IMU.cpp bugs?

Post by Lecostarius »

David,

2nd version is right. The >> operator divides by two as often as its argument says, so by changing >>3 into >>2 we divide by 4, not by 8 any more. To compensate that, we omit the *2.

So it is alt.EstAlt = (alt.EstAlt * 3 + BaroAlt ) >> 2; just as Dennis wrote.

--Leco

edsimmons3
Posts: 100
Joined: Thu Sep 10, 2015 11:29 am

Re: MW2.4 IMU.cpp bugs?

Post by edsimmons3 »

I realise this is less to do with the code and more to do with the docs, but I thought I should leave no doubt...

I *have* crashed because of the documentation suggesting to set altitude P and I to zero and gradually increase D. The docs on the wiki suggest it here http://www.multiwii.com/wiki/index.php? ... titude_PID

This should be changed and I'm willing to do it if given wiki access...

Cheers,
Ed

User avatar
Leo
Posts: 372
Joined: Wed Sep 17, 2014 7:01 am
Location: Germany
Contact:

Re: MW2.4 IMU.cpp bugs?

Post by Leo »

Setting the P and I to 0 and a value of D>0 is the way I set up my alt hold tuning. It hasn't caused any problems for me.

edsimmons3
Posts: 100
Joined: Thu Sep 10, 2015 11:29 am

Re: MW2.4 IMU.cpp bugs?

Post by edsimmons3 »

Hi Leo,

Thanks for the info - I am about to try to set this up again... I've doubled up on my foam over the baro hole to reduce any wind noise & I have completely removed the I2C GPS board for now. The previous crash may have had something to do with it, but altitude hold tuning is proving really hard for my quad.

The quad hovers pretty well throughout battery life with a little bit of throttle expo tweaking to get the power where needed mid-stick, but the altitude has been a fearful thing to tune!

Thanks,
Ed

ravid824
Posts: 26
Joined: Wed Jul 29, 2015 3:59 am
Location: California

Re: MW2.4 IMU.cpp bugs?

Post by ravid824 »

Same here, I had several crashes and I gave up on MW ALT HOLD.
I couldn't tune it, it might be related to the BARO quality or sensitivity, but I believe that is also connected with the PID algorithm.
As I mentioned in the other post, I tried the Cleanflight firmware and it started to work immidetaly with the default setting. There are using an improved renew PID which is extracted from the MW. The cleanflight is very recommended for those who need ALT and POS Hold. I wish that Multiwii will do the same and make some improvements in these areas.

edsimmons3
Posts: 100
Joined: Thu Sep 10, 2015 11:29 am

Re: MW2.4 IMU.cpp bugs?

Post by edsimmons3 »

I had a look at cleanflight's PID for alt hold. See https://github.com/cleanflight/cleanfli ... old.c#L186 for cleanflight vs https://github.com/multiwii/multiwii-fi ... U.cpp#L319

I think the discussion surrounding the PID in the other forum thread viewtopic.php?f=8&t=6738 is heading in the right direction - I would like to see this improved...

I hope to get a chance to rewrite the pid soon, it's an odd implementation! P and I both suffer at present (I think...) because the altitude that's fed to the PI part of the controller is just derived from the baro.

Thanks,
Ed

Post Reply