fmu-v4/v4pro: switch to new InvenSense icm20602/icm20608g drivers

[dagar]

Based on recent testing results (#14175) I think it's time to start gradually switching to the new InvenSense IMU drivers.

• 8 kHz gyro, 4 kHz accel
• DLPF disabled
• scheduled using data ready interrupts where possible (most boards)
• FIFO is emptied at 1 kHz, but can adjusted via IMU_GYRO_RATEMAX from 250 Hz - 2000 kHz.

Comparison with Master (pixracer multicopter 4001)

• small cpu decrease depending on setup
  • the actual transfer is done with DMA, but each DMA transaction is relatively expensive to setup and start (I'd estimate ~ 50 us), then once it's complete we have significantly more data to process (filter and integrate)
  • cpu usage decreases by about 2% with the default rate (1 kHz like current master)
  • cpu usage decreases by about 5% with IMU_GYRO_RATEMAX = 400 Hz
    • due to the current hardcoded integration time this causes the attitude controller to now run at 400 Hz, but it's still a good cpu decrease overall
• sensor pipeline is effectively unchanged at the moment, but these drivers also publish raw sensor_accel_fifo and sensor_gyro_fifo for logging/analysis and will be used directly later.
• control latency increase due to FIFO transfer time
  • IMU_GYRO_RATEMAX 2000 Hz: 205 us (master) --> 271 us (PR) (NOTE: master limited to 1 kHz)
  • IMU_GYRO_RATEMAX 1000 Hz (default): 205 us (master) --> 360 us (PR)
  • IMU_GYRO_RATEMAX 400 Hz: 205 us (master) --> 639 us (PR)
  • The icm20602 on the pixracer is a particularly bad case because the sensor is 10 MHz max, but due to the board clock configuration it's actually running at closer to 5.5 MHz. In other cases it's not quite as bad (eg mpu9250 @ 20 MHz).
  • If anyone views this as a problem we could probably shave off some latency by emptying the FIFO more intelligently as it fills rather than conservatively checking the number of samples, then transferring only that many.

nsh> listener sensor_accel_fifo

TOPIC: sensor_accel_fifo
 sensor_accel_fifo_s
        timestamp: 21640837  (0.002277 seconds ago)
        timestamp_sample: 21640175
        device_id: 3607306 (Type: 0x37, SPI:1 (0x0B)) 
        dt: 250.0000
        scale: 0.0047
        x: [-52, -56, -55, -55, -56, -56, -47, -59, -45, -54, 0, 0, 0, 0, 0, 0]
        y: [363, 357, 356, 369, 356, 359, 361, 375, 357, 363, 0, 0, 0, 0, 0, 0]
        z: [-2006, -2009, -2007, -2007, -2005, -2013, -2023, -2017, -2013, -2007, 0, 0, 0, 0, 0, 0]
        samples: 10

nsh> listener sensor_gyro_fifo

TOPIC: sensor_gyro_fifo
 sensor_gyro_fifo_s
        timestamp: 216006743  (0.002315 seconds ago)
        timestamp_sample: 216006122
        device_id: 3672842 (Type: 0x38, SPI:1 (0x0B)) 
        dt: 125.0000
        scale: 0.0010
        x: [2, 4, -6, -18, 14, -4, 2, 7, 3, 1, -6, 17, -12, 0, 2, -12, -2, 2, -3, -2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        y: [36, -52, -119, -11, 35, -48, -74, -5, 10, -70, -64, -3, 10, -60, -72, 3, 6, -52, -61, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        z: [7, -12, 8, -9, 6, 6, -4, 14, -9, 3, -2, 2, 8, 0, 5, 5, -25, 14, 2, -11, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        samples: 20

TODO:

• review rotation change (comment below)
• test px4_fmu-v4 (pixracer)
• test px4_fmu-v4pro (pixhawk 3 pro)

[dagar]

@PX4/testflights could you thoroughly test this on any platform with a pixracer or pixhawk 3 pro?

[dagar]

I've taken this as an opportunity to change the rotation convention/interpretation for InvenSense sensors.

In PX4 sensors we want data published with X forward, Y right, Z down (right handed coordinate system).
The InvenSense sensors provide data like this.

On a pixracer this sensor is on the bottom of the board with the sensor's X axis pointed in the direction of flight (to the right below).

Bottom of pixracer, (direction of flight arrow points right)

---->

Currently in PX4 master we start this sensor with a rotation of 2 (ROTATION_YAW_90), which I find a bit unintuitive, especially when things get more complicated (try to make sense of the isolated mpu9250 orientation on a cube...).
The existing mpu6000 driver (with icm20602 support) leaves the z axis as is (sensor z is up, but sensor is mounted upside down on the pixracer), then negate x and swap x <=> y.

New Drivers (icm20602, icm20608g, etc)

In the newer IMU drivers I've started from scratch I've interpreted this differently, starting with the sensor's X axis as an anchor and the fact that the sensor is upside down (on a pixracer), so it's rotation relative to the board is now ROTATION_ROLL_180 (-R 8). Once the drivers assemble the int16 raw data they immediately negate z (beause sensor's +Z is up and we want +Z down) and y (+y is right).

As boards are updated and drivers switched we'll have to update the sensor rotation, but we already have to go through board by board to configure DMA before transitioning. I also considered keeping the sensor's native frame and exposing more metadata, but that seemed like a waste.

Does anyone have an objection with moving forward like this? Is there a more obvious interpretation we should go with? @bkueng @davids5 @julianoes @RomanBapst @dakejahl @mcsauder @dinomani

[dinomani]

think its fine for me, is it consistent with the other imu sensors? like the bosch ? bmi088 [image: image.png] greetz DINO

…

On Wed, Feb 19, 2020 at 4:08 PM Daniel Agar ***@***.***> wrote: I've taken this as an opportunity to change the rotation convention/interpretation for InvenSense sensors. In PX4 sensors we want data published with X forward, Y right, Z down (right handed coordinate system). The InvenSense sensors provide data like this. [image: Screenshot from 2020-02-19 09-29-00] <https://user-images.githubusercontent.com/84712/74843704-4b7ce780-52fa-11ea-8bcc-6816c5c142fb.png> On a pixracer this sensor is on the bottom of the board with the sensor's X axis pointed in the direction of flight (to the right below). Bottom of pixracer, (direction of flight arrow points right) [image: Screenshot from 2020-02-19 09-30-16] <https://user-images.githubusercontent.com/84712/74843826-7b2bef80-52fa-11ea-9e6e-8a31ad59d184.png> ----> Currently in PX4 master we start this sensor with a rotation of 2 (ROTATION_YAW_90), which I find a bit unintuitive, especially when things get more complicated (try to make sense of the isolated mpu9250 orientation on a cube...). The existing mpu6000 driver (with icm20602 support) leaves the z axis as is (sensor z is up, but sensor is mounted upside down on the pixracer), then negate x and swap x <=> y. New Drivers (icm20602, icm20608g, etc) In the newer IMU drivers I've started from scratch I've interpreted this differently, starting with the sensor's X axis as an anchor and the fact that the sensor is upside down (on a pixracer), so it's rotation relative to the board is now ROTATION_ROLL_180 (-R 8). Once the drivers assemble the int16 raw data they immediately negate z (beause sensor's +Z is up and we want +Z down) and y (+y is right). As boards are updated and drivers switched we'll have to update the sensor rotation, but we already have to go through board by board to configure DMA before transitioning. I also considered keeping the sensor's native frame and exposing more metadata, but that seemed like a waste. Does anyone have an objection with moving forward like this? Is there a more obvious interpretation we should go with? @bkueng <https://github.com/bkueng> @davids5 <https://github.com/davids5> @julianoes <https://github.com/julianoes> @RomanBapst <https://github.com/RomanBapst> @dakejahl <https://github.com/dakejahl> @mcsauder <https://github.com/mcsauder> @dinomani <https://github.com/dinomani> — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#14184>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AMHRHANNNPCYWUPUZDNX4OLRDVDP7ANCNFSM4KXRJONA> .

-- www.auterion.com

[dagar]

think its fine for me, is it consistent with the other imu sensors? like the bosch ? bmi088 [image: image.png] greetz DINO

@dinomani No, but the goal would be to do this consistently everywhere. In the Bosch BMI088 case it looks like we're currently doing nothing.

Which means if the sensor was mounted on top of your board already orientated in the direction of flight you'd have to at least rotate it roll or pitch 180. This is the kind of confusing mess I want to eliminate.

For the InvenSense case I think the question boils down to if we keep the sensor's X axis as a starting point anchor or if looking at the device the -Y axis (the top) would be considered forward and therefore mapped to +X.

So that would be...

   X = -Y
   Y = X
   Z = -Z

Yes this annoying, but I'm hoping we can get it settled for good this time.

[jorge789]

Tested on PixRacer V4

Indoor Flight Test
Modes Tested
Stabilized Mode: Good.

Procedure
Arm and Take off in stabilized mode.

Notes
No issues noted, good flight in general.

Log
https://review.px4.io/plot_app?log=afd14494-72d9-4a90-a53c-e8d7968aa457

https://review.px4.io/plot_app?log=5378bcbc-602f-45cb-80af-4f4427568084

https://review.px4.io/plot_app?log=35c18620-51ce-4d96-a354-3ee61e681f6e

https://review.px4.io/plot_app?log=1711530e-df7d-48ab-bf96-a61e075df041

Tested on Pixhawk V4 pro

Indoor Flight Test
Modes Tested
Stabilized Mode: Good.

Procedure
Arm and Take off in stabilized mode.

Notes
No issues noted, good flight in general.

Log
https://review.px4.io/plot_app?log=4d7a467e-8b6f-46cc-9373-c52f9dab4d54

https://review.px4.io/plot_app?log=5daab84c-03df-45c0-b567-0603c430aef3

https://review.px4.io/plot_app?log=7de8166b-67c5-4f20-93c0-f0905cbd744f

https://review.px4.io/plot_app?log=e823e50b-fc1e-4324-aafc-13dfcc8342db

[mcsauder]

@dagar , I'm all for this! Nice work!

[bkueng]

Does anyone have an objection with moving forward like this?

No, just a minor point, as mentioned, requiring to change rotation when switching drivers is an additional source of error.

[dagar]

Does anyone have an objection with moving forward like this?

No, just a minor point, as mentioned, requiring to change rotation when switching drivers is an additional source of error.

Yes, it wasn't my first choice, but I'm committed to making it consistent across the entire code base with a sane convention.

[dagar]

If those control latency changes worried anyone here's a bit more info to show it's not as bad everywhere.

End-to-end control latency of new drivers compared to master.

IMU_GYRO_RATEMAX	master	new icm20602	new mpu9250
400 Hz (interval 2500 us)	205 us	639 us	407 us
1000 Hz (default, interval 1000 us)	205 us	360 us	217 us
2000 Hz (interval 500 us)	NA	271 us	228 us

Due to the clock configuration on the pixracer (px4_fmu-v4) the ICM20602 (10 MHz SPI) is actually only operating at 5.5 MHz. The MPU9250 (max 20 MHz SPI) isn't nearly as bad, but still only operating at 10.5 MHz. This is something we might be able to optimize per board.

[jorge789]

Tested on PixRacer V4:

Modes Tested

Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.
Mission Plan Mode (Automated): Good.
RTL: Good.

- Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceed to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint then swith RTL.

Notes:
No issues noted, good flight in general.

Log: https://review.px4.io/plot_app?log=ee055db8-6654-490a-a034-c28426586269

Log Master comparison
https://review.px4.io/plot_app?log=cf3d0a34-872d-4bae-ac3a-a1168b52c60f

Tested on Pixhawk V4 pro:

Modes Tested

Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.
Mission Plan Mode (Automated): Good.
RTL: Good.

- Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceed to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint then swith RTL.

Notes:
No issues noted, good flight in general.

Log: https://review.px4.io/plot_app?log=131ece49-886e-4f3b-b8d2-f1db63b204d7

Log Master comparison
https://review.px4.io/plot_app?log=b098a198-917e-4b57-baf6-0f09feab0649

[bkueng]

I gave this a quick spin as well, on a Pixracer with MPU9250 + ICM20608 + DShot. CPU load is definitely too high, it was 7% left on idle (vs 12% on master). So I disabled the MPU9250 for flying:

• https://logs.px4.io/plot_app?log=87121a98-453a-4e53-8470-6b577d14ef41
• https://logs.px4.io/plot_app?log=da6ecd25-2ce5-4f6e-a206-68182a0cbf78
• And an older log for reference on the same vehilce: https://logs.px4.io/plot_app?log=d3b93b05-ce85-4297-bd29-a6f16d66d2f5

[jkflying]

I'd be happy with the sample latency going up if it means we can do smarter filtering and get the delay down there instead.

[dagar]

Here's the short term fix to enable these drivers on stm32f7 (fmu-v5 fmu-v5x, etc). PX4/NuttX#85

[dagar]

I gave this a quick spin as well, on a Pixracer with MPU9250 + ICM20608 + DShot. CPU load is definitely too high, it was 7% left on idle (vs 12% on master). So I disabled the MPU9250 for flying:

• https://logs.px4.io/plot_app?log=87121a98-453a-4e53-8470-6b577d14ef41
• https://logs.px4.io/plot_app?log=da6ecd25-2ce5-4f6e-a206-68182a0cbf78
• And an older log for reference on the same vehilce: https://logs.px4.io/plot_app?log=d3b93b05-ce85-4297-bd29-a6f16d66d2f5

Thanks @bkueng, it looks like the ICM20608g was a little worse than the ICM20602 because it was separately sampling temperature every iteration. I have some updates that minimize the temperature update (1 Hz) and also skip the FIFO count check.

Overall this is the difference I'm seeing compared to master (px4_fmu-v4).

 PID COMMAND   CPU(ms) CPU(%)  USED/STACK PRIO(BASE) STATE FD
 153 wq:SPI1      2895 13.420  1168/ 1996 253 (253)  w:sem  5 <-- master mpu6000 (icm20602) @ 1000 Hz
 153 wq:SPI1     24379 10.771  1436/ 1996 253 (253)  w:sem  5 <-- new icm20602 @ 1000 Hz

 153 wq:SPI1      2907  5.758  1504/ 1996 253 (253)  w:sem  5 <-- new icm20602 @ 400 Hz
 153 wq:SPI1     12134 20.262  1568/ 1996 253 (253)  w:sem  5 <-- new icm20602 @ 2000 Hz
  50 wq:SPI1     15894 37.654  1520/ 1996 253 (253)  w:sem  5 <-- new icm20602 @ 4000 Hz

There's also an mpu9250 version of the new driver in a usable state, but I haven't implemented the mag yet.

[dagar]

Branch updated with various improvements from #14208.

• refactor Run() into simple state machine
• all sleeps removed
• perform reset and configuration in sensor bus thread
• when using data ready interrupt skip checking FIFO count
• fix periodic temperature sampling (rate limit to 1 Hz)
• propagate error counts (eg errors in FIFO duplicated accel samples)

Skipping or throttling the additional small (non-DMA) SPI transactions (FIFO count and temperature) can save a few percent cpu per sensor instance (relative to master) while still transferring all raw data.

@PX4/testflights could you please do another round of testing on any px4_fmu-v4 and px4_fmu-v4pro vehicles?

[Junkim3DR]

Tested on Pixhawk 3 v4Pro

Modes Tested

• Position Mode: Good.
• Altitude Mode: Good.
• Stabilized Mode: Good.
• Mission Plan Mode (Automated): Good.
• RTL (Return To Land): Good.

Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceed to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint then trigger RTL.

Notes
No issues noted, good flight in general.

Log

• https://review.px4.io/plot_app?log=016e3c4c-0148-48fa-8251-d50ef49963ba

[jorge789]

Tested on PixRacer V4:
Modes Tested

Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.
Mission Plan Mode (Automated): Good.
RTL: Good.

- Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceed to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint then swith RTL.

Notes:
No issues noted, good flight in general.

Log:
https://review.px4.io/plot_app?log=de640956-b07f-4e37-913d-701bdec1f908

Log
Master comparison
https://review.px4.io/plot_app?log=7d52fb4b-ae33-4d78-bf02-f3022aeffb40

[dagar]

Latest test logs look good. I also did a sanity check on the test rack to compare that all the sensor output is roughly equivalent before/after (device ids, temperature, scaling, etc).

Thanks everyone!