[Patch for r1179] Improved Barometer code

[Sebbi]

Hi there,

while tinkering with the main loop and my little accuracy experiment I found a nice way to optimise baro updates for speed and size. Changes towards current dev branch can be viewed here:
https://github.com/sebastianherp/multiw ... roved_baro [source]
https://github.com/sebastianherp/multiw ... roved_baro [diffs]

What changes are in this patch?

• Enhanced taskOrder switch statement in main loop
• Moved the 40 Hz timing from getEstimatedAltitude() to the main loop
• Baro_Update() now combines states, because it is possible to start a new measurement right after an old one was completed instead of waiting for the main loop to call Baro_update() again. It also gets rid of the altitude calculation, and averages the pressure instead (moved from getEstimatedAltitude() into Baro_Common()) which saves 200+ µs when Baro_update() is in the calculation-state.
• getEstimatedAltitude() gets called every 25 ms and calculates the altitude (after 10 seconds to let it settle first) from ground pressure difference (code copied from Ardupilot project). It uses only log() to do that which saves bytes and time (100+ µs) compared to the previous pow() approach

On my copter (ATmega 2560, QuadX, FreeImu 4.3 and SUMPPM) this modification saves 334 bytes and cycle time decreases in those cycles where baro related code is executed. [edit:] And it flies (as posted below) [/edit]

Caveats:

• not tested with a BMP085, but changes included there as well
• don't know how it affects mahowiks BaroPID changes
• ground pressure should probably be also reset(able) when arming the copter/plane
• BaroTemperature is unused, but could be shown somewhere in the GUI or transmitted via other telemetry means to find out how warm/cold it is 200 meters up

Anybody willing to peer review / test this small modification?

Edit (10/10/2012):
Changed the lists to reflect the current state of the patch and updated title (patch is now against r1179)
getEstimatedAltitude() needs to be called in 25 ms steps (40 Hz) or the BaroPID code doesn't work (so I changed it back ... tried 10 Hz). Maybe altitude estimation and BaroPID calculation should be in seperate functions?

[Sebbi]

I now have flown one battery pack with this patch / modification and it worked. Since this was also my first try with the improved ALTHOLD code, I don't know to which modification the ca. 1 meter drop as soon as ALTHOLD is enabled can be attributed to. But it held the altitude fine afterwards.

Test setup was as mentioned above: ATmega 2560 with QuadX, FreeImu 4.3 and SUMPPM enabled and default PIDs.

[copterrichie]

video would be very helpful.

[Sebbi]

video would be very helpful.

I will make one, but i don't think it will be helpful ... the changes are more in execution speed and byte savings than an actual effect on flight dynamics (if one can speed of a dynamic flight when trying to hover). The new alt hold code seems to compensate short term fluctuations of the barometer quite nicely. Maybe I should make a video without that part of the code being active

[pm1]

I got rid of that statement, since those functions didn't seem to take a lot of time to process, especially since the baro functions have their own internal timer and gps should only take time when new data is available. Didn't test sonar yet.

Maybe have a look here: viewtopic.php?f=8&t=2507

On mega2560
GPS_Newdata (serial) max about 5000 us
Baro_Update max about 1000 us
...

on 328P
GPS_Newdata (i2c) about 1000 us

I think, worst case will be about 8000 us.

[Sebbi]

I got rid of that statement, since those functions didn't seem to take a lot of time to process, especially since the baro functions have their own internal timer and gps should only take time when new data is available. Didn't test sonar yet.

Maybe have a look here: viewtopic.php?f=8&t=2507

On mega2560
GPS_Newdata (serial) max about 5000 us
Baro_Update max about 1000 us
...

on 328P
GPS_Newdata (i2c) about 1000 us

I think, worst case will be about 8000 us.

I haven't seen that thread before. Looks like the same "problem"

The Baro_update() here is a little bit faster in worst case since it doesn't calculate the altitude anymore. I agree that if all those calls take their respective max time in the same cycle that can't be good for the PID controller at the end of the cycle. But the next cycles should quickly compensate and I'd argue nobody would notice the difference ... still, having PID controllers that adjust for different cycle time would be nice.

[pm1]

... still, having PID controllers that adjust for different cycle time would be nice.

Or ensure, that it is called at about let*s say 5 ms. In the rest of the time I look, if something else has to be done. I will try to readout the Gyro each 2ms, I think, the rest is not very time critical at all... I will go this way.

[Sebbi]

By "time critical" ... do you mean that is needs to be done in certain intervals or that it should be done as fast as possible?

Regarding the code ... anybody had a look at it? Do you think it is an improvement for how it's done now?

[Hamburger]

Can only look next week.
But I think constant as low as possible cycle time for gyro acc and compute are mandatory for flight performance.

[Alexinparis]

Hi,

The main loop had a switch statement which executed only one of 5 functions (Mag_getADC, Baro_update, getEstimatedAltitude, GPS_NewData and sonar/landing lights) in every cycle. I got rid of that statement, since those functions didn't seem to take a lot of time to process

The purpose of the switch statement is to ensure nor more than one "extra" computation is done in the same loop.
The basic idea is to minimize the (max-min) loop time. It's probably not the best way to do this because we don't take into account the task computation weight in the switch, nor the priority of the tasks.
You took time to measure each computation time, it could be a nice input to reorganize task order to minimize the time jitter.

Your code doesn't prevent a loop time accumulation which could lead to quite high spikes.

I like the possibility to get rid of pow approximation.

Or ensure, that it is called at about let*s say 5 ms. In the rest of the time I look, if something else has to be done. I will try to readout the Gyro each 2ms, I think, the rest is not very time critical at all... I will go this way.

