Re: ArduPilot AP_InertialSensor_MPU9150 userspace driver

[Andrew Tridgell]

Hi Victor,

> I'm correcting some of the links (commits have messed them up). I'd really
> appreciate if you could clarify this matter to me

sure. It is a little complex, so no wonder it needs some explanation!

before I start I should mention that I have rebased your current code on
top of ardupilot master and put the result here:

https://github.com/tridge/ardupilot/tree/victor-wip

it is a good idea to rebase regularly when doing development. In this
case there were some conflicts because of the VRBrain merge, but they
were easy to resolve.

> - the update method<https://github.com/BeaglePilot/ardupilot/blob/master/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp#L1126> which
> i guess is called by the scheduler somewhere, waits until

update() in AP_InertialSensor drivers is called by the AHRS code when it
is asked to update the AHRS status. Specifically in
AP_AHRS_DCM::update(), which is called from primary AHRS scheduler task
in each vehicle. That means it is called at 400Hz in copter and 50Hz in
plane and rover.

The job of the update() call in the INS drivers (INS ==
AP_InertialSensor) is to fill in the gyro and accel vectors in the INS
object with the latest available values.

The key to understanding update() is that there are two valid but quite
different approaches to implementing it:

1) it could average all of the samples that have arrived from the
sensor since the last update() call, and use that average for the
gyro and accel vector. This is the strategy used on the APM2, as it
is computationally very cheap. It relies on the filters built into
the sensor in combination with the averaging process being
sufficient to reject noise and any aliasing.

2) it could instead inject each sample from the sensor into a more
complex software filter, and then take the latest value from the
filter output as the current estimate of the accel and gyro
vectors. This is computationally more expensive, but allows for a
more tailored filter to be used in an attempt to cope better with
vibration and aliasing. This is the stratgy used on the PX4 and
Pixhawk where the additional computational complexity is cheap
enough to be a reasonably strategy.

In combination with the above two approaches there are multiple valid
approaches to gathering the raw sensor data. This is a separate
implementation decision from how filtering is done.

1) the first approach is to use a fast timer (say 1kHz) and as each
timer goes off we read the sensor and update a sum[] buffer, the
calculate the average when update() is called. This is what is done
by the L3G4200D driver and the MPU9250 driver that you have been
working on. You can tell they use this method as they register a
"timer process" (a 1kHz callback) via the
hal.scheduler->register_timer_process() function.

2) the second approach is to let samples accumulate in either the
sensors FIFO hardware buffer or (equivalently) in a lower level
software buffer (say in the PX4Firmware driver) and then to only
access those when update() is called. This means the driver code
runs less often, but relies on there being some place (either
hardware or software) which can buffer the higher rate samples.

The right strategy for this second design decision depends on the
hardware, for example the way the FIFO works, how long it takes to read
the FIFO samples etc.

> _gyro_samples_available>=_gyro_samples_needed (link to the code point<https://github.com/BeaglePilot/ardupilot/blob/master/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp#L1105>
> ). _gyro_samples_needed is defined here<https://github.com/BeaglePilot/ardupilot/blob/master/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp#L364> for
> different rates. I'm guessing that at 400 Hz, the value of this parameter
> should be 2 but i don't understand the logic behind. What is the exact
> meaning of this parameter?

_gyro_samples_needed isn't a parameter, it is a private implementation
detail of the driver. So I'm guessing what you are really asking about
is the Sample_rate parameter that is passed to init_sensor(). That is an
enumerated value that tells the driver what rate the vehicle code will
be expecting to get samples at. The possible values are 50Hz, 100Hz,
200Hz and 400Hz. The copter code on Linux currently asks for 400Hz. The
plane and rover code asks for 50Hz.

So if the vehicle code calls init_sensor(AP_InertialSensor::RATE_50HZ)
then it is saying "I will be calling update() at an average of 50Hz, so
every 20ms and I expect you to give me new samples at that rate".

