accel and gyro calibration refactor and cleanup #14776

dagar · 2020-04-27T22:36:09Z

Problem

Currently in PX4 master each accel or gyro has a few requirements.

publish sensor_accel or sensor_gyro with raw data and temperature (if available)
integrate raw data and publish every 2500 us (this is what the estimator consumes) to sensor_accel_integrated or sensor_gyro_integrated
register a character device (/dev/accelX or /dev/gyroX) and implement a few simple IOCTLs for getting the device id and setting calibration

Unfortunately as the system evolved to this current state a number of implicit ordering assumptions around these interfaces slipped in. I only recently discovered real cases where this is violated.

Examples

sensors module assuming temperature compensation (sensor_correction) is ordered the same as sensor_accel_integrated/sensor_gyro_integrated
sensors module pushes calibration into drivers via IOCTL (/dev/accelX or /dev/gyroX)
- if the character device ordering doesn't match sensor_accel_integrated/sensor_gyro_integrated ordering then the enable/disable parameters will be wrong
calibration is done in sensor_accel/sensor_gyro order, then pushed into drivers in character device order
- this can go unnoticed because it will be "fixed" on the next parameter update when the sensors module reloads the parameters

We've mostly gotten away with this because typically all interfaces are created at the same time when starting the driver, but with the new drivers some (eg mpu6000) are performing longer more thorough reset procedures.

Solution

To solve this and clean up some legacy I've finished purging the accel and gyro character devices and IOCTLs. Then in all remaining usage of the uORB messages we don't allow any assumptions about ordering (always checking the device id field).

calibration is completely removed from the drivers and handled "downstream" in the sensors module (SensorCalibration helper)
drv_accel.h and drv_gyro.h completely gone along with all IOCTLs for accel and gyro
calibration no longer needs to be cleared existing before starting
temperature calibration (TC) remove all scale (SCL) parameters
- gyro and baro scale are completely unused
- regular accel calibration scale can be used (CAL_ACC*_xSCALE)
temperature calibration: use regular accel & gyro offsets used on top of temperature calibration (if available) instead of modifying the TC offsets
- I believe this makes more sense than modifying the TC parameters, but it may need to be discussed
  - some setups have specific read only memory for storing factory calibration, so we can't tweak the offsets later or store the scale factors
  - this way only the temperature_calibration module needs to know about the TC parameters their ordering relative to sensor_accel
