From 44df0fb7a25e8387812399458a18fadb24a7a03f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 14 Feb 2021 16:23:35 -0500 Subject: [PATCH] Analog Devices ADIS16448 rewrite - new IMU driver structure with state machine (no sleeps in bus thread) - verify all configured registers and trigger reset on failure - detect if DIO1 or DIO2 are actually connected for data ready interrupt usage - don't use CRC-16 on burst transfers except for verified lots --- boards/cuav/x7pro/default.cmake | 2 +- boards/cubepilot/cubeorange/console.cmake | 2 +- boards/cubepilot/cubeorange/default.cmake | 2 +- boards/cubepilot/cubeyellow/console.cmake | 2 +- boards/cubepilot/cubeyellow/default.cmake | 2 +- boards/holybro/durandal-v1/default.cmake | 2 +- boards/holybro/pix32v5/default.cmake | 2 +- boards/nxp/fmurt1062-v1/default.cmake | 2 +- boards/px4/fmu-v2/default.cmake | 2 +- boards/px4/fmu-v2/lpe.cmake | 2 +- boards/px4/fmu-v2/test.cmake | 2 +- boards/px4/fmu-v3/ctrlalloc.cmake | 2 +- boards/px4/fmu-v3/default.cmake | 2 +- boards/px4/fmu-v3/rtps.cmake | 2 +- boards/px4/fmu-v3/stackcheck.cmake | 2 +- boards/px4/fmu-v4/cannode.cmake | 2 +- boards/px4/fmu-v4/ctrlalloc.cmake | 2 +- boards/px4/fmu-v4/default.cmake | 2 +- boards/px4/fmu-v4/optimized.cmake | 2 +- boards/px4/fmu-v4/rtps.cmake | 2 +- boards/px4/fmu-v4/stackcheck.cmake | 2 +- boards/px4/fmu-v4/uavcanv1.cmake | 2 +- boards/px4/fmu-v5/critmonitor.cmake | 2 +- boards/px4/fmu-v5/ctrlalloc.cmake | 2 +- boards/px4/fmu-v5/debug.cmake | 2 +- boards/px4/fmu-v5/default.cmake | 2 +- boards/px4/fmu-v5/fixedwing.cmake | 2 +- boards/px4/fmu-v5/irqmonitor.cmake | 2 +- boards/px4/fmu-v5/multicopter.cmake | 2 +- boards/px4/fmu-v5/rover.cmake | 2 +- boards/px4/fmu-v5/rtps.cmake | 2 +- boards/px4/fmu-v5/stackcheck.cmake | 2 +- boards/px4/fmu-v5/uavcanv1.cmake | 2 +- boards/px4/fmu-v5x/base_phy_DP83848C.cmake | 2 +- boards/px4/fmu-v5x/default.cmake | 2 +- boards/px4/fmu-v6u/default.cmake | 2 +- boards/px4/fmu-v6u/stackcheck.cmake | 2 +- boards/px4/fmu-v6x/default.cmake | 2 +- boards/px4/fmu-v6x/stackcheck.cmake | 2 +- boards/spracing/h7extreme/default.cmake | 2 +- src/drivers/imu/CMakeLists.txt | 2 +- src/drivers/imu/adis16448/ADIS16448.cpp | 514 --------------- src/drivers/imu/adis16448/ADIS16448.h | 206 ------ src/drivers/imu/analog_devices/CMakeLists.txt | 34 + .../analog_devices/adis16448/ADIS16448.cpp | 594 ++++++++++++++++++ .../analog_devices/adis16448/ADIS16448.hpp | 138 ++++ .../Analog_Devices_ADIS16448_registers.hpp | 161 +++++ .../adis16448/CMakeLists.txt | 12 +- .../adis16448/adis16448_main.cpp | 17 +- 49 files changed, 983 insertions(+), 775 deletions(-) delete mode 100644 src/drivers/imu/adis16448/ADIS16448.cpp delete mode 100644 src/drivers/imu/adis16448/ADIS16448.h create mode 100644 src/drivers/imu/analog_devices/CMakeLists.txt create mode 100644 src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp create mode 100644 src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp create mode 100644 src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp rename src/drivers/imu/{ => analog_devices}/adis16448/CMakeLists.txt (89%) rename src/drivers/imu/{ => analog_devices}/adis16448/adis16448_main.cpp (91%) diff --git a/boards/cuav/x7pro/default.cmake b/boards/cuav/x7pro/default.cmake index bf4a25a36b..1e011dcc1e 100644 --- a/boards/cuav/x7pro/default.cmake +++ b/boards/cuav/x7pro/default.cmake @@ -30,7 +30,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi088 diff --git a/boards/cubepilot/cubeorange/console.cmake b/boards/cubepilot/cubeorange/console.cmake index 77f016be3e..043e4ce6cd 100644 --- a/boards/cubepilot/cubeorange/console.cmake +++ b/boards/cubepilot/cubeorange/console.cmake @@ -30,7 +30,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/invensense/icm20602 diff --git a/boards/cubepilot/cubeorange/default.cmake b/boards/cubepilot/cubeorange/default.cmake index b4353de6ab..0ba9f31077 100644 --- a/boards/cubepilot/cubeorange/default.cmake +++ b/boards/cubepilot/cubeorange/default.cmake @@ -30,7 +30,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/invensense/icm20602 diff --git a/boards/cubepilot/cubeyellow/console.cmake b/boards/cubepilot/cubeyellow/console.cmake index 2dc0be9875..2f2201d4d3 100644 --- a/boards/cubepilot/cubeyellow/console.cmake +++ b/boards/cubepilot/cubeyellow/console.cmake @@ -29,7 +29,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/invensense/icm20602 diff --git a/boards/cubepilot/cubeyellow/default.cmake b/boards/cubepilot/cubeyellow/default.cmake index f5697ce4a9..4b3a5d67b3 100644 --- a/boards/cubepilot/cubeyellow/default.cmake +++ b/boards/cubepilot/cubeyellow/default.cmake @@ -29,7 +29,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/invensense/icm20602 diff --git a/boards/holybro/durandal-v1/default.cmake b/boards/holybro/durandal-v1/default.cmake index af56dae993..82c153a347 100644 --- a/boards/holybro/durandal-v1/default.cmake +++ b/boards/holybro/durandal-v1/default.cmake @@ -29,7 +29,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi088 diff --git a/boards/holybro/pix32v5/default.cmake b/boards/holybro/pix32v5/default.cmake index 1c85d25df5..20f8115817 100644 --- a/boards/holybro/pix32v5/default.cmake +++ b/boards/holybro/pix32v5/default.cmake @@ -28,7 +28,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi055 diff --git a/boards/nxp/fmurt1062-v1/default.cmake b/boards/nxp/fmurt1062-v1/default.cmake index fa4f14052f..23dc2d9989 100644 --- a/boards/nxp/fmurt1062-v1/default.cmake +++ b/boards/nxp/fmurt1062-v1/default.cmake @@ -24,7 +24,7 @@ px4_add_board( distance_sensor # all available distance sensor drivers # dshot not ported gps - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 #imu # all available imu drivers diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index 5957115394..f65075961f 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -34,7 +34,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 imu/l3gd20 diff --git a/boards/px4/fmu-v2/lpe.cmake b/boards/px4/fmu-v2/lpe.cmake index 1757a4912c..a21c45816d 100644 --- a/boards/px4/fmu-v2/lpe.cmake +++ b/boards/px4/fmu-v2/lpe.cmake @@ -32,7 +32,7 @@ px4_add_board( distance_sensor # all available distance sensor drivers gps #heater - #imu/adis16448 + #imu/analog_devices/adis16448 #imu # all available imu drivers imu/l3gd20 imu/lsm303d diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake index aab53f5cd4..732f116d41 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -33,7 +33,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 imu/l3gd20 diff --git a/boards/px4/fmu-v3/ctrlalloc.cmake b/boards/px4/fmu-v3/ctrlalloc.cmake index e17094c16d..726900aaf6 100644 --- a/boards/px4/fmu-v3/ctrlalloc.cmake +++ b/boards/px4/fmu-v3/ctrlalloc.cmake @@ -31,7 +31,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/l3gd20 diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index 2ade27c004..6891046e53 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -31,7 +31,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/l3gd20 diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index 012a8a6fb4..cc2ad9ee8e 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -30,7 +30,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/l3gd20 diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake index 0dcc05bee9..cba93b5f94 100644 --- a/boards/px4/fmu-v3/stackcheck.cmake +++ b/boards/px4/fmu-v3/stackcheck.cmake @@ -30,7 +30,7 @@ px4_add_board( distance_sensor # all available distance sensor drivers gps #heater - imu/adis16448 + imu/analog_devices/adis16448 #imu # all available imu drivers imu/l3gd20 imu/lsm303d diff --git a/boards/px4/fmu-v4/cannode.cmake b/boards/px4/fmu-v4/cannode.cmake index cb54af827d..fb922141b6 100644 --- a/boards/px4/fmu-v4/cannode.cmake +++ b/boards/px4/fmu-v4/cannode.cmake @@ -42,7 +42,7 @@ px4_add_board( #dshot gps #imu # all available imu drivers - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 imu/invensense/icm20602 diff --git a/boards/px4/fmu-v4/ctrlalloc.cmake b/boards/px4/fmu-v4/ctrlalloc.cmake index df65099c1e..d94052facf 100644 --- a/boards/px4/fmu-v4/ctrlalloc.cmake +++ b/boards/px4/fmu-v4/ctrlalloc.cmake @@ -29,7 +29,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/invensense/icm20602 diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 65495b3e1c..6c88eb2c8e 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -29,7 +29,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/invensense/icm20602 diff --git a/boards/px4/fmu-v4/optimized.cmake b/boards/px4/fmu-v4/optimized.cmake index 1dba592281..05bb456997 100644 --- a/boards/px4/fmu-v4/optimized.cmake +++ b/boards/px4/fmu-v4/optimized.cmake @@ -30,7 +30,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 imu/invensense/icm20602 diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index eac3a3fb82..d87230acab 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -26,7 +26,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/invensense/icm20602 diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index cbdb8569cd..45e7577208 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -26,7 +26,7 @@ px4_add_board( gps heater #imu # all available imu drivers - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 imu/invensense/icm20602 diff --git a/boards/px4/fmu-v4/uavcanv1.cmake b/boards/px4/fmu-v4/uavcanv1.cmake index b689e945d7..b4a9b29283 100644 --- a/boards/px4/fmu-v4/uavcanv1.cmake +++ b/boards/px4/fmu-v4/uavcanv1.cmake @@ -28,7 +28,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/invensense/icm20602 diff --git a/boards/px4/fmu-v5/critmonitor.cmake b/boards/px4/fmu-v5/critmonitor.cmake index a490448a66..48d0368bbd 100644 --- a/boards/px4/fmu-v5/critmonitor.cmake +++ b/boards/px4/fmu-v5/critmonitor.cmake @@ -27,7 +27,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi055 diff --git a/boards/px4/fmu-v5/ctrlalloc.cmake b/boards/px4/fmu-v5/ctrlalloc.cmake index cd3f1ec6a7..0adf5cff0d 100644 --- a/boards/px4/fmu-v5/ctrlalloc.cmake +++ b/boards/px4/fmu-v5/ctrlalloc.cmake @@ -28,7 +28,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi055 diff --git a/boards/px4/fmu-v5/debug.cmake b/boards/px4/fmu-v5/debug.cmake index f7f1a79584..e157e46b59 100644 --- a/boards/px4/fmu-v5/debug.cmake +++ b/boards/px4/fmu-v5/debug.cmake @@ -29,7 +29,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 #imu/bosch/bmi055 diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 3ad3f7fe8e..8eedc7b875 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -28,7 +28,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi055 diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index f7b35b887e..9ff4e3286d 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -23,7 +23,7 @@ px4_add_board( differential_pressure # all available differential pressure drivers distance_sensor # all available distance sensor drivers gps - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 #imu # all available imu drivers diff --git a/boards/px4/fmu-v5/irqmonitor.cmake b/boards/px4/fmu-v5/irqmonitor.cmake index 89c5d85f78..84f51e730a 100644 --- a/boards/px4/fmu-v5/irqmonitor.cmake +++ b/boards/px4/fmu-v5/irqmonitor.cmake @@ -27,7 +27,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi055 diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index 607e1a74c6..f966510aa2 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -24,7 +24,7 @@ px4_add_board( distance_sensor # all available distance sensor drivers dshot gps - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 #imu # all available imu drivers diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index 63909681e5..94d03e80e9 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -22,7 +22,7 @@ px4_add_board( camera_trigger distance_sensor # all available distance sensor drivers gps - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi055 diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index e3df35d28e..615857d1b2 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -27,7 +27,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi055 diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 8fa71424c2..d497763de4 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -29,7 +29,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 #imu/bosch/bmi055 diff --git a/boards/px4/fmu-v5/uavcanv1.cmake b/boards/px4/fmu-v5/uavcanv1.cmake index 56aaf8ed3e..b75b5353ff 100644 --- a/boards/px4/fmu-v5/uavcanv1.cmake +++ b/boards/px4/fmu-v5/uavcanv1.cmake @@ -28,7 +28,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi055 diff --git a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake index ca8280efe0..e9bdbce90e 100644 --- a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake +++ b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake @@ -28,7 +28,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi088 diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index 0e57ae8611..d760d664df 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -29,7 +29,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi088 diff --git a/boards/px4/fmu-v6u/default.cmake b/boards/px4/fmu-v6u/default.cmake index c04dcd5d8a..4c839c807c 100644 --- a/boards/px4/fmu-v6u/default.cmake +++ b/boards/px4/fmu-v6u/default.cmake @@ -28,7 +28,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi088 diff --git a/boards/px4/fmu-v6u/stackcheck.cmake b/boards/px4/fmu-v6u/stackcheck.cmake index c04dcd5d8a..4c839c807c 100644 --- a/boards/px4/fmu-v6u/stackcheck.cmake +++ b/boards/px4/fmu-v6u/stackcheck.cmake @@ -28,7 +28,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi088 diff --git a/boards/px4/fmu-v6x/default.cmake b/boards/px4/fmu-v6x/default.cmake index 0ba7936de3..0444cfb5d6 100644 --- a/boards/px4/fmu-v6x/default.cmake +++ b/boards/px4/fmu-v6x/default.cmake @@ -29,7 +29,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi088 diff --git a/boards/px4/fmu-v6x/stackcheck.cmake b/boards/px4/fmu-v6x/stackcheck.cmake index 0ba7936de3..0444cfb5d6 100644 --- a/boards/px4/fmu-v6x/stackcheck.cmake +++ b/boards/px4/fmu-v6x/stackcheck.cmake @@ -29,7 +29,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi088 diff --git a/boards/spracing/h7extreme/default.cmake b/boards/spracing/h7extreme/default.cmake index 26c97d62b4..9c7f0ada3a 100644 --- a/boards/spracing/h7extreme/default.cmake +++ b/boards/spracing/h7extreme/default.cmake @@ -24,7 +24,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 #imu/bmi088 diff --git a/src/drivers/imu/CMakeLists.txt b/src/drivers/imu/CMakeLists.txt index 54947f6dd5..ef867fd353 100644 --- a/src/drivers/imu/CMakeLists.txt +++ b/src/drivers/imu/CMakeLists.txt @@ -31,9 +31,9 @@ # ############################################################################ -add_subdirectory(adis16448) add_subdirectory(adis16477) add_subdirectory(adis16497) +add_subdirectory(analog_devices) add_subdirectory(bma180) add_subdirectory(bmi055) add_subdirectory(bmi088) diff --git a/src/drivers/imu/adis16448/ADIS16448.cpp b/src/drivers/imu/adis16448/ADIS16448.cpp deleted file mode 100644 index 7897ace6d1..0000000000 --- a/src/drivers/imu/adis16448/ADIS16448.cpp +++ /dev/null @@ -1,514 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ADIS16448.cpp - */ - -#include "ADIS16448.h" - -ADIS16448::ADIS16448(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode) : - SPI(DRV_IMU_DEVTYPE_ADIS16448, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_accel(get_device_id(), rotation), - _px4_baro(get_device_id()), - _px4_gyro(get_device_id(), rotation), - _px4_mag(get_device_id(), rotation) -{ - _px4_accel.set_scale(ADIS16448_ACCEL_SENSITIVITY); - _px4_gyro.set_scale(ADIS16448_GYRO_INITIAL_SENSITIVITY); - _px4_mag.set_scale(ADIS16448_MAG_SENSITIVITY); - - _px4_mag.set_external(external()); -} - -ADIS16448::~ADIS16448() -{ - // Delete the perf counter. - perf_free(_perf_read); - perf_free(_perf_transfer); - perf_free(_perf_bad_transfer); - perf_free(_perf_crc_bad); -} - -int -ADIS16448::init() -{ - // Do SPI init (and probe) first. - int ret = SPI::init(); - - if (ret != PX4_OK) { - DEVICE_DEBUG("SPI setup failed %d", ret); - - // If probe/setup failed, return result. - return ret; - } - - ret = measure(); - - if (ret != PX4_OK) { - PX4_ERR("measure failed"); - return PX4_ERROR; - } - - start(); - - return OK; -} - -bool ADIS16448::reset() -{ - // Software reset - write_reg16(ADIS16448_GLOB_CMD, 1 << 7); // GLOB_CMD bit 7 Software reset - - // Reset Recovery Time 90 ms - usleep(90000); - - if (!self_test()) { - return false; - } - - // Factory calibration restore - //write_reg16(ADIS16448_GLOB_CMD, 1 << 1); // GLOB_CMD bit 1 Factory calibration restore - - // include the CRC-16 code in burst read output sequence - write_reg16(ADIS16448_MSC_CTRL, 1 << 4); - - // Set digital FIR filter tap. - //if (!set_dlpf_filter(BITS_FIR_NO_TAP_CFG)) { - // return PX4_ERROR; - //} - - // Set IMU sample rate. - if (!set_sample_rate(_sample_rate)) { - return false; - } - - // Set gyroscope scale to default value. - //if (!set_gyro_dyn_range(GYRO_INITIAL_SENSITIVITY)) { - // return false; - //} - - // Settling time. - usleep(100000); - - return true; -} - -bool ADIS16448::self_test() -{ - bool ret = true; - - // start internal self test routine - write_reg16(ADIS16448_MSC_CTRL, 0x04); // MSC_CTRL bit 10 Internal self test (cleared upon completion) - - // Automatic Self-Test Time 45 ms - usleep(45000); - - // check test status (ADIS16448_DIAG_STAT) - const uint16_t status = read_reg16(ADIS16448_DIAG_STAT); - - const bool self_test_error = (status & (1 << 5)); // 5: Self-test diagnostic error flag - - if (self_test_error) { - //PX4_ERR("self test failed DIAG_STAT: 0x%04X", status); - - // Magnetometer - const bool mag_fail = (status & (1 << 0)); // 0: Magnetometer functional test - - if (mag_fail) { - // tolerate mag test failure (likely due to surrounding magnetic field) - PX4_WARN("Magnetometer functional test fail"); - } - - // Barometer - const bool baro_fail = (status & (1 << 1)); // 1: Barometer functional test - - if (baro_fail) { - PX4_ERR("Barometer functional test test fail"); - ret = false; - } - - // Gyroscope - const bool gyro_x_fail = (status & (1 << 10)); // 10: X-axis gyroscope self-test failure - const bool gyro_y_fail = (status & (1 << 11)); // 11: Y-axis gyroscope self-test failure - const bool gyro_z_fail = (status & (1 << 12)); // 12: Z-axis gyroscope self-test failure - - if (gyro_x_fail || gyro_y_fail || gyro_z_fail) { - PX4_ERR("gyroscope self-test failure"); - ret = false; - } - - // Accelerometer - const bool accel_x_fail = (status & (1 << 13)); // 13: X-axis accelerometer self-test failure - const bool accel_y_fail = (status & (1 << 14)); // 14: Y-axis accelerometer self-test failure - const bool accel_z_fail = (status & (1 << 15)); // 15: Z-axis accelerometer self-test failure - - if (accel_x_fail || accel_y_fail || accel_z_fail) { - PX4_ERR("accelerometer self-test failure"); - ret = false; - } - } - - return ret; -} - -int -ADIS16448::probe() -{ - bool reset_success = reset(); - - // Retry 5 time to get the ADIS16448 PRODUCT ID number. - for (size_t i = 0; i < 5; i++) { - // Read product ID. - _product_ID = read_reg16(ADIS16448_PRODUCT_ID); - - if (_product_ID == ADIS16448_Product) { - break; - } - - reset_success = reset(); - } - - if (!reset_success) { - DEVICE_DEBUG("unable to successfully reset"); - return PX4_ERROR; - } - - // Recognize product serial number. - uint16_t serial_number = (read_reg16(ADIS16334_SERIAL_NUMBER) & 0xfff); - - // Verify product ID. - switch (_product_ID) { - case ADIS16448_Product: - DEVICE_DEBUG("ADIS16448 is detected ID: 0x%02x, Serial: 0x%02x", _product_ID, serial_number); - modify_reg16(ADIS16448_GPIO_CTRL, 0x0200, 0x0002); // Turn on ADIS16448 adaptor board led. - return OK; - } - - DEVICE_DEBUG("unexpected ID 0x%02x", _product_ID); - - return -EIO; -} - -bool -ADIS16448::set_sample_rate(uint16_t desired_sample_rate_hz) -{ - uint16_t smpl_prd = 0; - - if (desired_sample_rate_hz <= 51) { - smpl_prd = BITS_SMPL_PRD_16_TAP_CFG; - - } else if (desired_sample_rate_hz <= 102) { - smpl_prd = BITS_SMPL_PRD_8_TAP_CFG; - - } else if (desired_sample_rate_hz <= 204) { - smpl_prd = BITS_SMPL_PRD_4_TAP_CFG; - - } else if (desired_sample_rate_hz <= 409) { - smpl_prd = BITS_SMPL_PRD_2_TAP_CFG; - - } else { - smpl_prd = BITS_SMPL_PRD_NO_TAP_CFG; - } - - modify_reg16(ADIS16448_SMPL_PRD, 0x1F00, smpl_prd); - - if ((read_reg16(ADIS16448_SMPL_PRD) & 0x1F00) != smpl_prd) { - PX4_ERR("failed to set IMU sample rate"); - - return false; - } - - return true; -} - -bool -ADIS16448::set_dlpf_filter(uint16_t desired_filter_tap) -{ - // Set the DLPF FIR filter tap. This affects both accelerometer and gyroscope. - modify_reg16(ADIS16448_SENS_AVG, 0x0007, desired_filter_tap); - - // Verify data write on the IMU. - if ((read_reg16(ADIS16448_SENS_AVG) & 0x0007) != desired_filter_tap) { - PX4_ERR("failed to set IMU filter"); - - return false; - } - - return true; -} - -bool -ADIS16448::set_gyro_dyn_range(uint16_t desired_gyro_dyn_range) -{ - uint16_t gyro_range_selection = 0; - - if (desired_gyro_dyn_range <= 250) { - gyro_range_selection = BITS_GYRO_DYN_RANGE_250_CFG; - - } else if (desired_gyro_dyn_range <= 500) { - gyro_range_selection = BITS_GYRO_DYN_RANGE_500_CFG; - - } else { - gyro_range_selection = BITS_GYRO_DYN_RANGE_1000_CFG; - } - - modify_reg16(ADIS16448_SENS_AVG, 0x0700, gyro_range_selection); - - // Verify data write on the IMU. - if ((read_reg16(ADIS16448_SENS_AVG) & 0x0700) != gyro_range_selection) { - PX4_ERR("failed to set gyro range"); - return false; - - } else { - _px4_gyro.set_scale(((gyro_range_selection >> 8) / 100.0f) * M_PI_F / 180.0f); - } - - return true; -} - -void -ADIS16448::print_status() -{ - I2CSPIDriverBase::print_status(); - perf_print_counter(_perf_read); - perf_print_counter(_perf_transfer); - perf_print_counter(_perf_bad_transfer); - perf_print_counter(_perf_crc_bad); -} - -void -ADIS16448::modify_reg16(unsigned reg, uint16_t clearbits, uint16_t setbits) -{ - uint16_t val = read_reg16(reg); - val &= ~clearbits; - val |= setbits; - write_reg16(reg, val); -} - -uint16_t -ADIS16448::read_reg16(unsigned reg) -{ - uint16_t cmd[1]; - - cmd[0] = ((reg | DIR_READ) << 8) & 0xff00; - - transferhword(cmd, nullptr, 1); - usleep(T_STALL); - transferhword(nullptr, cmd, 1); - usleep(T_STALL); - - return cmd[0]; -} - -void -ADIS16448::write_reg16(unsigned reg, uint16_t value) -{ - uint16_t cmd[2]; - - cmd[0] = ((reg | DIR_WRITE) << 8) | (0x00ff & value); - cmd[1] = (((reg + 0x1) | DIR_WRITE) << 8) | ((0xff00 & value) >> 8); - - transferhword(cmd, nullptr, 1); - usleep(T_STALL); - transferhword(cmd + 1, nullptr, 1); - usleep(T_STALL); -} - -void -ADIS16448::start() -{ - // Start polling at the specified interval - ScheduleOnInterval((1_s / _sample_rate), 10000); -} - -// computes the CCITT CRC16 on the data received from a burst read -static uint16_t ComputeCRC16(uint16_t burstData[13]) -{ - uint16_t crc = 0xFFFF; // Holds the CRC value - - unsigned int data; // Holds the lower/Upper byte for CRC computation - static constexpr unsigned int POLY = 0x1021; // Divisor used during CRC computation - - // Compute CRC on burst data starting from XGYRO_OUT and ending with TEMP_OUT. - // Start with the lower byte and then the upper byte of each word. - // i.e. Compute XGYRO_OUT_LSB CRC first and then compute XGYRO_OUT_MSB CRC. - for (int i = 1; i < 12; i++) { - unsigned int upperByte = (burstData[i] >> 8) & 0xFF; - unsigned int lowerByte = (burstData[i] & 0xFF); - data = lowerByte; // Compute lower byte CRC first - - for (int ii = 0; ii < 8; ii++, data >>= 1) { - if ((crc & 0x0001) ^ (data & 0x0001)) { - crc = (crc >> 1) ^ POLY; - - } else { - crc >>= 1; - } - } - - data = upperByte; // Compute upper byte of CRC - - for (int ii = 0; ii < 8; ii++, data >>= 1) { - if ((crc & 0x0001) ^ (data & 0x0001)) { - crc = (crc >> 1) ^ POLY; - - } else { - crc >>= 1; - } - } - } - - crc = ~crc; // Compute complement of CRC - data = crc; - crc = (crc << 8) | (data >> 8 & 0xFF); // Perform byte swap prior to returning CRC - - return crc; -} - -/** - * convert 12 bit integer format to int16. - */ -static int16_t -convert12BitToINT16(uint16_t word) -{ - int16_t outputbuffer = 0; - - if ((word >> 11) & 0x1) { - outputbuffer = (word & 0xfff) | 0xf000; - - } else { - outputbuffer = (word & 0x0fff); - } - - return (outputbuffer); -} - -int -ADIS16448::measure() -{ - // Start measuring. - perf_begin(_perf_read); - - // Fetch the full set of measurements from the ADIS16448 in one pass (burst read). -#pragma pack(push, 1) // Ensure proper memory alignment. - struct Report { - uint16_t cmd; - - uint16_t DIAG_STAT; - - int16_t XGYRO_OUT; - int16_t YGYRO_OUT; - int16_t ZGYRO_OUT; - - int16_t XACCL_OUT; - int16_t YACCL_OUT; - int16_t ZACCL_OUT; - - int16_t XMAGN_OUT; - int16_t YMAGN_OUT; - int16_t ZMAGN_OUT; - - uint16_t BARO_OUT; - - uint16_t TEMP_OUT; - - uint16_t CRC16; - } report{}; -#pragma pack(pop) - - report.cmd = ((ADIS16448_GLOB_CMD | DIR_READ) << 8) & 0xff00; - - const hrt_abstime timestamp_sample = hrt_absolute_time(); - - perf_begin(_perf_transfer); - - if (OK != transferhword((uint16_t *)&report, ((uint16_t *)&report), sizeof(report) / sizeof(int16_t))) { - perf_end(_perf_transfer); - perf_end(_perf_read); - - perf_count(_perf_bad_transfer); - - return -EIO; - } - - perf_end(_perf_transfer); - - // checksum - if (report.CRC16 != ComputeCRC16((uint16_t *)&report.DIAG_STAT)) { - perf_count(_perf_crc_bad); - perf_end(_perf_read); - return -EIO; - } - - // error count - const uint64_t error_count = perf_event_count(_perf_bad_transfer) + perf_event_count(_perf_crc_bad); - - // temperature - const float temperature = (convert12BitToINT16(report.TEMP_OUT) * 0.07386f) + 31.0f; // 0.07386°C/LSB, 31°C = 0x000 - - _px4_accel.set_error_count(error_count); - _px4_accel.set_temperature(temperature); - _px4_accel.update(timestamp_sample, report.XACCL_OUT, report.YACCL_OUT, report.ZACCL_OUT); - - _px4_gyro.set_error_count(error_count); - _px4_gyro.set_temperature(temperature); - _px4_gyro.update(timestamp_sample, report.XGYRO_OUT, report.YGYRO_OUT, report.ZGYRO_OUT); - - // DIAG_STAT bit 7: New data, xMAGN_OUT/BARO_OUT - const bool baro_mag_update = (report.DIAG_STAT & (1 << 7)); - - if (baro_mag_update) { - _px4_mag.set_error_count(error_count); - _px4_mag.set_temperature(temperature); - _px4_mag.update(timestamp_sample, report.XMAGN_OUT, report.YMAGN_OUT, report.ZMAGN_OUT); - - _px4_baro.set_error_count(error_count); - _px4_baro.set_temperature(temperature); - _px4_baro.update(timestamp_sample, report.BARO_OUT * ADIS16448_BARO_SENSITIVITY); - } - - // Stop measuring. - perf_end(_perf_read); - - return OK; -} - -void -ADIS16448::RunImpl() -{ - // Make another measurement. - measure(); -} diff --git a/src/drivers/imu/adis16448/ADIS16448.h b/src/drivers/imu/adis16448/ADIS16448.h deleted file mode 100644 index 0b0b167996..0000000000 --- a/src/drivers/imu/adis16448/ADIS16448.h +++ /dev/null @@ -1,206 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file adis16448.cpp - * - * Driver for the Analog device ADIS16448 connected via SPI. - * - * @author Amir Melzer - * @author Andrew Tridgell - * @author Pat Hickey - * @author Lorenz Meier - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -#define DIR_READ 0x00 -#define DIR_WRITE 0x80 - -#define ADIS16448_GPIO_CTRL 0x32 /* Auxiliary digital input/output control */ -#define ADIS16448_MSC_CTRL 0x34 /* Miscellaneous control */ -#define ADIS16448_SMPL_PRD 0x36 /* Internal sample period (rate) control */ -#define ADIS16448_SENS_AVG 0x38 /* Dynamic range and digital filter control */ -#define ADIS16448_DIAG_STAT 0x3C /* System status */ -#define ADIS16448_GLOB_CMD 0x3E /* System command */ -#define ADIS16448_PRODUCT_ID 0x56 /* Product identifier */ -#define ADIS16334_SERIAL_NUMBER 0x58 /* Serial number, lot specific */ - -#define ADIS16448_Product 0x4040/* Product ID Description for ADIS16448 */ - -#define BITS_SMPL_PRD_NO_TAP_CFG (0<<8) -#define BITS_SMPL_PRD_2_TAP_CFG (1<<8) -#define BITS_SMPL_PRD_4_TAP_CFG (2<<8) -#define BITS_SMPL_PRD_8_TAP_CFG (3<<8) -#define BITS_SMPL_PRD_16_TAP_CFG (4<<8) - -#define BITS_GYRO_DYN_RANGE_1000_CFG (4<<8) -#define BITS_GYRO_DYN_RANGE_500_CFG (2<<8) -#define BITS_GYRO_DYN_RANGE_250_CFG (1<<8) - -#define BITS_FIR_NO_TAP_CFG (0<<0) -#define BITS_FIR_2_TAP_CFG (1<<0) -#define BITS_FIR_4_TAP_CFG (2<<0) -#define BITS_FIR_8_TAP_CFG (3<<0) -#define BITS_FIR_16_TAP_CFG (4<<0) -#define BITS_FIR_32_TAP_CFG (5<<0) -#define BITS_FIR_64_TAP_CFG (6<<0) -#define BITS_FIR_128_TAP_CFG (7<<0) - -#define T_STALL 9 - -static constexpr float ADIS16448_ACCEL_SENSITIVITY{1.0f / 1200.0f * CONSTANTS_ONE_G}; // 1200 LSB/g -static constexpr float ADIS16448_GYRO_INITIAL_SENSITIVITY{math::radians(1.0 / 25.0)}; // 25 LSB/°/sec -static constexpr float ADIS16448_BARO_SENSITIVITY{0.02f}; // 20 microbar per LSB, -static constexpr float ADIS16448_MAG_SENSITIVITY{1.0 / 7.0 / 1000.0}; // 7 LSB/mgauss - - -static constexpr float ADIS16448_ACCEL_GYRO_UPDATE_RATE{819.2}; // accel and gryo max update 819.2 samples per second -static constexpr float ADIS16448_MAG_BARO_UPDATE_RATE{51.2}; // xMAGN_OUT and BARO_OUT registers update at 51.2 samples per second - -class ADIS16448 : public device::SPI, public I2CSPIDriver -{ -public: - ADIS16448(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode); - virtual ~ADIS16448(); - - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); - static void print_usage(); - - int init() override; - - void print_status() override; - - void RunImpl(); -protected: - int probe() override; - -private: - - enum class - Register : uint8_t { - GPIO_CTRL = 0x32, // Auxiliary digital input/output control - MSC_CTRL = 0x34, // Miscellaneous control - SMPL_PRD = 0x36, // Internal sample period (rate) control - SENS_AVG = 0x38, // Dynamic range and digital filter control - - DIAG_STAT = 0x3C, // System status - GLOB_CMD = 0x3E, // System command - - PRODUCT_ID = 0x56, // Product identifier - SERIAL_NUMBER = 0x58, // Serial number, lot specific - }; - - /** - * Fetch measurements from the sensor and update the report buffers. - */ - int measure(); - - /** - * Modify a register in the ADIS16448 - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg16(unsigned reg, uint16_t clearbits, uint16_t setbits); - - /** - * Resets the chip and measurements ranges - */ - bool reset(); - - bool self_test(); - - /** - * Read a register from the ADIS16448 - * @arg reg The register to read. - * @return Returns the register value. - */ - uint16_t read_reg16(unsigned reg); - - /** - * Write a register in the ADIS16448 - * @param reg The register to write. - * @param value The new value to write. - */ - void write_reg16(unsigned reg, uint16_t value); - - /** - * Set low pass filter frequency. - */ - bool set_dlpf_filter(uint16_t frequency_hz); - - /** - * Set the gyroscope dynamic range. - */ - bool set_gyro_dyn_range(uint16_t desired_gyro_dyn_range); - - /** - * Set sample rate (approximate) - 1kHz to 5Hz. - */ - bool set_sample_rate(uint16_t desired_sample_rate_hz); - - /** - * Start automatic measurement. - */ - void start(); - - PX4Accelerometer _px4_accel; - PX4Barometer _px4_baro; - PX4Gyroscope _px4_gyro; - PX4Magnetometer _px4_mag; - - uint16_t _product_ID{0}; // Product ID code. - - static constexpr float _sample_rate{ADIS16448_ACCEL_GYRO_UPDATE_RATE}; - - perf_counter_t _perf_read{perf_counter_t(perf_alloc(PC_ELAPSED, "ADIS16448: read"))}; - perf_counter_t _perf_transfer{perf_counter_t(perf_alloc(PC_ELAPSED, "ADIS16448: transfer"))}; - perf_counter_t _perf_bad_transfer{perf_counter_t(perf_alloc(PC_COUNT, "ADIS16448: bad transfers"))}; - perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, "ADIS16448: CRC16 bad"))}; - -}; diff --git a/src/drivers/imu/analog_devices/CMakeLists.txt b/src/drivers/imu/analog_devices/CMakeLists.txt new file mode 100644 index 0000000000..cef30903c9 --- /dev/null +++ b/src/drivers/imu/analog_devices/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_subdirectory(adis16448) diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp new file mode 100644 index 0000000000..0f88db4963 --- /dev/null +++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp @@ -0,0 +1,594 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "ADIS16448.hpp" + +using namespace time_literals; + +// computes the CCITT CRC16 on the data received from a burst read +static uint16_t ComputeCRC16(uint16_t burstData[13]) +{ + uint16_t crc = 0xFFFF; // Holds the CRC value + + unsigned int data; // Holds the lower/Upper byte for CRC computation + static constexpr unsigned int POLY = 0x1021; // Divisor used during CRC computation + + // Compute CRC on burst data starting from XGYRO_OUT and ending with TEMP_OUT. + // Start with the lower byte and then the upper byte of each word. + // i.e. Compute XGYRO_OUT_LSB CRC first and then compute XGYRO_OUT_MSB CRC. + for (int i = 1; i < 12; i++) { + unsigned int upperByte = (burstData[i] >> 8) & 0xFF; + unsigned int lowerByte = (burstData[i] & 0xFF); + data = lowerByte; // Compute lower byte CRC first + + for (int ii = 0; ii < 8; ii++, data >>= 1) { + if ((crc & 0x0001) ^ (data & 0x0001)) { + crc = (crc >> 1) ^ POLY; + + } else { + crc >>= 1; + } + } + + data = upperByte; // Compute upper byte of CRC + + for (int ii = 0; ii < 8; ii++, data >>= 1) { + if ((crc & 0x0001) ^ (data & 0x0001)) { + crc = (crc >> 1) ^ POLY; + + } else { + crc >>= 1; + } + } + } + + crc = ~crc; // Compute complement of CRC + data = crc; + crc = (crc << 8) | (data >> 8 & 0xFF); // Perform byte swap prior to returning CRC + + return crc; +} + +// convert 12 bit integer format to int16. +static int16_t convert12BitToINT16(uint16_t word) +{ + int16_t output = 0; + + if ((word >> 11) & 0x1) { + // sign extend + output = (word & 0xFFF) | 0xF000; + + } else { + output = (word & 0x0FFF); + } + + return output; +} + +ADIS16448::ADIS16448(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_drdy_gpio_t drdy_gpio) : + SPI(DRV_IMU_DEVTYPE_ADIS16448, MODULE_NAME, bus, device, SPIDEV_MODE3, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), + _drdy_gpio(drdy_gpio), // TODO: DRDY disabled + _px4_accel(get_device_id(), rotation), + _px4_baro(get_device_id()), + _px4_gyro(get_device_id(), rotation), + _px4_mag(get_device_id(), rotation) +{ +} + +ADIS16448::~ADIS16448() +{ + perf_free(_reset_perf); + perf_free(_perf_crc_bad); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); +} + +int ADIS16448::init() +{ + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool ADIS16448::Reset() +{ + _state = STATE::RESET; + DataReadyInterruptDisable(); + ScheduleClear(); + ScheduleNow(); + return true; +} + +void ADIS16448::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void ADIS16448::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_reset_perf); + perf_print_counter(_perf_crc_bad); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); +} + +int ADIS16448::probe() +{ + // Power-On Start-Up Time 205 ms + if (hrt_absolute_time() < 205_ms) { + PX4_WARN("Power-On Start-Up Time is 205 ms"); + } + + const uint16_t PROD_ID = RegisterRead(Register::PROD_ID); + + if (PROD_ID != Product_identification) { + DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID); + return PX4_ERROR; + } + + const uint16_t SERIAL_NUM = RegisterRead(Register::SERIAL_NUM); + const uint16_t LOT_ID1 = RegisterRead(Register::LOT_ID1); + const uint16_t LOT_ID2 = RegisterRead(Register::LOT_ID2); + + PX4_INFO("Serial Number: 0x%02x, Lot ID1: 0x%02x ID2: 0x%02x", SERIAL_NUM, LOT_ID1, LOT_ID2); + + // Only enable CRC-16 for verified lots (HACK to support older ADIS16448AMLZ with no explicit detection) + if (LOT_ID1 == 0x1824) { + _check_crc = true; + } + + return PX4_OK; +} + +void ADIS16448::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + perf_count(_reset_perf); + // GLOB_CMD: software reset + RegisterWrite(Register::GLOB_CMD, GLOB_CMD_BIT::Software_reset); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(90_ms); // Reset Recovery Time 90 ms + break; + + case STATE::WAIT_FOR_RESET: + + if (_self_test_passed) { + if ((RegisterRead(Register::PROD_ID) == Product_identification)) { + // if reset succeeded then configure + _state = STATE::CONFIGURE; + ScheduleNow(); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + } else { + RegisterWrite(Register::MSC_CTRL, MSC_CTRL_BIT::Internal_self_test); + _state = STATE::SELF_TEST_CHECK; + ScheduleDelayed(45_ms); // Automatic Self-Test Time 45 ms + } + + break; + + case STATE::SELF_TEST_CHECK: { + const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT); + + if (DIAG_STAT & DIAG_STAT_BIT::Self_test_diagnostic_error_flag) { + PX4_ERR("self test failed"); + + // Magnetometer + if (DIAG_STAT & DIAG_STAT_BIT::Magnetometer_functional_test) { + // tolerate mag test failure (likely due to surrounding magnetic field) + PX4_ERR("Magnetometer functional test fail"); + } + + // Barometer + if (DIAG_STAT & DIAG_STAT_BIT::Barometer_functional_test) { + PX4_ERR("Barometer functional test test fail"); + } + + // Gyroscope + const bool gyro_x_fail = DIAG_STAT & DIAG_STAT_BIT::X_axis_gyroscope_self_test_failure; + const bool gyro_y_fail = DIAG_STAT & DIAG_STAT_BIT::Y_axis_gyroscope_self_test_failure; + const bool gyro_z_fail = DIAG_STAT & DIAG_STAT_BIT::Z_axis_gyroscope_self_test_failure; + + if (gyro_x_fail || gyro_y_fail || gyro_z_fail) { + PX4_ERR("gyroscope self-test failure"); + } + + // Accelerometer + const bool accel_x_fail = DIAG_STAT & DIAG_STAT_BIT::X_axis_accelerometer_self_test_failure; + const bool accel_y_fail = DIAG_STAT & DIAG_STAT_BIT::Y_axis_accelerometer_self_test_failure; + const bool accel_z_fail = DIAG_STAT & DIAG_STAT_BIT::Z_axis_accelerometer_self_test_failure; + + if (accel_x_fail || accel_y_fail || accel_z_fail) { + PX4_ERR("accelerometer self-test failure"); + } + + _self_test_passed = false; + _state = STATE::RESET; + ScheduleDelayed(1000_ms); + + } else { + PX4_DEBUG("self test passed"); + _self_test_passed = true; + _state = STATE::RESET; + ScheduleNow(); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading + _state = STATE::READ; + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + + // backup schedule as a watchdog timeout + ScheduleDelayed(100_ms); + + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(SAMPLE_INTERVAL_US, SAMPLE_INTERVAL_US); + } + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::READ: { + if (_data_ready_interrupt_enabled) { + // push backup schedule back + ScheduleDelayed(SAMPLE_INTERVAL_US * 2); + } + + bool success = false; + + struct BurstRead { + uint16_t cmd; + uint16_t DIAG_STAT; + int16_t XGYRO_OUT; + int16_t YGYRO_OUT; + int16_t ZGYRO_OUT; + int16_t XACCL_OUT; + int16_t YACCL_OUT; + int16_t ZACCL_OUT; + int16_t XMAGN_OUT; + int16_t YMAGN_OUT; + int16_t ZMAGN_OUT; + uint16_t BARO_OUT; + uint16_t TEMP_OUT; + uint16_t CRC16; + } buffer{}; + + // ADIS16448 burst report should be 224 bits + static_assert(sizeof(BurstRead) == (224 / 8), "ADIS16448 report not 224 bits"); + + buffer.cmd = static_cast(Register::GLOB_CMD) << 8; + set_frequency(SPI_SPEED_BURST); + + if (transferhword((uint16_t *)&buffer, (uint16_t *)&buffer, sizeof(buffer) / sizeof(uint16_t)) == PX4_OK) { + + bool publish_data = true; + + // checksum + if (_check_crc) { + if (buffer.CRC16 != ComputeCRC16((uint16_t *)&buffer.DIAG_STAT)) { + perf_count(_perf_crc_bad); + publish_data = false; + } + } + + if (buffer.DIAG_STAT == DIAG_STAT_BIT::SPI_communication_failure) { + perf_count(_bad_transfer_perf); + publish_data = false; + } + + if (publish_data) { + + const uint32_t error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf); + _px4_accel.set_error_count(error_count); + _px4_gyro.set_error_count(error_count); + + // temperature 0.07386°C/LSB, 31°C = 0x000 + const float temperature = (convert12BitToINT16(buffer.TEMP_OUT) * 0.07386f) + 31.f; + + _px4_accel.set_temperature(temperature); + _px4_gyro.set_temperature(temperature); + + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + const int16_t accel_x = buffer.XACCL_OUT; + const int16_t accel_y = (buffer.YACCL_OUT == INT16_MIN) ? INT16_MAX : -buffer.YACCL_OUT; + const int16_t accel_z = (buffer.ZACCL_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZACCL_OUT; + + const int16_t gyro_x = buffer.XGYRO_OUT; + const int16_t gyro_y = (buffer.YGYRO_OUT == INT16_MIN) ? INT16_MAX : -buffer.YGYRO_OUT; + const int16_t gyro_z = (buffer.ZGYRO_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZGYRO_OUT; + + _px4_accel.update(now, accel_x, accel_y, accel_z); + _px4_gyro.update(now, gyro_x, gyro_y, gyro_z); + + // DIAG_STAT bit 7: New data, xMAGN_OUT/BARO_OUT + if (buffer.DIAG_STAT & DIAG_STAT_BIT::New_data_xMAGN_OUT_BARO_OUT) { + _px4_mag.set_error_count(error_count); + _px4_mag.set_temperature(temperature); + + const int16_t mag_x = buffer.XMAGN_OUT; + const int16_t mag_y = (buffer.YMAGN_OUT == INT16_MIN) ? INT16_MAX : -buffer.YMAGN_OUT; + const int16_t mag_z = (buffer.ZMAGN_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZMAGN_OUT; + _px4_mag.update(now, mag_x, mag_y, mag_z); + + _px4_baro.set_error_count(error_count); + _px4_baro.set_temperature(temperature); + + float pressure_pa = buffer.BARO_OUT * 0.02f; // 20 μbar per LSB + _px4_baro.update(now, pressure_pa); + } + + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + + } else { + perf_count(_bad_transfer_perf); + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 10) { + Reset(); + return; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_cfg[_checked_register])) { + _last_config_check_timestamp = now; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } + } + } + + break; + } +} + +bool ADIS16448::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + _px4_accel.set_scale(0.833f * 1e-3f * CONSTANTS_ONE_G); // 0.833 mg/LSB + _px4_gyro.set_scale(math::radians(0.04f)); // 0.04 °/sec/LSB + _px4_mag.set_scale(142.9f * 1e-6f); // μgauss/LSB + + _px4_accel.set_range(18.f * CONSTANTS_ONE_G); + _px4_gyro.set_range(math::radians(1000.f)); + + _px4_mag.set_external(external()); + + return success; +} + +int ADIS16448::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void ADIS16448::DataReady() +{ + ScheduleNow(); +} + +bool ADIS16448::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + return false; + } + + // check if DIO1 is connected to data ready + { + // DIO1 output set low + RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO1_DIRECTION); + bool write0_valid = (px4_arch_gpioread(_drdy_gpio) == 1); + + // DIO1 output set high + RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO1_DATA_LEVEL | GPIO_CTRL_BIT::GPIO1_DIRECTION); + bool write1_valid = (px4_arch_gpioread(_drdy_gpio) == 0); + + // DIO1 output set low again + RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO1_DIRECTION); + bool write2_valid = (px4_arch_gpioread(_drdy_gpio) == 1); + + if (write0_valid && write1_valid && write2_valid) { + PX4_INFO("DIO1 DRDY valid"); + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; + + } else { + PX4_DEBUG("DIO1 DRDY invalid"); + } + } + + // check if DIO2 is connected to data ready + { + // DIO2 output set low + RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO2_DIRECTION); + bool write0_valid = (px4_arch_gpioread(_drdy_gpio) == 1); + + // DIO2 output set high + RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO2_DATA_LEVEL | GPIO_CTRL_BIT::GPIO2_DIRECTION); + bool write1_valid = (px4_arch_gpioread(_drdy_gpio) == 0); + + // DIO2 output set low again + RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO2_DIRECTION); + bool write2_valid = (px4_arch_gpioread(_drdy_gpio) == 1); + + if (write0_valid && write1_valid && write2_valid) { + PX4_INFO("DIO2 DRDY valid"); + + } else { + PX4_DEBUG("DIO2 DRDY invalid"); + } + } + + return false; +} + +bool ADIS16448::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +bool ADIS16448::RegisterCheck(const register_config_t ®_cfg) +{ + bool success = true; + + const uint16_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +uint16_t ADIS16448::RegisterRead(Register reg) +{ + set_frequency(SPI_SPEED); + + uint16_t cmd[1]; + cmd[0] = (static_cast(reg) << 8); + + transferhword(cmd, nullptr, 1); + usleep(SPI_STALL_PERIOD); + transferhword(nullptr, cmd, 1); + + return cmd[0]; +} + +void ADIS16448::RegisterWrite(Register reg, uint16_t value) +{ + set_frequency(SPI_SPEED); + + uint16_t cmd[2]; + cmd[0] = (((static_cast(reg)) | DIR_WRITE) << 8) | ((0x00FF & value)); + cmd[1] = (((static_cast(reg) + 1) | DIR_WRITE) << 8) | ((0xFF00 & value) >> 8); + + transferhword(cmd, nullptr, 1); + usleep(SPI_STALL_PERIOD); + transferhword(cmd + 1, nullptr, 1); +} + +void ADIS16448::RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits) +{ + const uint16_t orig_val = RegisterRead(reg); + + uint16_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp new file mode 100644 index 0000000000..4987fef32b --- /dev/null +++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ADIS16448.hpp + * + * Driver for the Analog Devices ADIS16448 connected via SPI. + * + */ + +#pragma once + +#include "Analog_Devices_ADIS16448_registers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace Analog_Devices_ADIS16448; + +class ADIS16448 : public device::SPI, public I2CSPIDriver +{ +public: + ADIS16448(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_drdy_gpio_t drdy_gpio); + ~ADIS16448() override; + + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + void exit_and_cleanup() override; + + struct register_config_t { + Register reg; + uint16_t set_bits{0}; + uint16_t clear_bits{0}; + }; + + int probe() override; + + bool Reset(); + + bool Configure(); + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + bool RegisterCheck(const register_config_t ®_cfg); + + uint16_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint16_t value); + void RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits); + + const spi_drdy_gpio_t _drdy_gpio; + + PX4Accelerometer _px4_accel; + PX4Barometer _px4_baro; + PX4Gyroscope _px4_gyro; + PX4Magnetometer _px4_mag; + + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"))}; + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + int _failure_count{0}; + + bool _check_crc{false}; // CRC-16 not supported on earlier models (eg ADIS16448AMLZ) + bool _data_ready_interrupt_enabled{false}; + + bool _self_test_passed{false}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + SELF_TEST_CHECK, + CONFIGURE, + READ, + } _state{STATE::RESET}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{4}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::MSC_CTRL, MSC_CTRL_BIT::CRC16_for_burst, 0 }, + { Register::SMPL_PRD, SMPL_PRD_BIT::internal_sampling_clock, SMPL_PRD_BIT::decimation_rate }, + { Register::SENS_AVG, SENS_AVG_BIT::Measurement_range_1000_set, SENS_AVG_BIT::Measurement_range_1000_clear | SENS_AVG_BIT::Filter_Size_Variable_B }, + { Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO2_DIRECTION | GPIO_CTRL_BIT::GPIO1_DIRECTION, 0}, + }; +}; diff --git a/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp b/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp new file mode 100644 index 0000000000..a3d221d36c --- /dev/null +++ b/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp @@ -0,0 +1,161 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Analog_Devices_ADIS16448_registers.hpp + * + * Analog Devices ADIS16448 registers. + * + */ + +#pragma once + +#include + +// TODO: move to a central header +static constexpr uint16_t Bit0 = (1 << 0); +static constexpr uint16_t Bit1 = (1 << 1); +static constexpr uint16_t Bit2 = (1 << 2); +static constexpr uint16_t Bit3 = (1 << 3); +static constexpr uint16_t Bit4 = (1 << 4); +static constexpr uint16_t Bit5 = (1 << 5); +static constexpr uint16_t Bit6 = (1 << 6); +static constexpr uint16_t Bit7 = (1 << 7); +static constexpr uint16_t Bit8 = (1 << 8); +static constexpr uint16_t Bit9 = (1 << 9); +static constexpr uint16_t Bit10 = (1 << 10); +static constexpr uint16_t Bit11 = (1 << 11); +static constexpr uint16_t Bit12 = (1 << 12); +static constexpr uint16_t Bit13 = (1 << 13); +static constexpr uint16_t Bit14 = (1 << 14); +static constexpr uint16_t Bit15 = (1 << 15); + +namespace Analog_Devices_ADIS16448 +{ +static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2 MHz SPI serial interface +static constexpr uint32_t SPI_SPEED_BURST = 1 * 1000 * 1000; // 1 MHz SPI serial interface for burst read + +static constexpr uint32_t SPI_STALL_PERIOD = 9; // 9 us Stall period between data + +static constexpr uint16_t DIR_WRITE = 0x80; + +static constexpr uint16_t Product_identification = 0x4040; + +static constexpr uint32_t SAMPLE_INTERVAL_US = (1e6f / 819.2f); // ~819.2 Hz + + +enum class Register : uint16_t { + GPIO_CTRL = 0x32, // Auxiliary digital input/output control + + MSC_CTRL = 0x34, // Miscellaneous control + SMPL_PRD = 0x36, // Internal sample period (rate) control + SENS_AVG = 0x38, // Dynamic range and digital filter control + + DIAG_STAT = 0x3C, // System status + GLOB_CMD = 0x3E, // System command + + LOT_ID1 = 0x52, // Lot identification number + LOT_ID2 = 0x54, // Lot identification number + PROD_ID = 0x56, // Product identifier + SERIAL_NUM = 0x58, // Lot-specific serial number +}; + +// MSC_CTRL +enum MSC_CTRL_BIT : uint16_t { + Checksum_memory_test = Bit11, // Checksum memory test (cleared upon completion) + Internal_self_test = Bit10, // Internal self test (cleared upon completion) + // Not used = Bit5 + CRC16_for_burst = Bit4, // include the CRC-16 code in burst read output sequence + // Not used = Bit3 + Data_ready_enable = Bit2, + Data_ready_polarity = Bit1, // 1 = active high when data is valid + Data_ready_line_select = Bit0, // Data ready line select 1 = DIO2, 0 = DIO1 +}; + +// DIAG_STAT +enum DIAG_STAT_BIT : uint16_t { + Z_axis_accelerometer_self_test_failure = Bit15, // 1 = fail, 0 = pass + Y_axis_accelerometer_self_test_failure = Bit14, // 1 = fail, 0 = pass + X_axis_accelerometer_self_test_failure = Bit13, // 1 = fail, 0 = pass + Z_axis_gyroscope_self_test_failure = Bit12, // 1 = fail, 0 = pass + Y_axis_gyroscope_self_test_failure = Bit11, // 1 = fail, 0 = pass + X_axis_gyroscope_self_test_failure = Bit10, // 1 = fail, 0 = pass + + New_data_xMAGN_OUT_BARO_OUT = Bit7, // New data, xMAGN_OUT/BARO_OUT + Flash_test_checksum_flag = Bit6, // 1 = fail, 0 = pass + Self_test_diagnostic_error_flag = Bit5, // 1 = fail, 0 = pass + + SPI_communication_failure = Bit3, // 1 = fail, 0 = pass + + Barometer_functional_test = Bit1, // 1 = fail, 0 = pass + Magnetometer_functional_test = Bit0, // 1 = fail, 0 = pass +}; + +// GLOB_CMD +enum GLOB_CMD_BIT : uint16_t { + Software_reset = Bit7, +}; + +// SMPL_PRD +enum SMPL_PRD_BIT : uint16_t { + // [12:8] D, decimation rate setting, binomial, + decimation_rate = Bit12 | Bit11 | Bit10 | Bit9, // disable + + internal_sampling_clock = Bit0, // 1 = internal sampling clock, 819.2 SPS +}; + +// SENS_AVG +enum SENS_AVG_BIT : uint16_t { + // [10:8] Measurement range (sensitivity) selection + Measurement_range_1000_set = Bit10, // 100 = ±1000°/sec (default condition) + Measurement_range_1000_clear = Bit9 | Bit8, + + // [2:0] Filter Size Variable B + Filter_Size_Variable_B = Bit2 | Bit1 | Bit0, // disable + +}; + +// GPIO_CTRL +enum GPIO_CTRL_BIT : uint16_t { + GPIO4_DATA_LEVEL = Bit11, // General-Purpose I/O Line 1 (DIO1) data level + GPIO3_DATA_LEVEL = Bit10, // General-Purpose I/O Line 1 (DIO1) data level + GPIO2_DATA_LEVEL = Bit9, // General-Purpose I/O Line 1 (DIO1) data level + GPIO1_DATA_LEVEL = Bit8, // General-Purpose I/O Line 1 (DIO1) data level + + GPIO4_DIRECTION = Bit3, // General-Purpose I/O Line 4 (DIO4) direction control 1 = output, 0 = input + GPIO3_DIRECTION = Bit2, // General-Purpose I/O Line 3 (DIO3) direction control 1 = output, 0 = input + GPIO2_DIRECTION = Bit1, // General-Purpose I/O Line 2 (DIO2) direction control 1 = output, 0 = input + GPIO1_DIRECTION = Bit0, // General-Purpose I/O Line 1 (DIO1) direction control 1 = output, 0 = input +}; + +} // namespace Analog_Devices_ADIS16448 diff --git a/src/drivers/imu/adis16448/CMakeLists.txt b/src/drivers/imu/analog_devices/adis16448/CMakeLists.txt similarity index 89% rename from src/drivers/imu/adis16448/CMakeLists.txt rename to src/drivers/imu/analog_devices/adis16448/CMakeLists.txt index 4f4463fb03..ed488387ef 100644 --- a/src/drivers/imu/adis16448/CMakeLists.txt +++ b/src/drivers/imu/analog_devices/adis16448/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2021 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -30,18 +30,22 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + px4_add_module( - MODULE drivers__imu__adis16448 + MODULE drivers__imu__analog_devices__adis16448 MAIN adis16448 COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable + ${MAX_CUSTOM_OPT_LEVEL} + #-DDEBUG_BUILD SRCS ADIS16448.cpp + ADIS16448.hpp adis16448_main.cpp + Analog_Devices_ADIS16448_registers.hpp DEPENDS - px4_work_queue drivers_accelerometer drivers_barometer drivers_gyroscope drivers_magnetometer + px4_work_queue ) diff --git a/src/drivers/imu/adis16448/adis16448_main.cpp b/src/drivers/imu/analog_devices/adis16448/adis16448_main.cpp similarity index 91% rename from src/drivers/imu/adis16448/adis16448_main.cpp rename to src/drivers/imu/analog_devices/adis16448/adis16448_main.cpp index fbf3cdfc66..b04c9d6403 100644 --- a/src/drivers/imu/adis16448/adis16448_main.cpp +++ b/src/drivers/imu/analog_devices/adis16448/adis16448_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,13 +31,12 @@ * ****************************************************************************/ -#include "ADIS16448.h" +#include "ADIS16448.hpp" #include #include -void -ADIS16448::print_usage() +void ADIS16448::print_usage() { PRINT_MODULE_USAGE_NAME("adis16448", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("imu"); @@ -51,7 +50,7 @@ I2CSPIDriverBase *ADIS16448::instantiate(const BusCLIArguments &cli, const BusIn int runtime_instance) { ADIS16448 *instance = new ADIS16448(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode); + cli.bus_frequency, iterator.DRDYGPIO()); if (!instance) { PX4_ERR("alloc failed"); @@ -71,7 +70,7 @@ extern "C" int adis16448_main(int argc, char *argv[]) int ch; using ThisDriver = ADIS16448; BusCLIArguments cli{false, true}; - cli.default_spi_frequency = 1000000; + cli.default_spi_frequency = SPI_SPEED; while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { switch (ch) { @@ -92,13 +91,11 @@ extern "C" int adis16448_main(int argc, char *argv[]) if (!strcmp(verb, "start")) { return ThisDriver::module_start(cli, iterator); - } - if (!strcmp(verb, "stop")) { + } else if (!strcmp(verb, "stop")) { return ThisDriver::module_stop(iterator); - } - if (!strcmp(verb, "status")) { + } else if (!strcmp(verb, "status")) { return ThisDriver::module_status(iterator); }