Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

drivers/imu/invensense/icm42670p: cleanup and small fixes #19660

Merged
merged 2 commits into from
May 17, 2022
Merged

drivers/imu/invensense/icm42670p: cleanup and small fixes #19660

merged 2 commits into from
May 17, 2022

Conversation

dagar
Copy link
Member

@dagar dagar commented May 16, 2022

No description provided.

mcsauder
mcsauder reviewed May 16, 2022
View reviewed changes
src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp
GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps
GYRO_FS_SEL_1000_DPS = Bit5, // 0b001 = ±1000 dps
GYRO_FS_SEL_500_DPS = Bit6, // 0b010 = ±500 dps
GYRO_FS_SEL_250_DPS = Bit6 | Bit5, // 0b011 = ±250 dps
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi @dagar . What do you think about leaving the lower dynamic gyro and accel ranges in the enum so that if anyone wants to configure the range manually they have an easy copy paste into the register config in the header file?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@mcsauder can we merge this As is?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we have any evidence that the lower range offered any measurable improvement? At the moment I'm much more concerned with the failures that happen due to the range being insufficient (eg accel clipping).

Additionally the icm42670p supports 20 bit data output (just like the icm42688p), but we haven't implemented it. Bothering to get the full output resolution at max dynamic range is still more resolution than lower dynamic range + 16 bit raw data.

I can add 20 bit support to this driver later.

mcsauder
mcsauder previously approved these changes May 16, 2022
View reviewed changes
Copy link
Contributor

@mcsauder mcsauder left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

One comment, otherwise these changes look good to me. I do not have hardware to test, but the logic changes all look correct from what I can see. Nice cleanup @dagar !

@davids5
Copy link
Member

davids5 commented May 16, 2022

@dagar Tested and working!

@davids5
Copy link
Member

davids5 commented May 16, 2022 

update: 1s, topics: 132, total publications: 257, 23.0 kB/s
TOPIC NAME        INST #SUB RATE #Q SIZE
sensors_status_baro  0    0   23  1   64
sensors_status_imu   0    2  206  1   96
sensors_status_mag   0    0   28  1   64
nsh> work_queue status

Work Queue: 14 threads                          RATE        INTERVAL
|__ 1) wq:rate_ctrl
|   \__ 1) vehicle_angular_velocity         411.4 Hz         2431 us
|__ 2) wq:SPI1
|   \__ 1) icm42670p                        398.1 Hz         2512 us
|__ 3) wq:SPI2
|   \__ 1) icm42688p                        405.2 Hz         2468 us
|__ 4) wq:SPI3
|   \__ 1) icm20649                         411.3 Hz         2432 us (2444 us)
|__ 5) wq:I2C1
|   \__ 1) ina226                             2.0 Hz       500276 us
|__ 6) wq:I2C2
|   |__ 1) bmp388                            23.1 Hz        43296 us (43300 us)
|   \__ 2) ina226                             2.0 Hz       500278 us
|__ 7) wq:I2C4
|   |__ 1) bmm150                            20.0 Hz        49978 us (50000 us)
|   \__ 2) bmp388                            23.1 Hz        43296 us (43300 us)
|__ 8) wq:nav_and_controllers
|   |__ 1) ekf2_selector                    302.5 Hz         3306 us
|   |__ 2) sensors                          205.7 Hz         4861 us
|   |__ 3) vehicle_acceleration             205.7 Hz         4861 us
|   |__ 4) vehicle_air_data                  23.1 Hz        43309 us
|   |__ 5) vehicle_gps_position               3.3 Hz       299861 us
|   \__ 6) vehicle_magnetometer              27.8 Hz        36031 us
|__ 9) wq:INS0
|   |__ 1) ekf2                             205.7 Hz         4862 us
|   \__ 2) vehicle_imu                      205.9 Hz         4856 us
|__ 10) wq:INS1
|   |__ 1) ekf2                             202.6 Hz         4935 us
|   \__ 2) vehicle_imu                      202.9 Hz         4929 us
|__ 11) wq:INS2
|   |__ 1) ekf2                             199.1 Hz         5024 us
|   \__ 2) vehicle_imu                      199.3 Hz         5017 us
|__ 12) wq:hp_default
|   |__ 1) board_adc                        100.0 Hz        10000 us (10000 us)
|   |__ 2) gyro_fft                         205.7 Hz         4861 us
|   |__ 3) manual_control                     5.0 Hz       200004 us
|   |__ 4) rc_update                          0.0 Hz            0 us
|   |__ 5) safety_button                     30.3 Hz        32999 us (33000 us)
|   \__ 6) tone_alarm                         0.0 Hz     20605208 us
|__ 13) wq:uavcan
|   |__ 1) uavcan                           333.3 Hz         3000 us (3000 us)
|   |__ 2) uavcan-actuators-esc              10.0 Hz        99973 us (100000 us)
|   \__ 3) uavcan-actuators-servo            10.0 Hz        99952 us (100000 us)
\__ 14) wq:lp_default
    |__ 1) gyro_calibration                  50.0 Hz        19999 us (20000 us)
    |__ 2) load_mon                           2.0 Hz       499584 us (500000 us)
    |__ 3) mag_bias_estimator                50.0 Hz        19999 us (20000 us)
    \__ 4) send_event                        30.0 Hz        33331 us (33333 us)