As long as the gyro is read as fast as possible, I'm agree with you for the other sensors.
But setting a high delay in order to have a constant delay is not an option if the gyro measurement follows the same constraint.

[Sebbi]

my 2 cent on "timing":
1) gyro, PID controllers and motor pwm should be done as fast as possible and doesn't need to be done in constant time intervals as long as the PID controller takes the time since the last step into account (see great Wikipedia article about PID controllers)
2) the rest can be done with the desired or sensible frequency to get good enough results. I don't think that the copter needs to correct its orientation 300 times per second when in angle mode, since I can hold it level just fine with my sub-300-Hz-fingers in acro mode

But that belongs in a different thread, I believe.

@alex:
ok, I will change that part of the patch back to the "taskswitcher" and measure the timing of the subfunctions ... maybe they can be combined into less switch-cases.

[Sebbi]

Hi,

The main loop had a switch statement which executed only one of 5 functions (Mag_getADC, Baro_update, getEstimatedAltitude, GPS_NewData and sonar/landing lights) in every cycle. I got rid of that statement, since those functions didn't seem to take a lot of time to process

The purpose of the switch statement is to ensure nor more than one "extra" computation is done in the same loop.
The basic idea is to minimize the (max-min) loop time. It's probably not the best way to do this because we don't take into account the task computation weight in the switch, nor the priority of the tasks.
You took time to measure each computation time, it could be a nice input to reorganize task order to minimize the time jitter.

Your code doesn't prevent a loop time accumulation which could lead to quite high spikes.

I like the possibility to get rid of pow approximation.

I pushed my current changes to Github: https://github.com/sebastianherp/multiw ... roved_baro

Reintroduced the tastOrder switch statement and made it more efficient so no cycles are wasted. Also found out that my compass only delivers new values every 66,6 ms ... so I put that on a "scheduler" too (see commit message https://github.com/sebastianherp/multiw ... 5d87564d1d).

The code only calculates the pressure when new barovalues are available. Current MultiWii also calculates the altitude everytime which takes up to 300 µs more time. This is now done when we need it (in getEstimatedAltitude) and the log version saves some µs, too ... and it even accounts for temperature changes

P.S.: Not flight tested.

[Sebbi]

Looks like romek_b used one of the ideas already (r1174). I merged my branch with the current _shared branch and adapted his changes so it works with the other aspect (don't calculate altitude in Baro_Update()). Updated subject of thread to reflect that ...

[Sebbi]

Changed the getEstimatedAltitude() frequency back to 40 Hz (was 10 Hz), because the new BaroPID algorithm relies on it being 40 Hz.

And it's flight tested again ... works and I attribute all the flaws to BaroPID calculation and wrong PID values on my side. Why? Because the readout on my smartphone accurately showed the height and height changes ... will redo that flight test and log the data to confirm that though.

[Sebbi]

A video of the testflight:
http://www.youtube.com/watch?v=i0dX4NYVpk4

Next step: figuring out why the altitude dropped when alt mode was activated.

[Alexinparis]

some notes:

I think this task management is no doubt better than the previous one.

Code: Select all

Mag_getADC();       // avg 21 µs, max 350 µs (HMC5883)

Don't forget there is a 100ms inside Mag_getADC()
this delay should be in fact removed and put in the sheduler.

Code: Select all

GPS_NewData();        // averages 240 µs, max 2400 µs (10 Hz serial GPS, r1172)

this should be much better since r1176

Code: Select all

BaroAlt = log( BaroGroundPressure / (BaroPressureSum/(float)(BARO_TAB_SIZE - 1)) ) * (BaroTemperature+27315) * 29.271267f; 

Do you have some graph showing the inaccuracy of this approximation with the pow() implementation ?
(just to know where we go with this approximation)
I'm surprised a Temperature term is used here since it is already compensated in BaroPressure formula.

[Sebbi]

Don't forget there is a 100ms inside Mag_getADC()
this delay should be in fact removed and put in the sheduler.

I will move it tomorrow. Didn't notice this. I will also time GPS_NewData() again

Regarding the pressure-2-altitude conversion:
1) The currently used pow() function calculates height above sealevel. This means it assumes that sealevel (0m) has a pressure of 1013,25 hPa at a temperature of 15°C
2) The log() method with the same input variables (1013,25 hPa ground pressure and 15°C) delivers almost the same values up to 100m. It then rapidly diverges with an error of around 100+ meters 1 km above sealevel. But to be fair ... the pow() method isn't exact either since the pressure in 20 km pressure should still be 55 hPa, but the method would return only an altitude of 1887 m (see http://en.wikipedia.org/wiki/Barometric_formula)
3) Temperature is used to calculate the calibrated pressure value, but temperature is also responsible for pressure/air density changes. The log() method seems to account for that AND returns relative altitude when a ground pressure measurement is available.

Both furmulas assuming ground is standard sealevel:

Sealevel at 30°C:

Some measured ground pressure:

[Sebbi]

Hi,

the code made it into the software, thanks.

But I found a small bug (looks like the code was manually changed instead of applying a patch):

Code: Select all

baroTemperature = b5; // in 0.01 degC (same as MS561101BA temperature)

in i2c_BMP085_Calculate() in Sensors.ino should be:

Code: Select all

baroTemperature = (b5 * 10 + 8) >> 4; // in 0.01 degC (same as MS561101BA temperature)

note: I had this wrong in my code too (the +8 was multiplied by 10, too), but the rest was there. B5 needs to be multiplied by 10 and then divided by 16, or the temperature will be off by a factor of 1.6 for the BMP085. That would lead to a calculated altitude that is 3.1% too high