I originally started this months ago and was going to save it for v1.12 (accel and gyro calibration cleanup #14237), but it's worth finishing now to resolve these potential problems.

TODO:

consider transitioning old TC_ SCL (scale factors) to regular accel scale (all the logs I could find had a scale factor error ~ 1% or less)
test everywhere (including configurations with temperature compensation enabled) and compare calibrations

Future Work

update sensor_mag calibration similarly
- mags aren't immune to the issues above, but it's less likely with only a single sensor_mag topic, no temperature calibration, and simpler drivers (no FIFO or integration)
move all IMU integration down to sensors/vehicle_imu
- the goal is to remove virtually all processing from the high rate, high priority sensor bus threads
use full sensor_gyro_fifo for filtering in vehicle_angular_velocity
potentially drop PX4Accelerometer/PX4Gyroscope (it might be nothing more than an orb publisher wrapper)

dagar · 2020-04-27T22:54:47Z

Fixes #14717.
Possibly related to #14505.

bkueng · 2020-04-28T07:23:03Z

On a high level this looks good and the right direction. I'll need to go through the details.

A quicker and less intrusive solution would be to ensure everything gets advertised during serialized (driver) startup.

dagar · 2020-04-28T14:11:30Z

A quicker and less intrusive solution would be to ensure everything gets advertised during serialized (driver) startup.

I largely agree if I didn't already have this laying around, although I'm aware of some minor issues that we'd need to work on that front.

I'm going to spend some time testing this across a range of boards. If it uncovers any surprises at all we can fall back to serialized driver registration + uorb advertise. Either way we need a solution immediately.

dagar · 2020-04-29T01:29:17Z

A quicker and less intrusive solution would be to ensure everything gets advertised during serialized (driver) startup.

Or both - #14784

dagar · 2020-04-29T03:16:42Z

So far everything checks out on a temperature calibration pixhawk (reported #14717) as well as basic calibration on fmu-v4 & fmu-v5.

dagar · 2020-05-04T15:18:36Z

This is ready for review and testing, but with #14784 merged this is no longer a v1.11 release blocker.

julianoes · 2020-05-05T07:28:35Z

@dagar any tips on how to review this or what I should focus on?

dagar · 2020-05-05T20:30:19Z

@dagar any tips on how to review this or what I should focus on?

Is the overall design clear? The general idea is to get to the point where sensor drivers do nothing more than publish raw data. Then things like calibrations are entirely applied downstream by the sensors module and we no longer need IOCTLs or character devices to directly push changes into each.

julianoes

I tried to do a review pass.

msg/sensor_correction.msg

julianoes · 2020-05-06T07:15:16Z

msg/sensor_correction.msg

+uint32[3] accel_device_ids
+float32[3] accel_offset_0	# accelerometer 0 XYZ offsets in the sensor frame in m/s/s
+float32[3] accel_offset_1	# accelerometer 1 XYZ offsets in the sensor frame in m/s/s
+float32[3] accel_offset_2	# accelerometer 2 XYZ offsets in the sensor frame in m/s/s


And no more accel scale calibration?

Currently in master if you have TC enabled the regular calibration is stored to TC parameters. The scale factor is stored directly (eg CAL_ACC0_XSCALE -> TC_A0_SCL_0). The offset (eg CAL_ACCx_XOFF) is applied by adjusting the TC coefficients (x0, y0, z0 terms).

The new idea is to basically keep them as separate as possible. Regular calibration (CAL_ACC*) is always there and operates the same way (on top of TC adjustment if available). Calibration code doesn't need to poke into TC parameters at all. This is important because in some cases the TC parameters are stored read only.

src/modules/commander/accelerometer_calibration.cpp

dagar · 2020-06-01T16:21:40Z

Conflicts resolved and rebased on master.

- remove all remaining IOCTLs for accel and gyro - move accel/gyro calibration entirely to sensors module - calibration no longer needs to clear existing before starting - temperature calibration (TC) remove all scale (SCL) parameters - gyro and baro scale are completely unused - regular accel calibration scale can be used (CAL_ACC*_xSCALE)

julianoes

Check my inline comment.

Also, the title says "(including important bug fix)". Is that still true? And if so should that fix go into the release?

julianoes · 2020-06-02T09:55:53Z

src/modules/commander/accelerometer_calibration.cpp

+			for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
+				sensor_accel_s arp;
+
+				if (accel_sub[s].update(&arp)) {
+					// fetch optional thermal offset corrections in sensor/board frame
+					Vector3f offset{0, 0, 0};
+					sensor_correction_sub.update(&sensor_correction);
+
+					if (sensor_correction.timestamp > 0 && arp.device_id != 0) {
+						for (uint8_t i = 0; i < MAX_ACCEL_SENS; i++) {
+							if (sensor_correction.accel_device_ids[i] == arp.device_id) {
+								switch (i) {


Can you give i and s more descriptive names so it becomes clear? I'm still confused what should be i and what s. Also you loop both i and s from 0 to MAX_ACCEL_SENS and I assume one of the two should be 3 for 3 axis.

It's looping through every available accel (sensor_accel) many times and accumulating the data. For each sample it loops through the current sensor_correction (temperature_compensation) and removes the offset if available. Doing it in the loop like this is inefficient/lazy, but it's fine for a short run like this and minimizes state.

dagar · 2020-06-02T12:28:22Z

Also, the title says "(including important bug fix)". Is that still true? And if so should that fix go into the release?

No, we can relax slightly. I'd still like to get this in soon.

…calibration_cleanup

dagar · 2020-06-18T02:18:48Z

I've rebased to resolve the conflicts with the recent gyro calibration change (median filter) and tested this thoroughly comparing with master with and without temperature calibration.

dagar added bug Admin: Enhancement (improvement) 💡 Drivers 🔧 Sensors, Actuators, etc labels Apr 27, 2020

dagar added this to the Release v1.11.0 milestone Apr 27, 2020

dagar requested review from julianoes and bkueng April 27, 2020 22:52

dagar mentioned this pull request Apr 28, 2020

Thermal Calibration corrupts accelometers data to show wrong horizon roll #14717

Closed

dagar force-pushed the pr-sensors_accel_gyro_calibration_cleanup branch from 4909fc8 to b2092f0 Compare April 28, 2020 19:00

dagar force-pushed the pr-sensors_accel_gyro_calibration_cleanup branch from b2092f0 to 882a15d Compare May 1, 2020 18:27

weekly-digest bot mentioned this pull request May 3, 2020

Weekly Digest (26 April, 2020 - 3 May, 2020) #14814

Closed

dagar force-pushed the pr-sensors_accel_gyro_calibration_cleanup branch from 882a15d to 10132ae Compare May 4, 2020 15:17

dagar marked this pull request as ready for review May 4, 2020 15:18

dagar modified the milestones: Release v1.11.0, Release v1.12.0 May 4, 2020

julianoes reviewed May 6, 2020

View reviewed changes

dagar force-pushed the pr-sensors_accel_gyro_calibration_cleanup branch 4 times, most recently from 0fdc095 to 60a8414 Compare May 7, 2020 04:16

dagar force-pushed the pr-sensors_accel_gyro_calibration_cleanup branch 3 times, most recently from 2ec25f4 to 7bdb0ee Compare May 10, 2020 20:45

bresch mentioned this pull request May 13, 2020

Offboard temp cal: check topic instance #14897

Merged

dagar mentioned this pull request May 20, 2020

move IMU integration to sensors/vehicle_imu to fix potential accel/gyro sync issues #14906

Merged

8 tasks

dagar force-pushed the pr-sensors_accel_gyro_calibration_cleanup branch 3 times, most recently from f285ece to 5b6ceef Compare June 1, 2020 15:37

dagar force-pushed the pr-sensors_accel_gyro_calibration_cleanup branch from 5b6ceef to 9a7684d Compare June 1, 2020 16:20

dagar force-pushed the pr-sensors_accel_gyro_calibration_cleanup branch 2 times, most recently from 35325b1 to c706097 Compare June 1, 2020 17:00

dagar force-pushed the pr-sensors_accel_gyro_calibration_cleanup branch from c706097 to 8c23b7f Compare June 1, 2020 17:02

julianoes reviewed Jun 2, 2020

View reviewed changes

dagar changed the title ~~accel and gyro calibration refactor and cleanup (including important bug fix)~~ accel and gyro calibration refactor and cleanup Jun 2, 2020

dagar added 2 commits June 2, 2020 08:29

Merge remote-tracking branch 'px4/master' into pr-sensors_accel_gyro_…

b67a26e

…calibration_cleanup

accel & gyro cal improve loop iterator naming

86d1311

julianoes previously approved these changes Jun 2, 2020

View reviewed changes

Merge remote-tracking branch 'px4/master' into pr-sensors_accel_gyro_…

5611ca5

…calibration_cleanup

dagar dismissed julianoes’s stale review via 5611ca5 June 6, 2020 16:24

dagar mentioned this pull request Jun 8, 2020

ERROR [sensors] fewer accels than calibrations, resetting all CAL_GYROx_ID #15006

Closed

dagar added 2 commits June 9, 2020 16:41

Merge remote-tracking branch 'px4/master' into pr-sensors_accel_gyro_…

bc34c71

…calibration_cleanup

Merge remote-tracking branch 'px4/master' into pr-sensors_accel_gyro_…

44d2f11

…calibration_cleanup

dagar force-pushed the pr-sensors_accel_gyro_calibration_cleanup branch from fce7dac to 44d2f11 Compare June 18, 2020 00:07

dagar merged commit f55ed09 into master Jun 18, 2020

dagar deleted the pr-sensors_accel_gyro_calibration_cleanup branch June 18, 2020 02:50

dagar mentioned this pull request Jun 18, 2020

sensors: vehicle_imu fix scaling for correction and back #15121

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

accel and gyro calibration refactor and cleanup #14776

accel and gyro calibration refactor and cleanup #14776

dagar commented Apr 27, 2020 •

edited

Loading

dagar commented Apr 27, 2020

bkueng commented Apr 28, 2020

dagar commented Apr 28, 2020

dagar commented Apr 29, 2020

dagar commented Apr 29, 2020

dagar commented May 4, 2020

julianoes commented May 5, 2020

dagar commented May 5, 2020

julianoes left a comment

julianoes May 6, 2020

dagar May 6, 2020 •

edited

Loading

dagar commented Jun 1, 2020

julianoes left a comment

julianoes Jun 2, 2020

dagar Jun 2, 2020

dagar Jun 2, 2020

dagar commented Jun 2, 2020

dagar commented Jun 18, 2020

accel and gyro calibration refactor and cleanup #14776

accel and gyro calibration refactor and cleanup #14776

Conversation

dagar commented Apr 27, 2020 • edited Loading

Problem

Solution

Future Work

dagar commented Apr 27, 2020

bkueng commented Apr 28, 2020

dagar commented Apr 28, 2020

dagar commented Apr 29, 2020

dagar commented Apr 29, 2020

dagar commented May 4, 2020

julianoes commented May 5, 2020

dagar commented May 5, 2020

julianoes left a comment

Choose a reason for hiding this comment

julianoes May 6, 2020

Choose a reason for hiding this comment

dagar May 6, 2020 • edited Loading

Choose a reason for hiding this comment

dagar commented Jun 1, 2020

julianoes left a comment

Choose a reason for hiding this comment

julianoes Jun 2, 2020

Choose a reason for hiding this comment

dagar Jun 2, 2020

Choose a reason for hiding this comment

dagar Jun 2, 2020

Choose a reason for hiding this comment

dagar commented Jun 2, 2020

dagar commented Jun 18, 2020

dagar commented Apr 27, 2020 •

edited

Loading

dagar May 6, 2020 •

edited

Loading