[DO NOT MERGE] initial SPI DMA IMU driver (mpu9250) for testing/review/discussion

[dagar]

Project board for the larger effort: https://github.com/PX4/Firmware/projects/23

This is an initial prototype mpu9250 driver using SPI DMA on NuttX reading all raw data from the sensor. There's still quite a lot to do before this will be ready for PX4 master, but it should be mostly working on a pixracer (at least for bench testing).

The mpu9250 accel and gyro are read at 4 kHz. In an mpu6500 (or mpu9250 ignoring mag) all 32 kHz of raw gyro data is transferred.

• includes several ideas from the Drivers code cleanup (Drivers Code Cleanup #9279)
• includes ModuleBaseMulti (ModuleBase allow multiple instances of a module #9143)
• introduces base classes for Accel/Gyro/Mag (calibration, filtering, integration, publishing)

TODO

• work queue with thread per bus needed PX4 general work queue #8814
  • this is temporarily using a task per mpu9250 instance
• review ModuleBaseMulti and merge separately
• review mpu9250 i2c only mode (crazyflie or external mpu9250)
• review SPI DMA on all desired NuttX boards and mpu9250 fallback mode
• posix and qurt compatibility
• clip counters
• split sensor messages for control (1 kHz gyro publication)
• From Jake: I've got calibration working. I've got more work on my private repo I'll push up Monday. Tackling one last bug before it can fly. I think the problem is a non-atomic read fifo_count follow by read fifo. If something preempts between those operations I can get an overflow without knowing it. I'll confirm or deny this Monday.
• NuttX side - WIP: STM32F7 DMA per SPI NuttX#25

[bkueng]

Awesome! Great to see progress on this.

I see that the filter is applied to the downsampled 1kHz signal. I think it should be done on the raw signal though (at least for the control path).

@dakejahl do you have a log (with high-rate logging)?

[dakejahl]

Heres a log: https://review.px4.io/plot_app?log=06a7a485-5c02-4d97-9bf8-eb05f7569004

It's not high rate. Even with high rate logging the logger only can run at ~250hz. Anything we can do to increase this?

As you can see in this log there's still some work to do to clean up the data.

[bkueng]

Even with high rate logging the logger only can run at ~250hz. Anything we can do to increase this?

You can reduce the log interval here: https://github.com/PX4/Firmware/blob/master/src/modules/logger/logger.cpp#L283.
Or you can let it directly poll on the topic when starting it manually: logger start -t -b 14 -p sensor_gyro. In terms of CPU load I'm not sure how high you can go with the logging rate, we'll probably have to optimize for that.

[dakejahl]

@bkueng @dagar Maybe we could talk about this tomorrow during the dev call? Or please feel free to add to this PR, I honestly don't really know what I am doing 😆

I think maybe part of the problem is I am integrating the down sampled data but not filtering it?

[dakejahl]

21% cpu for the 6500 (32khz gyro data) -- (without prefilter it runs at 11%)
11% cpu for the 9250 (only 4khz gyro) -- (without prefilter it runs 9%)

This is running a filter over the raw data before down sampling to 1khz. Floating point math :(

			accel_x += _accel_prefilter_x.apply(int_accel_x[i]);
			accel_y += _accel_prefilter_y.apply(int_accel_y[i]);
			accel_z += _accel_prefilter_z.apply(int_accel_z[i]);

			gyro_x += _gyro_prefilter_x.apply(int_gyro_x[i]);
			gyro_y += _gyro_prefilter_y.apply(int_gyro_y[i]);
			gyro_z += _gyro_prefilter_z.apply(int_gyro_z[i]);

Heres a log of a flight with this code: https://review.px4.io/plot_app?log=282df03c-b320-45a9-8dad-952381b05054

It actually flies! Clearly I needed to filter before downsampling, thanks @bkueng! Next step is to optimize the filter code.

[mhkabir]

Are you adding filtering to the estimator path?

[dakejahl]

@mhkabir yes, see src/lib/drivers for the gyro, accel, and mag base classes

[dagar]

This is the design we're working towards while at the same time trying to push all common accel/gyro/mag logic in reusable classes. Ideally this would allow us to move over all the existing IMU drivers, then split sensor_gyro without having to touch every single driver.

The prefiltering should be dropped and we need to look at structuring this such that the integration (estimator path) and filtering (control path) are performed on the raw data (separately).

[dakejahl]

How do we push the raw data to the rest of the system when we are collecting multiple measurements per cycle inside of the driver?

[dagar]

Updated to latest PX4/NuttX#25 and Firmware master.

[dagar]

TODO: review NuttX CONFIG_SPI_TRIGGER and HWFEAT_TRIGGER
https://groups.google.com/forum/#!topic/nuttx/I30Wqws7VSc
PX4/NuttX@8328539

Followup - https://groups.google.com/forum/#!topic/nuttx/4EE8xx-oIGY

[dagar]

NuttX changes to configure DMA per SPI for both stm32 and stm32f7 have been upstreamed.

[dagar]

The NuttX 7.28 update has been merged to master giving us the capability to configure DMA per SPI separately. After the new work queue (#11261) goes in we can start enabling SPI DMA.