WIP: High-rate IMU sampling

[AndreasAntener]

Continuing #7446. @dagar @priseborough @jgoppert . I needed a better idea about vibration in a system and this is a good start. This is also one step towards #7446 (comment)

This still suffers from missing DMA/FIFO access. But I was able to log unfiltered data from 2 IMUs at 2kHz, which is already pretty nice. And you can publish gyro control messages at 2kHz as well, but in that case the MC attitude controller uses 22% CPU ;)

This is NOT test flown, I'm really just bench testing at the moment.

Tasks / open questions

• really disable DLPF
• separation done for MPU6000 and MPU9250
• separate gyro message for control and estimation
• add message to log unfiltered (but NOT raw) data for spectral analysis
• clip detector (should that be done in the driver?)
• do everything for the rest of gyro/accel drivers
• optimize message used for estimation: do we need RAW data there? (do we need to publish raw data at all?)
• add interface for new driver options (e.g. to enable unfiltered publishing)
• check ISR load, probably the cause for increased CPU load in every task when increasing the publishing rates
• read data with DMA/FIFO

Is that going into the right direction?
@dagar you mentioned you could help with DMA?

[dagar]

I think we can stop doing this now.

[dagar]

I would consider consolidating these and dropping as many as possible. If you profile a nuttx board you'll find they definitely aren't free.

[dagar]

Awesome! I didn't know you wanted to push this through, but we might as well do it now.

I was thinking of SPI DMA and doing the work in a high priority thread per bus. It also might make sense to split it up a bit. There's a larger driver architecture discussion we need to have, but let me see if I can make some progress on SPI DMA drivers this weekend.

[dagar]

Questions

• is logging raw feasible under normal operation? Do we need queuing for that uorb message or an or use a single message with an array of values

[dagar]

Rebased on master and added a trivial formatting fix.

[dagar]

If we only run the attitude control portion with attitude updates (8ca0302), the mc_att_control cpu usage is 17% at 2kHz and ~8-9% at 1kHz.

What's interesting though is that the actual work of the rate controller (copy gyro data, PID, publish) is incredibly small (~ 20us). It's something else in the system (uORB, poll, etc) that's killing us.
#6840

sensor_baro                      0    3   68     0 1
sensor_accel                     0    1  250     0 1
sensor_gyro                      0    1  254     0 1
sensor_gyro_control              0    1 1020     0 1
sensor_gyro_unfiltered           0    0 2040     0 1
sensor_gyro                      1    1  237     8 1
sensor_mag                       0    1   99     0 1
sensor_accel                     1    1  234     0 1
adc_report                       0    1  100     0 1
system_power                     0    2  100    11 1
vehicle_control_mode             0    8    4     0 1
actuator_controls_0              0    8 1020  6327 1
sensor_combined                  0    6  254   928 1
sensor_preflight                 0    1  254     0 1
battery_status                   0    8   84   273 1
vehicle_status                   0   10    4     0 1
actuator_armed                   0    9    4     2 1
safety                           0    1   49     0 1
manual_control_setpoint          0    5    4     0 1
vehicle_local_position           0    8  254  1716 1
vehicle_attitude                 0    8  254  1040 1
estimator_status                 0    5  254  2335 1
commander_state                  0    0    4     0 1
servorail_status                 0    0   49     0 1
actuator_outputs                 0    4   49   201 1
multirotor_motor_limits          0    1   49     0 1
actuator_outputs                 1    2  347   321 1
ekf2_timestamps                  0    1  254     0 1
vehicle_rates_setpoint           0    2  254     0 1
sensor_bias                      0    4  254   754 1
task_stack_info                  0    0    1     0 2
wind_estimate                    0    4  254   240 1
ekf2_innovations                 0    1  254     0 1
mc_att_ctrl_status               0    0 1020     0 1

mpu600 perf

mpu6k_duplicates: 714135 events
mpu6k_reset: 0 events
mpu6k_good_trans: 1398388 events
mpu6k_bad_reg: 0 events
mpu6k_bad_trans: 0 events
mpu6k_read: 1398423 events, 98378869us elapsed, 70us avg, min 51us max 90us 6.436us rms
mpu6k_gyro_read: 0 events
mpu6k_acc_read: 0 events
mpu6k_publish_interval_gyro_control: 1398477 events, 489us avg, min 342us max 637us 24.193us rms
mpu6k_publish_interval_gyro: 174814 events, 3919us avg, min 3797us max 4039us 21.613us rms
mpu6k_sample_interval_gyro: 1398550 events, 489us avg, min 357us max 622us 23.251us rms
mpu6k_sample_interval_accel: 684338 events, 1001us avg, min 449us max 2497us 107.922us rms

[AndreasAntener]

@dagar I see you already added the mc att control update too, cool :). Yeah the load must be somewhere else, I was pretty surprised when I saw 22% for not doing so much..

is logging raw feasible under normal operation? Do we need queuing for that uorb message or an or use a single message with an array of values

It would definitely need some more magic to do this. Currently, for logging 2 IMUs at 2kHz I needed the logger to run at 4kHz, and even with a larger buffer I see periodic drops at the write operation (I think). I'm using the fastest possible SD cards and only logging the IMU data. The logger uses ~20% CPU in that case. At least for VTOL I don't see this happening at the moment with a v2/v3. But as we see there is a lot of cpu power vanishing, so lets have a look how this develops.

[dagar]

Closing this PR, but keeping the branch for now.
New effort (with SPI DMA) in #10602