nsh> icm42670p status
INFO  [SPI_I2C] Running on SPI Bus 1
INFO  [icm42670p] FIFO empty interval: 2500 us (400.0 Hz)
icm42670p: bad register: 0 events
icm42670p: bad transfer: 1 events
icm42670p: FIFO empty: 0 events
icm42670p: FIFO overflow: 0 events
icm42670p: FIFO reset: 1 events
icm42670p: DRDY missed: 0 events

nsh> sensors status
selected gyro: 2424858 (0)
validator: best: 0, prev best: 1, failsafe: YES (1 events)
sensor #0, prio: 50, state: OK
        val:  -0.0085, lp:  -0.0001 mean dev:  -0.0000 RMS:   0.1370 conf:   1.0000
        val:   0.0114, lp:  -0.0011 mean dev:   0.0000 RMS:   0.2039 conf:   1.0000
        val:  -0.0059, lp:  -0.0004 mean dev:   0.0000 RMS:   0.1200 conf:   1.0000
sensor #1, prio: 50, state: OK
        val:   0.0029, lp:   0.0080 mean dev:  -0.0000 RMS:   0.1363 conf:   1.0000
        val:   0.0130, lp:  -0.0001 mean dev:  -0.0000 RMS:   0.2073 conf:   1.0000
        val:   0.0143, lp:   0.0007 mean dev:  -0.0000 RMS:   0.1197 conf:   1.0000
sensor #2, prio: 50, state: OK
        val:  -0.0025, lp:  -0.0020 mean dev:   0.0000 RMS:   0.1410 conf:   1.0000
        val:  -0.0251, lp:  -0.0004 mean dev:   0.0000 RMS:   0.2055 conf:   1.0000
        val:  -0.0167, lp:   0.0023 mean dev:   0.0000 RMS:   0.1203 conf:   1.0000

selected accel: 2424858 (0)
validator: best: 0, prev best: 1, failsafe: YES (1 events)
sensor #0, prio: 50, state: OK
        val:  -1.5580, lp:  -1.5120 mean dev:  -0.0016 RMS:   0.6126 conf:   1.0000
        val:  -0.3709, lp:  -0.4081 mean dev:  -0.0005 RMS:   0.5074 conf:   1.0000
        val:  10.0290, lp:  10.1100 mean dev:   0.0203 RMS:   0.5879 conf:   1.0000
sensor #1, prio: 50, state: OK
        val:  -1.5171, lp:  -1.5743 mean dev:  -0.0016 RMS:   0.6197 conf:   1.0000
        val:  -0.6881, lp:  -0.6384 mean dev:  -0.0005 RMS:   0.5136 conf:   1.0000
        val:   9.6887, lp:   9.6980 mean dev:   0.0196 RMS:   0.5826 conf:   1.0000
sensor #2, prio: 50, state: OK
        val:  -1.6816, lp:  -1.6644 mean dev:  -0.0017 RMS:   0.6152 conf:   1.0000
        val:  -0.5146, lp:  -0.5313 mean dev:  -0.0004 RMS:   0.5181 conf:   1.0000
        val:   9.3943, lp:   9.7322 mean dev:   0.0197 RMS:   0.5851 conf:   1.0000

[vehicle_magnetometer] selected MAG: 4395041 (0)
validator: best: 0, prev best: 0, failsafe: NO (0 events)
sensor #0, prio: 50, state: OK
        val:   0.0179, lp:   0.0224 mean dev:   0.0071 RMS:   0.1206 conf:   1.0000
        val:  -0.0733, lp:  -0.0734 mean dev:  -0.0020 RMS:   0.0607 conf:   1.0000
        val:  -0.1246, lp:  -0.1334 mean dev:   0.0006 RMS:   0.0598 conf:   1.0000
