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

dagar · 2020-05-15T05:36:25Z

IMUs now only publish sensor_accel + sensor_gyro (sensor_accel_fifo/sensor_gyro_fifo optional extra)
- move accel & gyro integration to sensors/vehicle_imu where we can ensure synchronization between data
- delete sensor_accel_integrated/sensor_gyro_integrated
- merge sensor_accel_status/sensor_gyro_status into vehicle_imu_status
new uORB::SubscriptionCallback mechanism to throttle callbacks by number of updates

update: 1s, num topics: 68
TOPIC NAME       INST #SUB RATE #LOST #Q SIZE
vehicle_imu         0    3  200   226  1   56 
vehicle_imu_status  0    3  200     0  1   56 
vehicle_imu         1    3  200   226  1   56 
vehicle_imu_status  1    3  200     0  1   56 
vehicle_imu         2    3  199   225  1   56 
vehicle_imu_status  2    3  199     0  1   56

TODO:

SITL lockstep issues
test clipping still works correctly
test bmi088
test bmi055
test nxphlite (separate accel + gyro)
test lsm303d + l3gd20 combo (pixhawk 1 & 2 secondary IMU)
verify mavlink changes
voted_sensors_update restore error count

dagar · 2020-05-15T05:36:51Z

FYI @bkueng @bresch

TSC21 · 2020-05-15T15:44:33Z

msg/tools/uorb_rtps_message_ids.yaml

@@ -267,18 +267,12 @@ rtps:
    id: 117
  - msg: sensor_accel_fifo
    id: 118
-  - msg: sensor_accel_status
-    id: 119


@dagar can you rearrange the ID's so we don't have unused numbers? Thanks!

Shift the entire set after 118 down? What's the expectation/requirement for these id numbers (persistence, etc)?

bresch · 2020-05-19T08:30:23Z

@dagar I tested that on a BMI088 and the sensor combined data is now free of duplicates!

- sensors: voted_sensos_update consume vehicle_imu - delete sensor_accel_integrated, sensor_gyro_integrated - merge sensor_accel_status/sensor_gyro_status into vehicle_imu_status

bkueng

Thanks for pushing this, looks good.
I went through this, not quite line-by-line, but close.

Can you ensure the vehicle_imu publication ordering? I.e. advertisement on object creation and creation ordering.

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

bkueng · 2020-05-20T08:03:53Z

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

+	report.device_id = _device_id;
+	report.temperature = _temperature;
+	report.error_count = _error_count;
+	report.x = val_calibrated(0);


This now uses the trapezoidal integral whereas before it was the average. Almost the same, and possibly not noticeable, but IMO worth pointing out.

src/modules/sensors/voted_sensors_update.h

src/modules/uORB/SubscriptionCallback.hpp

src/lib/drivers/device/integrator.h

bkueng · 2020-05-20T08:31:50Z

src/lib/drivers/device/integrator.h

+	 *
+	 * @param reset_samples	    	New reset time interval for the integrator.
+	 */
+	void set_reset_samples(uint8_t reset_samples) { _reset_samples_min = reset_samples; }


Why configure both the number of samples + integral time?

VehicleIMU initially uses the configured integration rate (IMU_INTEG_RATE), but then switches over to samples once it has a good measure for the actual accel/gyro publication rate.

I kept a relaxed min interval in place as a crude protection after seeing quite a bit of variability in the mixed cases (fxas21002c + fxos8701cq, lsm303d + l3gd20, etc) that I haven't really explored in detail yet.

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

bkueng · 2020-05-20T08:53:45Z

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

+			_accel_integrator.set_reset_interval(0);
+
+			// run when there are enough new gyro samples, unregister accel
+			_sensor_accel_sub.unregisterCallback();


Assuming gyro is publishing at a faster or equal rate

It should still be fine given the queue depth. We'll have to give this more thought over time depending on the range of situations we want to support/tolerate. After #14776 there's also the possibility of using the FIFO data at this level and integrating subsets to synchronize as closely as possible.

bkueng · 2020-05-20T08:54:33Z

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

