ICM20602 and ICM20608g: new standalone driver (8 kHz gyro, 4 kHz accel) #13103

dagar · 2019-10-06T19:30:24Z

This is a new driver for the Invensense ICM20602 that uses the sensor's FIFO and SPI DMA (on px4_fmu-v4) to transfer raw gyro data at 8 kHz and accel at 4 kHz. It can also do gyro at 32 kHz, but at that point the bus utilization is a bit too high to peacefully coexist with other sensors.

The intent is to keep these opinionated drivers simple and optimized to the point where it'll likely be easier to carry separate drivers for each sensor with minor differences rather than trying to support everything in one place (see current mpu6000 mess). We'll see how this actually pans out for the ICM20608, ICM20689, etc.

Originally I was going to do a pass and enable SPI DMA everywhere first (#12436), but given various problems and delays in that area I think we can move forward incrementally like this.

Features

8 kHz gyro (32 kHz possible)
4 kHz accel
FIFO emptied at 1 kHz (we can consider doing this faster or slower as appropriate)
clip counters
new low pass filter that operates on an array of data (LowPassFilter2pArray)
new sensor messages for better visibility
- sensor_{accel, gyro}_fifo: full raw data for optional logging and analysis
- sensor_{accel, gyro}_status: metadata, clipping, etc
- sensor_{accel, gyro}_integrated: delta angles and delta velocities
- eventually these will replace the existing sensor_{accel, gyro} monolith

Background (notes from @priseborough)

IMU Signal Processing

Failure to detect vibration induced inertial navigation failures early enough during system integration or operation has contributed to a number of vehicle loss incidents. In the majority of cases this has been caused by structural resonances excited by motor and propeller blade bassage frequency vibration.
The current practice of using the IMU’s internal DLPF is a major blocker for the following reasons:

IMU internal sampling is then limited to 1kHz which makes the ADC step susceptible to aliasing on airframes with high RPM/geared motors and high blade count propellers
The use of the IMU’s internal DLPF means that high frequency vibration clipping of the ADC cannot be detected externally until it has affected the INS
Data cannot be logged at the ADC sampling frequency so it is not possible to use a data driven approach to resolving vibration induced navigation failures.

Experience working with companies using PX4 has shown that the majority do not understand how propulsion system forcing, structural dynamics, IMU sampling and vibration isolation mount design interact to affect navigation system performance and for those that do, the inability to log the required data prevents the use of normal analytical methods.

The signal chain for autopilot feedback and navigation needs to be split for the following reasons:

Logging of raw unfiltered data from the sensors ADC is required if an analytical approach to resolving vibration induced navigation failures is to be adopted.
Angular rate control feedback requires data with minimum phase low/group delay with a maximum acceptable level of high frequency content that is application specific.
The best general purpose filter in terms of high frequency noise rejection for a given phase delay measured at the rate controller gain cross-over frequency of 5-15Hz is an IIR second order bi-quad filter with a Q of 1/sqrt(2).
The angular rate controller should be synchronised to the imu to reduce inter-sampling delay.
The angular rate controller should run at a higher rate than the attitude and position/height controllers and therefore needs to run at a faster rate than the INS/EKF.
The angular rate controller may be required to run at 1kHz or higher for some applications.
The INS/EKF uses delta angles and delta velocities that are obtained by integrating high rate gyro and accel measurements in body frame across the INS/EKF update interval.
The INS/EKF is very sensitive to errors caused by clipping and aliasing.
Correction for coning errors introduced when downsampling needs to applied as part of the downsampling integration algorithm.
It is unlikely that INS data would be required faster than 250Hz.

LorenzMeier · 2019-10-06T19:39:56Z

Awesome!

dagar · 2019-10-06T19:41:56Z

FYI @priseborough @RomanBapst

bkueng · 2019-10-07T07:43:04Z

src/systemcmds/top/CMakeLists.txt

@@ -33,7 +33,7 @@
 px4_add_module(
 	MODULE systemcmds__top
 	MAIN top
-	PRIORITY "SCHED_PRIORITY_MAX"
+	PRIORITY "SCHED_PRIORITY_DEFAULT"


This was set to max on purpose. I had several cases where this helped.

I was going to follow up with you. At SCHED_PRIORITY_MAX top can interfere with driver scheduling enough to cause an occasional fifo overflow and reset. I'd like to see if we can find a line in between.

How about an argument to optionally run at max priority, but by default at least lower than the sensor bus WQ threads?

At the time top was bumped to SCHED_PRIORITY_MAX the IMU drivers and a few other things were running out of ISRs (hrt_call_every).

Continued in #13115.

Out of curiosity: is a fifo overflow handled gracefully?

BazookaJoe1900 · 2019-10-07T12:40:01Z

That sound as huge potential for improvement of the flight control! cool.
Is there a theoretical/practical knowledge how its really going to improve the flight control?
Is it going to allow higher vibration tolerance?

msg/sensor_accel_control.msg

msg/sensor_accel_fifo.msg

dagar · 2019-10-07T14:20:53Z

Updated with some background information (thanks to @priseborough).

priseborough · 2019-10-18T05:16:16Z

I did a quick test of this branch with sensor comparison logging on an earlier version of the pixracer with a icm20608g IMU.

The test involved shaking the board vertically with a low amplitude acceleration at three frequency points. The first was a 900 -> 1100 Hz, followed by 3900 => 4100 and then 7900 ->8100.

The following shows the result obtained with the legacy code (log here: https://review.px4.io/plot_app?log=1187443a-c497-4966-93ab-60d99c60ee81)

Both IMU's aliased in both the 1kHz. and 4kHz band. The first third is the 1kHz band testing, the middle third is 4kHz and the last third the 8kHz. There was minimal aliasing at 8kHz.

Repeating the test using the code from this PR (log here https://logs.px4.io/plot_app?log=176fec85-661c-441f-bdf5-07a41811b4e1) shows that IMU0 now no longer aliases at 1kHz, eg

Both continue to alias at 4kHz as expected. The accelerometers on both IMU's lack effective internal anti-aliasing protection, highlighting why a high sampling rate and mechanical vibration isolation is desirable.

priseborough · 2019-10-18T05:25:04Z

BTW, the vibration test setup is a small consumer grade vibration speaker using a mobile phone signal generator app.

jorge789 · 2019-11-05T21:55:13Z

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 Takeoff in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceeds to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint activate RTL.

Notes:
No issues noted, good flight in general.

Log: https://review.px4.io/plot_app?log=44f75460-10ed-4b3a-aaeb-1f3eecc5d0a9

jorge789 · 2019-11-06T21:18:49Z

Tested on PixRacer V4:

Modes Tested

Position Mode: Good.
Altitude Mode: See comments below:
Stabilized Mode: Good.
Mission Plan Mode (Automated): Good.
RTL: Good.
Land. See comments below:

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

Notes:
Please note on altitude mode, when doing pitch and the sticks centered the vehicle would climb altitude. On landing it did a couple small leaps then landed and disarmed. Other than that good flight no visible observations of any issues.

Log: https://review.px4.io/plot_app?log=0511e98f-b0d4-456f-b79a-181676f7a9a9

Tested on Pixhawk Pro V4:

Modes Tested

Position Mode: Good.
Altitude Mode: See comments below:
Stabilized Mode: Good.
Mission Plan Mode (Automated): Good.
RTL: Good.
Land. See comments below:

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

Notes:
Please note on altitude mode, when doing pitch and the sticks centered the vehicle would climb altitude. On landing it did a couple small leaps then landed and disarmed. Other than that good flight no visible observations of any issues.

Log: https://review.px4.io/plot_app?log=aff566df-40f7-41fa-92d7-75e9992c9fa3

bkueng · 2019-11-08T08:51:49Z

boards/px4/fmu-v4/nuttx-config/include/board.h

@@ -269,6 +269,10 @@
 #define GPIO_SPI1_MOSI  (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz)
 #define GPIO_SPI1_SCK   (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz)

+#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1
+#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_2


This one conflicts with DShot (TIM1_UP). We can use DMAMAP_SPI1_TX_1 and move the SDIO to stream 6 (DMAMAP_SDIO_2).

Thanks, I haven't really looked at where things settled after dshot came in. We need to do a pass to get this in place for most of the boards. #12436

I've started capturing the full mapping in board_dma_map.h (next to board.h).

The current state is here: https://docs.google.com/spreadsheets/d/1mFul5rEZUhfWSB8qSNtGBibBrFA8MPpm0KkxnSyVReI/edit#gid=0. Moving this in-tree is fine with me, it should just be easy to see what are the alternative options.

dagar · 2019-11-19T23:50:29Z

Ok new plan. Let's get these drivers in on the side inactive by default and without the ecl clipping changes. They're still useful for analyzing vibration and we can switch over later once we can better handle clipping and get a lot more testing.

Would anyone care to review?

- uses the FIFO and SPI DMA - currently implemented on px4_fmu-v4 only - clip counters - new low pass filter that operates on an array of data (LowPassFilter2pArray) - new sensor messages for better visibility - sensor_{accel, gyro}_fifo: full raw data for optional logging and analysis - sensor_{accel, gyro}_status: metadata, clipping, etc - sensor_{accel, gyro}_integrated: delta angles and delta velocities - eventually these will replace the existing sensor_{accel, gyro} monolith

dagar · 2019-12-04T03:47:35Z

Merging this to enable further testing and iteration. Note, these new drivers must be manually started (with the existing mpu6000 stopped).

nsh> mpu6000 stop

nsh> icm20602 -R 8 start
icm20602 on SPI bus 1 at 5 (10000 KHz)

nsh> listener sensor_gyro_fifo

TOPIC: sensor_gyro_fifo
 sensor_gyro_fifo_s
        timestamp: 43686752  (0.000431 seconds ago)
        timestamp_sample: 43685585
        device_id: 3671306 (Type: 0x38, SPI:1 (0x05)) 
        dt: 125.0000
        scale: 0.0010
        x: [-27, -21, -33, -28, -25, -33, -16, -15]
        y: [-7, -25, -33, -21, -30, -10, 0, -7]
        z: [-8, -6, -4, -8, 10, -7, 3, 8]
        samples: 8

nsh> listener sensor_accel_status

TOPIC: sensor_accel_status
 sensor_accel_status_s
        timestamp: 50664663  (0.000564 seconds ago)
        error_count: 0
        clipping: [0, 0, 0]
        device_id: 3605770 (Type: 0x37, SPI:1 (0x05)) 
        temperature: 46.9094
        full_scale_range: 156.9064
        measure_rate: 1000
        sample_rate: 4000
        rotation: 8

mhkabir · 2019-12-04T03:50:26Z

What is the plan for the ICM support currently in the mpu6000 driver?

dagar · 2019-12-04T04:17:31Z

What is the plan for the ICM support currently in the mpu6000 driver?

Nothing until these are complete replacements. At that point the mpu6000 itself will be stripped down to be nothing but an optimized mpu6000 driver.

The real blockers for full deployment are:

SPI DMA support everywhere (currently there are minor issues on F7 and H7)
estimator robustness to IMU clipping

In the testing so far during periods of accelerometer clipping the immediate results are quite bad (enormous velocity estimate), and appear to be worse than the behavior of the current driver where the DLPF is enabled and the clipped accelerometer readings are obscured.

sukshata · 2020-07-20T16:26:37Z

Hi I am looking for ICM20608-G sensor driver code base migration can anyone help me with the github link to do the same?

dagar · 2020-07-20T16:32:41Z

Hi I am looking for ICM20608-G sensor driver code base migration can anyone help me with the github link to do the same?

The new icm20608g driver is here. https://github.com/PX4/Firmware/tree/master/src/drivers/imu/invensense/icm20608g

dagar added the Admin: Enhancement (improvement) 💡 label Oct 6, 2019

dagar force-pushed the pr-icm20602_fmu-v4 branch from 2b9eb33 to 00cc503 Compare October 6, 2019 19:41

dagar force-pushed the pr-icm20602_fmu-v4 branch 6 times, most recently from d0fa851 to 92a81bb Compare October 6, 2019 22:04

dagar marked this pull request as ready for review October 6, 2019 22:47

bkueng reviewed Oct 7, 2019

View reviewed changes

julianoes self-requested a review October 7, 2019 07:43

dusan19 reviewed Oct 7, 2019

View reviewed changes

msg/sensor_accel_control.msg Outdated Show resolved Hide resolved

msg/sensor_accel_fifo.msg Outdated Show resolved Hide resolved

dagar added this to the Release v1.11.0 milestone Oct 7, 2019

dagar mentioned this pull request Oct 7, 2019

top: decrease priority below IMU sensor WQ threads #13115

Merged

weekly-digest bot mentioned this pull request Oct 13, 2019

Weekly Digest (6 October, 2019 - 13 October, 2019) #13171

Closed

dagar force-pushed the pr-icm20602_fmu-v4 branch from 5283f7b to a398a86 Compare October 15, 2019 00:19

dagar changed the title ~~ICM20602: new standalone driver (8 kHz gyro, 4 kHz accel)~~ ICM20602 and ICM20608g: new standalone driver (8 kHz gyro, 4 kHz accel) Oct 15, 2019

dagar mentioned this pull request Oct 17, 2019

[WIP]: Rate control: introduce torque_setpoint, angular_accel_setpoint, and inertia matrix #13219

Closed

weekly-digest bot mentioned this pull request Oct 20, 2019

Weekly Digest (13 October, 2019 - 20 October, 2019) #13236

Closed

dagar force-pushed the pr-icm20602_fmu-v4 branch 5 times, most recently from 4bbf41c to 9f55fec Compare October 22, 2019 02:24

dagar force-pushed the pr-icm20602_fmu-v4 branch from ae6420f to e00454c Compare November 5, 2019 17:52

dagar force-pushed the pr-icm20602_fmu-v4 branch from e00454c to 0ef4725 Compare November 5, 2019 23:17

dagar force-pushed the pr-icm20602_fmu-v4 branch from 0ef4725 to 782564b Compare November 7, 2019 19:39

bkueng reviewed Nov 8, 2019

View reviewed changes

dagar force-pushed the pr-icm20602_fmu-v4 branch 4 times, most recently from cefbe93 to b435a13 Compare November 18, 2019 16:33

dagar force-pushed the pr-icm20602_fmu-v4 branch from b435a13 to b79fac8 Compare November 19, 2019 23:43

dagar force-pushed the pr-icm20602_fmu-v4 branch 3 times, most recently from fe029cd to bcf01d3 Compare November 26, 2019 02:38

dagar force-pushed the pr-icm20602_fmu-v4 branch 3 times, most recently from 1917774 to a87f5cc Compare December 4, 2019 01:17

dagar force-pushed the pr-icm20602_fmu-v4 branch from a87f5cc to 99e1c75 Compare December 4, 2019 02:57

dagar merged commit 709961e into master Dec 4, 2019

dagar deleted the pr-icm20602_fmu-v4 branch December 4, 2019 04:21

dagar mentioned this pull request Jan 3, 2020

[DO NOT MERGE] IMU clipping testing #13837

Closed

PX4 deleted a comment from sukshata Apr 15, 2021

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

ICM20602 and ICM20608g: new standalone driver (8 kHz gyro, 4 kHz accel) #13103

ICM20602 and ICM20608g: new standalone driver (8 kHz gyro, 4 kHz accel) #13103

dagar commented Oct 6, 2019 •

edited

Loading

LorenzMeier commented Oct 6, 2019

dagar commented Oct 6, 2019

bkueng Oct 7, 2019

dagar Oct 7, 2019

dagar Oct 7, 2019

dagar Oct 7, 2019

dagar Oct 7, 2019

bkueng Oct 8, 2019

BazookaJoe1900 commented Oct 7, 2019 •

edited

Loading

dagar commented Oct 7, 2019

priseborough commented Oct 18, 2019 •

edited

Loading

priseborough commented Oct 18, 2019

jorge789 commented Nov 5, 2019 •

edited by Tony3dr

Loading

jorge789 commented Nov 6, 2019

bkueng Nov 8, 2019

dagar Nov 8, 2019

dagar Nov 12, 2019

bkueng Nov 18, 2019

dagar commented Nov 19, 2019 •

edited

Loading

dagar commented Dec 4, 2019

mhkabir commented Dec 4, 2019

dagar commented Dec 4, 2019

sukshata commented Jul 20, 2020

dagar commented Jul 20, 2020

ICM20602 and ICM20608g: new standalone driver (8 kHz gyro, 4 kHz accel) #13103

ICM20602 and ICM20608g: new standalone driver (8 kHz gyro, 4 kHz accel) #13103

Conversation

dagar commented Oct 6, 2019 • edited Loading

Background (notes from @priseborough)

IMU Signal Processing

LorenzMeier commented Oct 6, 2019

dagar commented Oct 6, 2019

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

BazookaJoe1900 commented Oct 7, 2019 • edited Loading

dagar commented Oct 7, 2019

priseborough commented Oct 18, 2019 • edited Loading

priseborough commented Oct 18, 2019

jorge789 commented Nov 5, 2019 • edited by Tony3dr Loading

jorge789 commented Nov 6, 2019

Tested on PixRacer V4:

Tested on Pixhawk Pro V4:

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dagar commented Nov 19, 2019 • edited Loading

dagar commented Dec 4, 2019

mhkabir commented Dec 4, 2019

dagar commented Dec 4, 2019

sukshata commented Jul 20, 2020

dagar commented Jul 20, 2020

dagar commented Oct 6, 2019 •

edited

Loading

BazookaJoe1900 commented Oct 7, 2019 •

edited

Loading

priseborough commented Oct 18, 2019 •

edited

Loading

jorge789 commented Nov 5, 2019 •

edited by Tony3dr

Loading

dagar commented Nov 19, 2019 •

edited

Loading