The job of the sensor driver is then to provide samples at that rate as
best it can. How it does that depends on the low level sample rates that
the sensor supports.

When I see this code:

case RATE_400HZ:
_default_filter_hz = 20;
_sample_period_usec = 2500;
_gyro_samples_needed = 2;

then that tells me the driver must be thinking that low level sensor
samples will come in at 800Hz. It is then setting up a convenient
counter saying "I need to get 2 gyro samples before I am ready for a new
update()"

This may seem a bit strange, and indeed it is. The reason is works this
way is that the AP_InertialSensor driver acts as a metronome for the
whole vehicle. We use the arrival of samples in the gyro driver as the
key "clock" which drives all of the rest of ardupilot. To achieve this
the top level vehicle code calls:

ins.wait_for_sample(1000);

that says "spin chewing CPU time until the next gyro sample is
available". It is called when the top level code has finished all the
processing it needs to do for the last timestep, and has nothing more to
do until another gyro sample arrives.

By using this ins.wait_for_sample() approach we ensure that the timing
of the start of each "tick" in the code is as accurate as we can make
it. The timing of gyro sample arrivals is critical to the autopilots
function, which is why we let it drive the whole of the timing of the
system.

Now is where things start to get a little complex ..... (yep, even more
so!)

Now let's go back and look at the two filtering strategies at the start
of this email. If you choose to use a "average the last N samples"
approach then it makes perfect sense to wait for exactly N samples to
arrive from the gyro and then to declare that a new sample is available
when that happens. The above code that sets up _gyro_samples_needed is
assuming that approach. It is setup for a sensor that is configured to
produce samples at 800Hz, then it says "we will wait for 2 to arrive,
and then we know we are right on time and have a good average".

That approach doesn't work for all hardware. For example on the Pixhawk
we have two gyroscopes. One produces samples at 800Hz (the L3GD20) and
the other produces samples at 1kHz (the MPU6000). There is no integer
multiple of 400Hz that gives 1kHz, so this "wait for N samples" approach
doesn't work. It also doesn't fit neatly with the "push samples into a
filter" approach.

So on Pixhawk (in the AP_InertialSensor_PX4 driver) the
wait_for_sample() function is instead based on a high resolution system
clock. It doesn't care if the right number of gyro samples have arrived
(although it does check that there has been at least one). All it cares
is that the right number of microseconds has passed. It then relies on
the filter for each of its sensors to produce the current gyro and accel
estimate. That filter lives inside the PX4Firmware layer.

On the PXF we also have multiple gyros/accels, just like on the
Pixhawk. They also run at different hardware sample rates. That means we
can't use the "wait for N samples" approach. Instead we need to use an
approach like on the PX4 where the wait_for_sample() code is based on
the system clock, and where we rely on the filter to give the current
estimate based on whatever samples we happen to have gathered at the
time.

I hope that explains things a bit better! My apologies for such a wordy
response, but hopefully if you read it a couple of times it may make
some sense.

In summary, I suggest that for the PXF you do this:

1) use a system clock based wait_for_sample() implementation, in
exactly the same way that the AP_InertialSensor_PX4.cpp driver does

2) get rid of _gyro_samples_needed and all the other counters

3) create LowPassFilter2p filter instances for each low level sensor
(one for each axis of each sensor). Setup the filter with the
sample_freq that matches what you program the sensor with
(eg. 800Hz, 1kHz etc, depending on what the sensor is capable
of). Setup the cutoff_freq according to the (very badly named!)
_mpu6000_filter parameter as the other drivers do (the default
handling is a bit tricky there)

3) when wait_for_sample() decides the right amount of time has passed
then ensure all samples from the sensor have been pushed through
the filters, fill in the gyro/accel output vectors with the latest
value from the filters

Cheers, Tridge

PS: I've CCd drones-discuss as I think this may interest some others on
the dev team