MAG 4395041 EN: 1, offset: [0.000 0.000 0.000], scale: [1.000 1.000 1.000], Internal

[vehicle_air_data] selected BARO: 6780433 (1)
validator: best: 1, prev best: 1, failsafe: NO (0 events)
sensor #0, prio: 50, state: OK
        val: 99770.2578, lp: 99768.6875 mean dev:  -0.1907 RMS:   1.9243 conf:   1.0000
        val:  38.8800, lp:  38.8499 mean dev:   0.0911 RMS:   0.0471 conf:   1.0000
        val: 130.2267, lp: 130.3941 mean dev:   0.0078 RMS:   0.1620 conf:   1.0000
sensor #1, prio: 75, state: OK
        val: 99803.5313, lp: 99805.3984 mean dev:  -0.1818 RMS:   1.7512 conf:   1.0000
        val:  38.6700, lp:  38.6509 mean dev:   0.0800 RMS:   0.0593 conf:   1.0000
        val: 127.3598, lp: 127.2988 mean dev:   0.0075 RMS:   0.1475 conf:   1.0000
BARO 6780705 EN: 1, offset: 0.000, Internal
BARO 6780433 EN: 1, offset: 0.000, Ext

Airspeed status:
        no data

[vehicle_acceleration] selected sensor: 2424858, rate: 411.3 Hz, estimated bias: [0.1133 0.1850 0.4000]
ACC 2424858 EN: 1, offset: [-0.070 -0.253 -0.400], scale: [1.000 1.000 1.000], Internal

[vehicle_angular_velocity] selected sensor: 2424858, rate: 9048.1 Hz FIFO, estimated bias: [0.00005 -0.00144 0.00026]
GYRO 2424858 EN: 1, offset: [-0.004 0.016 0.013], Internal
vehicle_angular_velocity: gyro filter: 202317 events, 3165393us elapsed, 15.65us avg, min 7us max 227us 1.035us rms
vehicle_angular_velocity: gyro filter reset: 2 events
vehicle_angular_velocity: gyro selection changed: 2 events



[vehicle_imu] 0 - Accel: 2424858, interval: 2432.0 us (SD 28.2 us), Gyro: 2424858, interval: 2432.0 us (SD 28.2 us)
vehicle_imu: accel data gap: 0 events
vehicle_imu: gyro data gap: 0 events
ACC 2424858 EN: 1, offset: [-0.070 -0.253 -0.400], scale: [1.000 1.000 1.000], Internal
GYRO 2424858 EN: 1, offset: [-0.004 0.016 0.013], Internal

[vehicle_imu] 1 - Accel: 2490386, interval: 2467.1 us (SD 2.3 us), Gyro: 2490386, interval: 2467.1 us (SD 2.3 us)
vehicle_imu: accel data gap: 1 events
vehicle_imu: gyro data gap: 1 events
ACC 2490386 EN: 1, offset: [0.029 -0.003 -0.094], scale: [1.000 1.000 1.000], Internal
GYRO 2490386 EN: 1, offset: [0.000 0.000 0.000], Internal

[vehicle_imu] 2 - Accel: 2752522, interval: 2511.5 us (SD 0.5 us), Gyro: 2752522, interval: 2511.5 us (SD 0.5 us)
vehicle_imu: accel data gap: 1 events
vehicle_imu: gyro data gap: 1 events
ACC 2752522 EN: 1, offset: [0.002 0.003 0.221], scale: [1.000 1.000 1.000], Internal
GYRO 2752522 EN: 1, offset: [0.000 0.000 0.000], Internal

@dagar
Copy link
Member Author

dagar commented May 17, 2022

@davids5 one minor thing I missed was the speed. Quickly validate the last change? 4582199

@davids5 davids5 self-requested a review May 17, 2022 14:56
davids5
davids5 approved these changes May 17, 2022
View reviewed changes
Copy link
Member

@davids5 davids5 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Actual == requested == 24mHZ

@davids5 davids5 merged commit ff16131 into master May 17, 2022
@davids5 davids5 deleted the pr-icm42670p_cleanup branch May 17, 2022 15:14
This was referenced Sep 6, 2022
Merged
Closed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@davids5 davids5 davids5 approved these changes

@mcsauder mcsauder mcsauder left review comments

Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
3 participants
@dagar @davids5 @mcsauder