sensors/vehicle_imu: incremental step towards multi-EKF

[dagar]

One of the missing pieces for running multiple estimators (an instance per IMU or more) is the current sensors module only provides the voted primary IMU message (sensor_combined).

This pull request updates the sensors module to additionally launch a WorkItem per IMU (accel + gyro pair) that handles collecting the data, applying any corrections (thermal), board rotation (including level), then published as an instance of vehicle_imu.

The ekf2 module has been updated to optionally subscribe to a specific vehicle_imu set by the parameter EKF2_IMU_ID, or continue using sensor_combined.

The structure of this change is a bit weird, but it will enable further multi-ekf development and testing while the sensors module is refactored.

Next steps

1. update ModuleBase to enable multiple instances of certain modules - ModuleBase add common base and cleanup #12191
   • update ekf2 to launch a configured number of instances
   • in multi-instance mode publish all messages to estimator_X_n equivalents
2. create ekf2_selector to subscribe to all estimators messages, select best, and republish system vehicle_local_position, vehicle_global_position, vehicle_attitude, and handle resets, jumps between estimators, etc
3. (optional) parameter instances, each EKF instance should have it's own set of parameters with minimal overhead.
4. (investigating) move actual IMU integration work to VehicleIMU WorkItems and run in a WQ thread per EKF
5. (investigating) various improvements to ensure each "IMU" consists of synchronized accel + gyro data
   • pair up accel + gyro from same physical device. In many cases we only need a single device id per IMU (mpu6000, icm20602, etc).
   • sensors that provide accel + gyro simultaneous could instead publish a sensor_imu message rather than rely on reassembling sensor_accel + sensor_gyro downstream

nsh> listener vehicle_imu

TOPIC: vehicle_imu 2 instances

Instance 0:
 vehicle_imu_s
        timestamp: 1218855875  (0.003490 seconds ago)
        timestamp_sample: 1218852526
        accel_device_id: 3607306
        gyro_device_id: 3672842
        delta_angle: [0.0000, 0.0001, 0.0000]
        delta_velocity: [0.0001, -0.0001, -0.0393]
        dt: 3999
        integral_samples: 4
        integral_clip_count: 0

Instance 1:
 vehicle_imu_s
        timestamp: 1218868690  (0.002584 seconds ago)
        timestamp_sample: 1218865207
        accel_device_id: 1442826
        gyro_device_id: 2360330
        delta_angle: [-0.0000, 0.0000, 0.0001]
        delta_velocity: [0.0008, 0.0013, -0.0400]
        dt: 3999
        integral_samples: 4
        integral_clip_count: 0

[LorenzMeier]

@PX4/testflights Please bench and flight test this on a multicopter and share the log here.

[jorge789]

Tested on PixRacer V4

The flight log started since the battery was connected, the 4 flight cards were made

Flight Card 1

Modes Tested:
Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.

Procedure:
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode.

Notes:
No issues were noted, good flight in general.

Flight Card 2

Modes Tested
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure

Armed form QGC.
Took-off on mission plan mode
. Note that commands were sent form QGC (Armed and takeoff)
Once all waypoint completed vehicle switched to RTL and landed as expected.
Good flight in general.
Note:
No issues were noted, good flight in general.

Flight Card 3
Modes Tested
Position Mode: Good.
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure
Arm and Takeoff in position mode, after flying for approximately one minute, switched to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoints.
Good flight in general.

Note:
No issues were noted, good flight in general.

Flight Card 4
Modes Tested
Fail-safe: The radio control turned off and the vehicle returned and landed correctly.
Tested good

Log: https://review.px4.io/plot_app?log=6c0f7e59-6643-448b-9377-5c39b7b00b31

Tested on CUAV nano V5

Flight Card 1

Modes Tested:
Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.

Procedure:
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode.

Notes:
No issues were noted, good flight in general.

Log: https://review.px4.io/plot_app?log=f7059b42-9fce-4c6f-84ee-97e7755c4572

Flight Card 2

Modes Tested
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure

Armed form QGC.
Took-off on mission plan mode
. Note that commands were sent form QGC (Armed and takeoff)
Once all waypoint completed vehicle switched to RTL and landed as expected.
Good flight in general.
Note:
No issues were noted, good flight in general.

Log: https://review.px4.io/plot_app?log=e199634b-ca58-4080-891a-5035e042ae92

Flight Card 3
Modes Tested
Position Mode: Good.
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure
Arm and Takeoff in position mode, after flying for approximately one minute, switched to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoints.
Good flight in general.

Note:
No issues were noted, good flight in general.

Log: https://review.px4.io/plot_app?log=cb0107b9-1270-4fa7-9496-3b1ef718f35d

Flight Card 4
Modes Tested
Fail-safe: The radio control turned off and the vehicle returned and landed correctly.
Tested good

Log: https://review.px4.io/plot_app?log=9370931b-38b4-4454-90cb-9aeeacbed947

[Junkim3DR]

Tested on NXP FMUK66 v3

Modes Tested
Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.

Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode.

Notes
No issues noted, good flight in general.

Logs

• https://review.px4.io/plot_app?log=e4b70a38-8f86-49ba-8432-b433340f951d

[dagar]

Thanks everyone.