introduce sensor_gyro_control message (1 kHz rate controller)

[dagar]

With the code sprawl in the existing IMU drivers gradually coming under control (#12079, #12128, #12139, #12140, #12141) one of the things we can now easily do is change the underlying message architecture.

This PR creates a new sensor_gyro_control message that contains only calibrated + filtered (IMU_GYRO_CUTOFF) gyro data published for every sample. The existing sensor_gyro is only published with each integrator reset (250 Hz).

The result is the primary gyro, rate controller, and output module running synchronized at the native sample rate (currently 1 kHz on most setups).

update: 1s, num topics: 78
TOPIC NAME                       INST #SUB #MSG #LOST #QSIZE
vehicle_odometry                    0    3  125   396 1
ekf2_innovations                    0    1  125     0 1
rate_ctrl_status                    0    1 1000     0 1
landing_gear                        0    1  248     0 1
vehicle_rates_setpoint              0    5  248   876 1
ekf2_timestamps                     0    1  248     0 1
multirotor_motor_limits             0    1 1000     0 1
actuator_outputs                    0    4 1000  2242 1
commander_state                     0    0    1     0 1
vehicle_magnetometer                0    5   50    90 1
vehicle_air_data                    0    6   74   197 1
safety                              0    2    9     0 1
vehicle_land_detected               0    9    1     0 1
actuator_armed                      0    8    1     0 1
telemetry_status                    0    2    2     0 3
vehicle_local_position              0    8  125   325 1
sensor_bias                         0    6  125   309 1
estimator_status                    0    5  125   223 1
sensor_combined                     0    5  248   466 1
vehicle_attitude                    0   11  248  1093 1
vehicle_attitude_setpoint           0    6  248   876 1
sensor_preflight                    0    1  248     0 1
actuator_controls_0                 0    6 1000  2862 1
vehicle_control_mode                0   10    1     0 1
system_power                        0    2  100    11 1
sensor_gyro_control                 1    1  989     0 1
sensor_baro                         0    1   73     0 1
sensor_gyro_control                 0    1 1000     0 1
sensor_mag                          1    1  125    11 1
sensor_gyro                         1    1  248     0 1
sensor_accel                        1    1  248     0 1
sensor_gyro                         0    1  248     0 1
sensor_accel                        0    1  248     0 1
sensor_mag                          0    1   50     0 1
adc_report                          0    1  100     0 1
task_stack_info                     0    0    1     0 2
vehicle_status_flags                0    2    1     0 1
vehicle_status                      0   10    1     0 1
battery_status                      0    8   82   155 1

The downside is the increased cpu usage from mc_att_control and the output module (px4fmu, px4io, etc), but I have several ideas (#12207) that will get that under control. This also won't be safe to merge until all gyro sources are updated to utilize the PX4Gyroscope class (eg #12142, #12141, #12140, #12139, #12136, #12128).

[MaEtUgR]

Looks awsome 👍

[dagar]

Project board with all the blocking pieces that will need to be resolved before moving forward with this change.
https://github.com/PX4/Firmware/projects/30

[dagar]

Almost there!

[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 waypoin activate RTL and see landing behaviour.

Notes:
No issues noted, good flight in general.

Log:
https://review.px4.io/plot_app?log=377d55ed-a054-484e-9ac1-5bae71e5b1f3

Tested on CUAV V5+:
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 waypoin activate RTL and see landing behaviour.

Notes:
No issues noted, good flight in general.

Log:
https://review.px4.io/plot_app?log=805cb3fd-6786-4c0d-84b4-23b0fb3e551c

[Junkim3DR]

Tested on NXP FMUK66 v3:

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 activate RTL and see landing behaviour.

Notes:
No issues noted, good flight in general.

Log:
https://review.px4.io/plot_app?log=67a71020-0b34-4b71-a8f2-98c168b8fd19

Tested on Pixhawk Pro v4Pro:

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 activate RTL and see landing behaviour.

Notes:
No issues noted, good flight in general.

Log:
https://review.px4.io/plot_app?log=f8399297-922e-4481-9315-882ad5ffaa2b

Tested on Pixhawk 4 mini v5:

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 activate RTL and see landing behaviour.

Notes:
No issues noted, good flight in general.

Log:
https://review.px4.io/plot_app?log=16f11055-fc5c-4f4d-8a9e-a23691b25977

[mcsauder]

Flight tested on a 250 racer, pixhawk 4 mini. This PR flies great!

• https://review.px4.io/plot_app?log=e0ddf196-88c9-42d9-ae24-7daa4eb4999b
• https://review.px4.io/plot_app?log=ebb2dddf-34d6-4521-aa95-59bc259f7292
• https://review.px4.io/plot_app?log=6074dc4c-43de-4c27-8da7-d5f7f039c349

The last flight was a ton of fun! :) Nice work @dagar !

[mcsauder]

I did a bit more flight testing after resetting the airframe to ensure default params: 250 generic quad pixhawk 4 mini. Everything was awesome!

I put the code through a few more lively flight tests involving full throttle climb in manual to neutral throttle position mode forcing the autopilot to recover a ballistic climb. After recoveries at ~100ft altitude I followed by zero throttle freefalls in manual again recovered by position mode, (only a few impacted the ground before the autopilot arrested the descent rate, but apparently those logs did not survive. The flights that ended abruptly landed right-side up with no damage... this was an entertaining test session!!!):

• https://review.px4.io/plot_app?log=4fce8f13-24bc-4924-ab23-5c0fd8e40ca8

Fantastic results and fun to fly! Nice work @dagar !

[jlecoeur]

1 kHz rate control is getting closer!

I started to review, but I would need more info to understand where this is going. More precisely:

• are sensor drivers going to publish on both topics (sensor_gyro and sensor_gyro_control) forever, or is it an incremental step towards something simpler?
• I see the fact a driver may publish on two topics with different uorb instances as unecessarily confusing
• in VehicleAngularAcceleration, what prevents you from polling sensor_gyro_control only?

I feel that it would be much simpler to go for the following:

• PX4Gyroscope drivers publish raw (xyz_raw) and unfiltered (xyz) data at full rate in a single topic, with one instance per sensor (let's call the topic sensor_gyro)
• VehicleAngularVelocity polls on the selected sensor_gyro instance, then performs corrections, then
  applies low pass filters and publishes at full rate on topic vehicle_angular_velocity both the filtered values (xyz) for use in controllers, and unfiltered values (xyz_unfiltered) for use in estimators and whatever custom usage
• EKF2 polls vehicle_angular_velocity, integrates the non-filtered values, and performs its update at whatever reduced rate best fits

[dagar]

• are sensor drivers going to publish on both topics (sensor_gyro and sensor_gyro_control) forever, or is it an incremental step towards something simpler?

This is an incremental step until we can properly kill off the driver framework drivers (https://github.com/PX4/Firmware/tree/master/src/platforms/posix/drivers) and have all sources of IMU data respect the same interface (what PX4Gyroscope provides).

I feel that it would be much simpler to go for the following:

• PX4Gyroscope drivers publish raw (xyz_raw) and unfiltered (xyz) data at full rate in a single topic, with one instance per sensor (let's call the topic sensor_gyro)
• VehicleAngularVelocity polls on the selected sensor_gyro instance, then performs corrections, then
  applies low pass filters and publishes at full rate on topic vehicle_angular_velocity both the filtered values (xyz) for use in controllers, and unfiltered values (xyz_unfiltered) for use in estimators and whatever custom usage

That's close to where I'm headed. Once the DriverFramework drivers are gone (dagar#19) all IMU data will flow through PX4Accelerometer and PX4Gyroscope. At that point we'll be able to easily make these changes and drop more of the legacy debt. Eventually I'd like to get to the point where drivers simply provide the raw data and nearly all processing happens downstream.

• EKF2 polls vehicle_angular_velocity, integrates the non-filtered values, and performs its update at whatever reduced rate best fits

The estimator data path will be a bit different because we need to integrate raw data and then provide all instances of it (multi-EKF). At the moment I'm thinking of queuing the raw data (often 4 frames, but will be more soon) and doing the integration work in the same thread as the estimator (further optimizing the inner loop).

[jlecoeur]

Isn't _selected_sensor_device_id always valid? If it is not, ignore my inline suggestions

[jlecoeur]

Ok, thanks for the explanation. As long as vehicle_angular_velocity (which is what people will mostly use) stays as it is, I am OK with this intermediate step.

[dannyfpv]

Tested on Pixhawk4 v5 f-450
Modes Tested

Acro mode Mode: Good.

Notes:
No issues noted, good flight in general.
wind speed 8.0 m/s

Log:
https://review.px4.io/plot_app?log=ec1652a5-3099-4580-9d06-a3014134ad73

pixhawk4 pro f450 log:
https://review.px4.io/plot_app?log=c7d97248-1c3e-48d4-901c-992b36a6469b

[dagar]

Thanks @jlecoeur, all comments addressed.

[jlecoeur]

LGTM
I have not flight tested. I have flashed on a pixhawk 4, which revealed different publish rates by the gyros. This will have to be addressed in the drivers.

[dagar]

Thanks everyone.