+			_accel_integrator.set_reset_interval(relaxed_reset_interval_min);
+
+		} else {
+			// otherwise let the gyro set the configuration and scheduling


Why not always do that?

This started life implemented for the known good/simple cases (already synchronized invensense accel + gyro) and then evolved to accommodate the others. I think always following the gyro makes sense and keeps the scheduling regular relative to the inner loop.

Lockstep and SITL (non-root) was being a little weird with the possibility of gyro then accel publishing, but VehicleIMU running in between. I'll see if it can be simplified (sensor_gyro only scheduling).

…_integrate

dagar · 2020-05-20T14:26:24Z

If the flow of data seems a bit weird with sensors kicking off multiple VehicleIMU WorkItems that it then subscribes to it's because this is intended to accommodate the multi-EKF use case where the simple IMU voter (voted_sensors_update) may not even be needed.

#14650

dagar · 2020-05-20T14:29:44Z

This saves ~2.3 kB of flash on fmu-v2 and ~3.4 kB on fmu-v5.

dagar · 2020-05-20T14:30:25Z

Can you ensure the vehicle_imu publication ordering? I.e. advertisement on object creation and creation ordering.

Done.

…_integrate

bkueng

What is your testing and confidence level?

I still see a possibility for vehicle_imu reordering: intantiation is based on accel.device_id > 0 && gyro.device_id > 0, so it could be that a later IMU already has its ID published.

dagar · 2020-05-25T16:01:59Z

What is your testing and confidence level?

Reasonably confident for something that's never flown. I'm going to manually verify a few things like the propagation of clip counters. I'd also like to review bench log data from the non-synchronized cases we care about (bmi088, bmi055, nxp, original pixhawk, etc) and check for any surprises in the delta_angle_dt/delta_velocity_dt and ekf2 timeslip.

Reviewing an actual flight test with a bmi088 as primary would be great.

I still see a possibility for vehicle_imu reordering: intantiation is based on accel.device_id > 0 && gyro.device_id > 0, so it could be that a later IMU already has its ID published.

In the current PR sensors indexes VehicleIMUs as _vehicle_imu_list[X] = sensor_accelX + sensor_gyroX. One simple idea could be to only build up that list sequentially. The sensors module is rescanning the available accels & gyros up until arming.

Originally I was planning to pursue this post #14776 where the ordering will no longer matter. At that point I'd also like to do a pass to have sensors put more effort into the IMU accel + gyro pairing. Things like first pairing up matching device IDs, then remaining accel + gyro on the same bus, and only then pairing up the leftovers.

…_integrate

dagar · 2020-05-26T23:05:48Z

I gave up on the minimum interval time requirement/protection in addition to the number of samples and I believe things are now reasonably under control on the BMI088. The accel & gyro publication rate of the BMI088 isn't nearly as regular as the other new drivers using FIFOs and interrupts.

For example the BMI088 vs ICM20602 publication intervals on the Modalai board.

BMI088

vehicle_imu: publish interval: 41990 events, 5014.34us avg, min 4036us max 7055us 136.370us rms
vehicle_imu: accel update interval: 169309 events, 1243.74us avg, min 367us max 2551us 87.115us rms
vehicle_imu: gyro update interval: 209988 events, 1002.87us avg, min 224us max 3043us 164.365us rms

ICM20602

vehicle_imu: publish interval: 42162 events, 4990.85us avg, min 4649us max 5352us 37.751us rms
vehicle_imu: accel update interval: 168664 events, 1247.73us avg, min 1222us max 1273us 2.603us rms
vehicle_imu: gyro update interval: 168676 events, 1247.73us avg, min 1222us max 1273us 2.603us rms

Note this also includes bootup and the initial period before the actual publication rate is computed and integration is configured to an appropriate number of samples. http://ci.px4.io:8080/blue/organizations/jenkins/PX4_misc%2FFirmware-hardware/detail/pr-sensors_vehicle_imu_integrate/54/pipeline/399#step-790-log-64

bkueng

Looks as expected now.

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

Co-authored-by: Beat Küng <beat-kueng@gmx.net>

dagar · 2020-05-27T14:20:44Z

Occasionally it's integrating an extra gyro sample. I was experimenting with breaking out of the orb_copy + integration loop once the integrator minimum requirements are satisfied. It's not included at the moment because of concerns with falling behind the raw data.

…_integrate

…ss we've fallen behind by more than 1 sample

dagar · 2020-05-28T14:47:38Z

Occasionally it's integrating an extra gyro sample. I was experimenting with breaking out of the orb_copy + integration loop once the integrator minimum requirements are satisfied. It's not included at the moment because of concerns with falling behind the raw data.

Should be fixed with the last push.

Zooming in you can also get a sense of the (minimal) remaining real-time jitter in the system. The y axis is integration time (4 gyro samples in this case) in microseconds.

…_integrate

dagar · 2020-05-28T17:22:40Z

Note: mavlink SCALED_IMU, SCALED_IMU2, SCALED_IMU3 are now in the vehicle body frame (including board level adjustments).

The mavlink spec doesn't indicate the frame, but I'd imagine this is what people would expect.

…_integrate

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

dagar force-pushed the pr-sensors_vehicle_imu_integrate branch 3 times, most recently from 73b7e64 to 85e151c Compare May 15, 2020 14:53

TSC21 reviewed May 15, 2020

View reviewed changes

This was referenced May 19, 2020

px4_work_queue: minor status changes #14936

Merged

uORB: SubscriptionCallback add optional required updates for callback #14939

Closed

dagar force-pushed the pr-sensors_vehicle_imu_integrate branch from 85e151c to b5067f4 Compare May 19, 2020 18:21

sensors/vehicle_imu: integrate accel and gyro

2906f40

- sensors: voted_sensos_update consume vehicle_imu - delete sensor_accel_integrated, sensor_gyro_integrated - merge sensor_accel_status/sensor_gyro_status into vehicle_imu_status

dagar force-pushed the pr-sensors_vehicle_imu_integrate branch from b5067f4 to 2906f40 Compare May 19, 2020 18:26

lockstep testing hacks

9ba72f7

dagar force-pushed the pr-sensors_vehicle_imu_integrate branch from f46ecfd to 9ba72f7 Compare May 19, 2020 22:00

bkueng reviewed May 20, 2020

View reviewed changes

dagar added 3 commits May 20, 2020 09:38

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

792b157

…_integrate

changes from code review

3bef9c3

Integrator move from lib/drivers/device -> sensors/vehicle_imu

eeecdc5

dagar changed the title ~~[WIP]: move IMU integration to sensors/vehicle_imu to fix potential accel/gyro sync issues~~ move IMU integration to sensors/vehicle_imu to fix potential accel/gyro sync issues May 20, 2020

dagar marked this pull request as ready for review May 20, 2020 14:26

advertise vehicle_imu_status immediately

859b1eb

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

b3f8ed7

…_integrate

bkueng reviewed May 25, 2020

View reviewed changes

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

3cc350d

…_integrate

dagar force-pushed the pr-sensors_vehicle_imu_integrate branch from 69dab21 to c4b6c72 Compare May 26, 2020 22:34

sensors/vehicle_imu: remove debug perf counters

d20e341

bkueng previously approved these changes May 27, 2020

View reviewed changes

src/modules/sensors/vehicle_imu/VehicleIMU.cpp Outdated Show resolved Hide resolved

Update src/modules/sensors/vehicle_imu/VehicleIMU.cpp

09b3dab

Co-authored-by: Beat Küng <beat-kueng@gmx.net>

dagar dismissed bkueng’s stale review via 09b3dab May 27, 2020 14:00

dagar added 2 commits May 27, 2020 11:46

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

680de1a

…_integrate

sensors/vehicle_imu: don't integrate every available gyro sample unle…

e0180f3

…ss we've fallen behind by more than 1 sample

dagar force-pushed the pr-sensors_vehicle_imu_integrate branch from 3dc6e5a to 3f21722 Compare May 28, 2020 00:39

TESTING

470f418

dagar force-pushed the pr-sensors_vehicle_imu_integrate branch from 3f21722 to 470f418 Compare May 28, 2020 14:03

dagar added 4 commits May 28, 2020 10:49

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

5f07340

…_integrate

logger: limit vehicle_imu

8c0a3d6

vehicle_imu error counts

ba2fd75

fix typo and formatting

9c0f8c5

dagar force-pushed the pr-sensors_vehicle_imu_integrate branch from 260478b to 9c0f8c5 Compare May 28, 2020 15:54

dagar added 4 commits May 28, 2020 12:15

fix sensor_selection publication and add backup schedules

ebee09b

ekf2: remove accel/gyro bias reset warning (too noisy)

ec5a35f

sensors status cleanup whitespace and ordering

1b2769e

IMU_INTEG_RATE add enums and comment

44ec4ea

dagar force-pushed the pr-sensors_vehicle_imu_integrate branch from 40c1080 to 859b02e Compare May 28, 2020 16:57

sensors status minor tweaks

043f83a

dagar force-pushed the pr-sensors_vehicle_imu_integrate branch from 859b02e to 043f83a Compare May 28, 2020 17:03

remove IMU_INTEG_RATE enums

90c1864

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

e463c7e

…_integrate

dagar merged commit e34bdb4 into master May 30, 2020

dagar deleted the pr-sensors_vehicle_imu_integrate branch May 30, 2020 15:07

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

dagar commented May 15, 2020 •

edited

Loading

dagar commented May 15, 2020

TSC21 May 15, 2020

dagar May 15, 2020

bresch commented May 19, 2020

bkueng left a comment

bkueng May 20, 2020

bkueng May 20, 2020

dagar May 20, 2020

bkueng May 20, 2020

dagar May 20, 2020

bkueng May 20, 2020

dagar May 20, 2020

dagar May 20, 2020

dagar commented May 20, 2020

dagar commented May 20, 2020

dagar commented May 20, 2020

bkueng left a comment

dagar commented May 25, 2020

dagar commented May 26, 2020 •

edited

Loading

bkueng left a comment

dagar commented May 27, 2020 •

edited

Loading

dagar commented May 28, 2020

dagar commented May 28, 2020

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

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

Conversation

dagar commented May 15, 2020 • edited Loading

dagar commented May 15, 2020

Choose a reason for hiding this comment

Choose a reason for hiding this comment

bresch commented May 19, 2020

bkueng left a 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

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 May 20, 2020

dagar commented May 20, 2020

dagar commented May 20, 2020

bkueng left a comment

Choose a reason for hiding this comment

dagar commented May 25, 2020

dagar commented May 26, 2020 • edited Loading

BMI088

ICM20602

bkueng left a comment

Choose a reason for hiding this comment

dagar commented May 27, 2020 • edited Loading

dagar commented May 28, 2020

dagar commented May 28, 2020

dagar commented May 15, 2020 •

edited

Loading

dagar commented May 26, 2020 •

edited

Loading

dagar commented May 27, 2020 •

edited

Loading