WIP: High-rate IMU sampling

[AuterionWrikeBot]

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?

[AuterionWrikeBot]

➤ Daniel Agar commented:

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.

[AuterionWrikeBot]

➤ Daniel Agar commented:

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

[AuterionWrikeBot]

➤ Daniel Agar commented:

Rebased on master and added a trivial formatting fix.

[AuterionWrikeBot]

➤ Daniel Agar commented:

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 1mpu600 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

[AuterionWrikeBot]

➤ Andreas Daniel Antener commented:

@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.

[AuterionWrikeBot]

➤ Daniel Agar commented:

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.

[AuterionWrikeBot]

➤ Daniel Agar commented:

Rebased on master and added a trivial formatting fix.

[AuterionWrikeBot]

➤ Daniel Agar commented:

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

[AuterionWrikeBot]

➤ Andreas Daniel Antener commented:

@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.

[AuterionWrikeBot]

➤ Daniel Agar commented:

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 1mpu600 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