From ad934b49116afd6e615742c108d0ebfa90db05b3 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 4 Apr 2021 21:18:16 -0400 Subject: [PATCH] icm20948 i2c passthrough driver for ak09916 magnetometer (Here+ GPS/compass) - include icm20948 on most boards by default - create more test variants for default boards near flash limit (cuav_nora_test, cuav_x7pro_test, holybro_durandal-v1_test) --- .ci/Jenkinsfile-compile | 3 + .ci/Jenkinsfile-hardware | 24 +- .github/workflows/compile_nuttx.yml | 3 + .vscode/cmake-variants.yaml | 9 +- ROMFS/px4fmu_common/init.d/rc.sensors | 6 +- boards/airmind/mindpx-v2/default.cmake | 1 + boards/av/x-v1/default.cmake | 1 + boards/beaglebone/blue/default.cmake | 1 + boards/cuav/nora/default.cmake | 5 +- boards/cuav/nora/test.cmake | 139 +++++++++ boards/cuav/x7pro/default.cmake | 5 +- boards/cuav/x7pro/test.cmake | 139 +++++++++ .../cubeorange/init/rc.board_sensors | 2 - .../cubeyellow/init/rc.board_sensors | 2 - boards/emlid/navio2/default.cmake | 1 + boards/holybro/durandal-v1/default.cmake | 31 +- boards/holybro/durandal-v1/test.cmake | 135 ++++++++ boards/holybro/pix32v5/default.cmake | 1 + boards/modalai/fc-v1/default.cmake | 1 + boards/mro/x21-777/default.cmake | 21 +- boards/mro/x21/default.cmake | 21 +- boards/nxp/fmuk66-e/default.cmake | 1 + boards/nxp/fmuk66-e/rtps.cmake | 1 + boards/nxp/fmuk66-e/socketcan.cmake | 1 + boards/nxp/fmuk66-v3/default.cmake | 1 + boards/nxp/fmuk66-v3/rtps.cmake | 1 + boards/nxp/fmuk66-v3/socketcan.cmake | 1 + boards/nxp/fmurt1062-v1/default.cmake | 1 + boards/px4/fmu-v3/default.cmake | 2 +- boards/px4/fmu-v4/cannode.cmake | 1 + boards/px4/fmu-v4/default.cmake | 1 + boards/px4/fmu-v4pro/default.cmake | 1 + boards/px4/fmu-v5/ctrlalloc.cmake | 1 + boards/px4/fmu-v5/debug.cmake | 1 + boards/px4/fmu-v5/default.cmake | 1 + boards/px4/fmu-v5/fixedwing.cmake | 1 + boards/px4/fmu-v5/multicopter.cmake | 1 + boards/px4/fmu-v5/optimized.cmake | 1 + boards/px4/fmu-v5/rover.cmake | 1 + boards/px4/fmu-v5/rtps.cmake | 1 + boards/px4/fmu-v5/stackcheck.cmake | 1 + boards/px4/fmu-v5/uavcanv0periph.cmake | 1 + boards/px4/fmu-v5/uavcanv1.cmake | 1 + boards/px4/fmu-v5x/base_phy_DP83848C.cmake | 21 +- boards/px4/fmu-v5x/default.cmake | 21 +- boards/px4/fmu-v6u/default.cmake | 1 + boards/px4/fmu-v6u/test.cmake | 1 + boards/px4/fmu-v6x/default.cmake | 1 + boards/px4/raspberrypi/default.cmake | 1 + .../imu/invensense/icm20948/CMakeLists.txt | 15 +- .../imu/invensense/icm20948/ICM20948.cpp | 10 +- .../imu/invensense/icm20948/ICM20948.hpp | 4 +- .../icm20948/ICM20948_I2C_Passthrough.cpp | 289 ++++++++++++++++++ .../icm20948/ICM20948_I2C_Passthrough.hpp | 119 ++++++++ .../InvenSense_ICM20948_registers.hpp | 17 +- .../icm20948_i2c_passthrough_main.cpp | 98 ++++++ .../invensense/mpu9250/mpu9250_i2c_main.cpp | 2 +- 57 files changed, 1076 insertions(+), 98 deletions(-) create mode 100644 boards/cuav/nora/test.cmake create mode 100644 boards/cuav/x7pro/test.cmake create mode 100644 boards/holybro/durandal-v1/test.cmake create mode 100644 src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp create mode 100644 src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.hpp create mode 100644 src/drivers/imu/invensense/icm20948/icm20948_i2c_passthrough_main.cpp diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index 8bcb8d40c1..ac4d184ff6 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -45,7 +45,9 @@ pipeline { "cuav_can-gps-v1_canbootloader", "cuav_can-gps-v1_default", "cuav_nora_default", + "cuav_nora_test", "cuav_x7pro_default", + "cuav_x7pro_test", "cubepilot_cubeorange_console", "cubepilot_cubeorange_default", "cubepilot_cubeyellow_console", @@ -53,6 +55,7 @@ pipeline { "holybro_can-gps-v1_canbootloader", "holybro_can-gps-v1_default", "holybro_durandal-v1_default", + "holybro_durandal-v1_test", "holybro_kakutef7_default", "holybro_pix32v5_default", "modalai_fc-v1_default", diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index e37c83d5a3..88bf4d4f81 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -84,9 +84,9 @@ pipeline { } } - stage("cuav_x7pro_default") { + stage("cuav_x7pro_test") { stages { - stage("build cuav_x7pro_default") { + stage("build cuav_x7pro_test") { agent { docker { image 'px4io/px4-dev-nuttx-focal:2020-09-14' @@ -99,10 +99,10 @@ pipeline { sh 'make distclean' sh 'ccache -s' sh 'git fetch --tags' - sh 'make cuav_x7pro_default' + sh 'make cuav_x7pro_test' sh 'make sizes' sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'cuav_x7pro_default' + stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'cuav_x7pro_test' } post { always { @@ -119,9 +119,9 @@ pipeline { steps { sh 'export' sh 'find /dev/serial' - unstash 'cuav_x7pro_default' + unstash 'cuav_x7pro_test' // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cuav_x7pro_default/cuav_x7pro_default.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' + sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cuav_x7pro_test/cuav_x7pro_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' } } stage("configure") { @@ -823,9 +823,9 @@ pipeline { } } - stage("holybro_durandal-v1_default") { + stage("holybro_durandal-v1_test") { stages { - stage("build holybro_durandal-v1_default") { + stage("build holybro_durandal-v1_test") { agent { docker { image 'px4io/px4-dev-nuttx-focal:2020-09-14' @@ -838,10 +838,10 @@ pipeline { sh 'make distclean' sh 'ccache -s' sh 'git fetch --tags' - sh 'make holybro_durandal-v1_default' + sh 'make holybro_durandal-v1_test' sh 'make sizes' sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'holybro_durandal-v1_default' + stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'holybro_durandal-v1_test' } post { always { @@ -858,9 +858,9 @@ pipeline { steps { sh 'export' sh 'find /dev/serial' - unstash 'holybro_durandal-v1_default' + unstash 'holybro_durandal-v1_test' // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/holybro_durandal-v1_default/holybro_durandal-v1_default.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' + sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/holybro_durandal-v1_test/holybro_durandal-v1_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' } } stage("configure") { diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml index f6f2482cac..6c815aed38 100644 --- a/.github/workflows/compile_nuttx.yml +++ b/.github/workflows/compile_nuttx.yml @@ -26,7 +26,9 @@ jobs: cuav_can-gps-v1_debug, cuav_can-gps-v1_default, cuav_nora_default, + cuav_nora_test, cuav_x7pro_default, + cuav_x7pro_test, cubepilot_cubeorange_console, cubepilot_cubeorange_default, cubepilot_cubeyellow_console, @@ -36,6 +38,7 @@ jobs: holybro_can-gps-v1_debug, holybro_can-gps-v1_default, holybro_durandal-v1_default, + holybro_durandal-v1_test, holybro_kakutef7_default, holybro_pix32v5_default, modalai_fc-v1_default, diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index ebb9850906..24f76bbd67 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -96,16 +96,11 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: cuav_x7pro_default - cubepilot_cubeorange_default: + cubepilot_cubeorange_console: short: cubepilot_cubeorange buildType: MinSizeRel settings: - CONFIG: cubepilot_orange_default - cubepilot_cubeyellow_default: - short: cubepilot_cubeyellow - buildType: MinSizeRel - settings: - CONFIG: cubepilot_cubeyellow_default + CONFIG: cubepilot_orange_console emlid_navio2_default: short: emlid_navio2 buildType: MinSizeRel diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 8247025f8d..5c1838c1eb 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -117,8 +117,9 @@ fi # probe for optional external I2C devices if param compare SENS_EXT_I2C_PRB 1 then + icm20948_i2c_passthrough -X -q start + # compasses - ak09916 -X -R 6 -q start # external AK09916 (Here2) is rotated 270 degrees yaw hmc5883 -T -X -q start ist8308 -X -q start ist8310 -X -q start @@ -127,6 +128,9 @@ then qmc5883l -X -q start rm3100 -X -q start + # start last (wait for possible icm20948 passthrough mode) + ak09916 -X -q start + # differential pressure sensors if [ ${VEHICLE_TYPE} = fw -o ${VEHICLE_TYPE} = vtol ] then diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 5ef2da1abb..9bd56346b3 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -27,6 +27,7 @@ px4_add_board( gps #heater #imu # all available imu drivers + imu/invensense/icm20948 # required for ak09916 mag imu/l3gd20 imu/lsm303d imu/invensense/mpu6000 diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index b417292274..ca914f0053 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -30,6 +30,7 @@ px4_add_board( #imu # all available imu drivers imu/adis16477 imu/adis16497 + imu/invensense/icm20948 # required for ak09916 mag irlock lights # all available light drivers magnetometer # all available magnetometer drivers diff --git a/boards/beaglebone/blue/default.cmake b/boards/beaglebone/blue/default.cmake index 7343cb3e2f..70f22a1fc8 100644 --- a/boards/beaglebone/blue/default.cmake +++ b/boards/beaglebone/blue/default.cmake @@ -26,6 +26,7 @@ px4_add_board( gps #imu # all available imu drivers imu/analog_devices/adis16448 + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/mpu9250 linux_pwm_out #magnetometer # all available magnetometer drivers diff --git a/boards/cuav/nora/default.cmake b/boards/cuav/nora/default.cmake index f0b9361a16..108685fbd4 100644 --- a/boards/cuav/nora/default.cmake +++ b/boards/cuav/nora/default.cmake @@ -8,7 +8,6 @@ px4_add_board( ARCHITECTURE cortex-m7 ROMFSROOT px4fmu_common BUILD_BOOTLOADER - TESTING UAVCAN_INTERFACES 2 UAVCAN_TIMER_OVERRIDE 2 SERIAL_PORTS @@ -35,6 +34,7 @@ px4_add_board( imu/bosch/bmi088 imu/invensense/icm20649 imu/invensense/icm20689 + #imu/invensense/icm20948 # required for ak09916 mag irlock lights # all available light drivers lights/rgbled_pwm @@ -52,7 +52,6 @@ px4_add_board( rpm safety_button telemetry # all available telemetry drivers - test_ppm tone_alarm uavcan MODULES @@ -114,7 +113,7 @@ px4_add_board( sd_bench serial_test system_time - tests # tests and test runner + #tests # tests and test runner top topic_listener tune_control diff --git a/boards/cuav/nora/test.cmake b/boards/cuav/nora/test.cmake new file mode 100644 index 0000000000..95088a09d2 --- /dev/null +++ b/boards/cuav/nora/test.cmake @@ -0,0 +1,139 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR cuav + MODEL nora + LABEL test + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m7 + ROMFSROOT px4fmu_common + BUILD_BOOTLOADER + TESTING + UAVCAN_INTERFACES 2 + UAVCAN_TIMER_OVERRIDE 2 + SERIAL_PORTS + GPS1:/dev/ttyS0 + TEL1:/dev/ttyS1 + GPS2:/dev/ttyS2 + TEL2:/dev/ttyS3 + # CONSOLE: /dev/ttyS4 + # RC: /dev/ttyS5 + DRIVERS + adc/ads1115 + adc/board_adc + barometer # all available barometer drivers + batt_smbus + camera_capture + camera_trigger + differential_pressure # all available differential pressure drivers + distance_sensor # all available distance sensor drivers + dshot + gps + heater + #imu # all available imu drivers + imu/analog_devices/adis16448 + imu/bosch/bmi088 + imu/invensense/icm20649 + imu/invensense/icm20689 + imu/invensense/icm20948 # required for ak09916 mag + #irlock + lights # all available light drivers + lights/rgbled_pwm + magnetometer # all available magnetometer drivers + optical_flow # all available optical flow drivers + osd + pca9685 + pca9685_pwm_out + power_monitor/ina226 + #protocol_splitter + pwm_out_sim + pwm_out + rc_input + roboclaw + rpm + safety_button + telemetry # all available telemetry drivers + test_ppm + tone_alarm + uavcan + MODULES + airspeed_selector + attitude_estimator_q + battery_status + camera_feedback + commander + dataman + ekf2 + esc_battery + events + flight_mode_manager + fw_att_control + fw_pos_control_l1 + gyro_calibration + gyro_fft + land_detector + landing_target_estimator + load_mon + local_position_estimator + logger + mavlink + mc_att_control + mc_hover_thrust_estimator + mc_pos_control + mc_rate_control + #micrortps_bridge + navigator + rc_update + rover_pos_control + sensors + sih + temperature_compensation + #uuv_att_control + #uuv_pos_control + vmount + vtol_att_control + SYSTEMCMDS + bl_update + dmesg + dumpfile + esc_calib + gpio + hardfault_log + i2cdetect + led_control + mft + mixer + motor_ramp + motor_test + mtd + nshterm + param + perf + pwm + reboot + reflect + sd_bench + serial_test + system_time + tests # tests and test runner + top + topic_listener + tune_control + uorb + usb_connected + ver + work_queue + EXAMPLES + fake_gps + #fake_gyro + #fake_magnetometer + #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + #hello + #hwtest # Hardware test + #matlab_csv_serial + #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + #rover_steering_control # Rover example app + #uuv_example_app + #work_item + ) diff --git a/boards/cuav/x7pro/default.cmake b/boards/cuav/x7pro/default.cmake index 81266db729..ab6527ca2f 100644 --- a/boards/cuav/x7pro/default.cmake +++ b/boards/cuav/x7pro/default.cmake @@ -8,7 +8,6 @@ px4_add_board( ARCHITECTURE cortex-m7 ROMFSROOT px4fmu_common BUILD_BOOTLOADER - TESTING UAVCAN_INTERFACES 2 UAVCAN_TIMER_OVERRIDE 2 SERIAL_PORTS @@ -35,6 +34,7 @@ px4_add_board( imu/bosch/bmi088 imu/invensense/icm20649 imu/invensense/icm20689 + imu/invensense/icm20948 # required for ak09916 mag irlock lights # all available light drivers lights/rgbled_pwm @@ -52,7 +52,6 @@ px4_add_board( rpm safety_button telemetry # all available telemetry drivers - test_ppm tone_alarm uavcan MODULES @@ -114,7 +113,7 @@ px4_add_board( sd_bench serial_test system_time - tests # tests and test runner + #tests # tests and test runner top topic_listener tune_control diff --git a/boards/cuav/x7pro/test.cmake b/boards/cuav/x7pro/test.cmake new file mode 100644 index 0000000000..d875dc8182 --- /dev/null +++ b/boards/cuav/x7pro/test.cmake @@ -0,0 +1,139 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR cuav + MODEL x7pro + LABEL test + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m7 + ROMFSROOT px4fmu_common + BUILD_BOOTLOADER + TESTING + UAVCAN_INTERFACES 2 + UAVCAN_TIMER_OVERRIDE 2 + SERIAL_PORTS + GPS1:/dev/ttyS0 + TEL1:/dev/ttyS1 + GPS2:/dev/ttyS2 + TEL2:/dev/ttyS3 + # CONSOLE: /dev/ttyS4 + # RC: /dev/ttyS5 + DRIVERS + adc/ads1115 + adc/board_adc + barometer # all available barometer drivers + batt_smbus + camera_capture + camera_trigger + differential_pressure # all available differential pressure drivers + distance_sensor # all available distance sensor drivers + dshot + gps + heater + #imu # all available imu drivers + imu/analog_devices/adis16470 + imu/bosch/bmi088 + imu/invensense/icm20649 + imu/invensense/icm20689 + imu/invensense/icm20948 # required for ak09916 mag + #irlock + lights # all available light drivers + lights/rgbled_pwm + magnetometer # all available magnetometer drivers + optical_flow # all available optical flow drivers + osd + pca9685 + pca9685_pwm_out + power_monitor/ina226 + #protocol_splitter + pwm_out_sim + pwm_out + rc_input + roboclaw + rpm + safety_button + telemetry # all available telemetry drivers + test_ppm + tone_alarm + uavcan + MODULES + airspeed_selector + attitude_estimator_q + battery_status + camera_feedback + commander + dataman + ekf2 + esc_battery + events + flight_mode_manager + fw_att_control + fw_pos_control_l1 + gyro_calibration + gyro_fft + land_detector + landing_target_estimator + load_mon + local_position_estimator + logger + mavlink + mc_att_control + mc_hover_thrust_estimator + mc_pos_control + mc_rate_control + #micrortps_bridge + navigator + rc_update + rover_pos_control + sensors + sih + temperature_compensation + #uuv_att_control + #uuv_pos_control + vmount + vtol_att_control + SYSTEMCMDS + bl_update + dmesg + dumpfile + esc_calib + gpio + hardfault_log + i2cdetect + led_control + mft + mixer + motor_ramp + motor_test + mtd + nshterm + param + perf + pwm + reboot + reflect + sd_bench + serial_test + system_time + tests # tests and test runner + top + topic_listener + tune_control + uorb + usb_connected + ver + work_queue + EXAMPLES + fake_gps + #fake_gyro + #fake_magnetometer + #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + #hello + #hwtest # Hardware test + #matlab_csv_serial + #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + #rover_steering_control # Rover example app + #uuv_example_app + #work_item + ) diff --git a/boards/cubepilot/cubeorange/init/rc.board_sensors b/boards/cubepilot/cubeorange/init/rc.board_sensors index 8535383b4b..e84edf7e2f 100644 --- a/boards/cubepilot/cubeorange/init/rc.board_sensors +++ b/boards/cubepilot/cubeorange/init/rc.board_sensors @@ -13,5 +13,3 @@ icm20948 -s -b 4 -R 10 -M start ms5611 -s -b 1 start icm20649 -s -b 1 start -# standard Here/Here2 connected to GPS1 -ak09916 -X -b 1 -R 6 start # external AK09916 (Here2) is rotated 270 degrees yaw diff --git a/boards/cubepilot/cubeyellow/init/rc.board_sensors b/boards/cubepilot/cubeyellow/init/rc.board_sensors index 8535383b4b..e84edf7e2f 100644 --- a/boards/cubepilot/cubeyellow/init/rc.board_sensors +++ b/boards/cubepilot/cubeyellow/init/rc.board_sensors @@ -13,5 +13,3 @@ icm20948 -s -b 4 -R 10 -M start ms5611 -s -b 1 start icm20649 -s -b 1 start -# standard Here/Here2 connected to GPS1 -ak09916 -X -b 1 -R 6 start # external AK09916 (Here2) is rotated 270 degrees yaw diff --git a/boards/emlid/navio2/default.cmake b/boards/emlid/navio2/default.cmake index 6e401d6c64..b4fac09e05 100644 --- a/boards/emlid/navio2/default.cmake +++ b/boards/emlid/navio2/default.cmake @@ -24,6 +24,7 @@ px4_add_board( gps #imu # all available imu drivers imu/analog_devices/adis16448 + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/mpu9250 imu/st/lsm9ds1 linux_pwm_out diff --git a/boards/holybro/durandal-v1/default.cmake b/boards/holybro/durandal-v1/default.cmake index 8c12a1e8cf..f7c936675b 100644 --- a/boards/holybro/durandal-v1/default.cmake +++ b/boards/holybro/durandal-v1/default.cmake @@ -9,7 +9,6 @@ px4_add_board( ROMFSROOT px4fmu_common BUILD_BOOTLOADER IO px4_io-v2_default - TESTING UAVCAN_INTERFACES 2 SERIAL_PORTS GPS1:/dev/ttyS0 @@ -33,7 +32,8 @@ px4_add_board( imu/analog_devices/adis16448 imu/bosch/bmi088 imu/invensense/icm20689 - irlock + imu/invensense/icm20948 # required for ak09916 mag + #irlock lights # all available light drivers magnetometer # all available magnetometer drivers optical_flow # all available optical flow drivers @@ -48,7 +48,6 @@ px4_add_board( roboclaw rpm telemetry # all available telemetry drivers - test_ppm tone_alarm uavcan MODULES @@ -83,8 +82,8 @@ px4_add_board( sensors sih temperature_compensation - uuv_att_control - uuv_pos_control + #uuv_att_control + #uuv_pos_control vmount vtol_att_control SYSTEMCMDS @@ -110,7 +109,7 @@ px4_add_board( sd_bench serial_test system_time -# tests # tests and test runner + #tests # tests and test runner top topic_listener tune_control @@ -120,15 +119,15 @@ px4_add_board( work_queue EXAMPLES fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test + #fake_gyro + #fake_magnetometer + #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + #hello + #hwtest # Hardware test #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item + #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + #rover_steering_control # Rover example app + #uuv_example_app + #work_item ) diff --git a/boards/holybro/durandal-v1/test.cmake b/boards/holybro/durandal-v1/test.cmake new file mode 100644 index 0000000000..2837778cc2 --- /dev/null +++ b/boards/holybro/durandal-v1/test.cmake @@ -0,0 +1,135 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR holybro + MODEL durandal-v1 + LABEL test + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m7 + ROMFSROOT px4fmu_common + BUILD_BOOTLOADER + IO px4_io-v2_default + TESTING + UAVCAN_INTERFACES 2 + SERIAL_PORTS + GPS1:/dev/ttyS0 + TEL1:/dev/ttyS1 + TEL2:/dev/ttyS2 + TEL3:/dev/ttyS4 + TEL4:/dev/ttyS3 + DRIVERS + #adc/ads1115 + adc/board_adc + barometer # all available barometer drivers + batt_smbus + camera_capture + camera_trigger + differential_pressure # all available differential pressure drivers + distance_sensor # all available distance sensor drivers + dshot + gps + heater + #imu # all available imu drivers + #imu/analog_devices/adis16448 + imu/bosch/bmi088 + imu/invensense/icm20689 + imu/invensense/icm20948 # required for ak09916 mag + #irlock + lights # all available light drivers + magnetometer # all available magnetometer drivers + optical_flow # all available optical flow drivers + #osd + #pca9685 + #pca9685_pwm_out + power_monitor/ina226 + #protocol_splitter + pwm_out_sim + pwm_out + px4io + #roboclaw + #rpm + telemetry # all available telemetry drivers + test_ppm + tone_alarm + uavcan + MODULES + airspeed_selector + #attitude_estimator_q + battery_status + camera_feedback + commander + dataman + ekf2 + #esc_battery + events + flight_mode_manager + fw_att_control + fw_pos_control_l1 + gyro_calibration + gyro_fft + land_detector + landing_target_estimator + load_mon + #local_position_estimator + logger + mavlink + mc_att_control + mc_hover_thrust_estimator + mc_pos_control + mc_rate_control + #micrortps_bridge + navigator + rc_update + #rover_pos_control + sensors + #sih + #temperature_compensation + #uuv_att_control + #uuv_pos_control + vmount + vtol_att_control + SYSTEMCMDS + bl_update + dmesg + dumpfile + esc_calib + gpio + hardfault_log + i2cdetect + led_control + mft + mixer + motor_ramp + motor_test + mtd + nshterm + param + perf + pwm + reboot + #reflect + sd_bench + serial_test + system_time + tests # tests and test runner + top + topic_listener + tune_control + uorb + usb_connected + ver + work_queue + EXAMPLES + fake_gps + #fake_gyro + #fake_magnetometer + #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + #hello + #hwtest # Hardware test + #matlab_csv_serial + #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + #rover_steering_control # Rover example app + #uuv_example_app + #work_item + ) diff --git a/boards/holybro/pix32v5/default.cmake b/boards/holybro/pix32v5/default.cmake index 233f7d620e..1d5eaba7ef 100644 --- a/boards/holybro/pix32v5/default.cmake +++ b/boards/holybro/pix32v5/default.cmake @@ -33,6 +33,7 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 + imu/invensense/icm20948 # required for ak09916 mag irlock lights # all available light drivers lights/rgbled_pwm diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index 11e27489be..dd78af9a97 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -27,6 +27,7 @@ px4_add_board( gps imu/bosch/bmi088 imu/invensense/icm20602 + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/icm42688p irlock lights # all available light drivers diff --git a/boards/mro/x21-777/default.cmake b/boards/mro/x21-777/default.cmake index 2916133564..9fb83a8f2b 100644 --- a/boards/mro/x21-777/default.cmake +++ b/boards/mro/x21-777/default.cmake @@ -32,6 +32,7 @@ px4_add_board( #imu # all available imu drivers imu/invensense/icm20602 imu/invensense/icm20608g + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/mpu9250 irlock lights # all available light drivers @@ -121,15 +122,15 @@ px4_add_board( work_queue EXAMPLES fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test + #fake_gyro + #fake_magnetometer + #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + #hello + #hwtest # Hardware test #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item + #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + #rover_steering_control # Rover example app + #uuv_example_app + #work_item ) diff --git a/boards/mro/x21/default.cmake b/boards/mro/x21/default.cmake index 86b3b23322..44cf8e09db 100644 --- a/boards/mro/x21/default.cmake +++ b/boards/mro/x21/default.cmake @@ -33,6 +33,7 @@ px4_add_board( #imu # all available imu drivers imu/invensense/icm20602 imu/invensense/icm20608g + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/mpu9250 irlock lights # all available light drivers @@ -122,15 +123,15 @@ px4_add_board( work_queue EXAMPLES fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test + #fake_gyro + #fake_magnetometer + #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + #hello + #hwtest # Hardware test #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item + #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + #rover_steering_control # Rover example app + #uuv_example_app + #work_item ) diff --git a/boards/nxp/fmuk66-e/default.cmake b/boards/nxp/fmuk66-e/default.cmake index 2182ed226a..837647391c 100644 --- a/boards/nxp/fmuk66-e/default.cmake +++ b/boards/nxp/fmuk66-e/default.cmake @@ -28,6 +28,7 @@ px4_add_board( gps #imu # all available imu drivers imu/bosch/bmi088 + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/icm42688p irlock lights # all available light drivers diff --git a/boards/nxp/fmuk66-e/rtps.cmake b/boards/nxp/fmuk66-e/rtps.cmake index 06ddac7182..872368d675 100644 --- a/boards/nxp/fmuk66-e/rtps.cmake +++ b/boards/nxp/fmuk66-e/rtps.cmake @@ -28,6 +28,7 @@ px4_add_board( gps #imu # all available imu drivers imu/bosch/bmi088 + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/icm42688p irlock lights # all available light drivers diff --git a/boards/nxp/fmuk66-e/socketcan.cmake b/boards/nxp/fmuk66-e/socketcan.cmake index 7ede05dd02..ea9818cd3b 100644 --- a/boards/nxp/fmuk66-e/socketcan.cmake +++ b/boards/nxp/fmuk66-e/socketcan.cmake @@ -28,6 +28,7 @@ px4_add_board( gps #imu # all available imu drivers imu/bosch/bmi088 + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/icm42688p irlock lights # all available light drivers diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index 8ad8201f1b..8348971f9c 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -29,6 +29,7 @@ px4_add_board( #imu # all available imu drivers imu/fxas21002c imu/fxos8701cq + imu/invensense/icm20948 # required for ak09916 mag irlock lights # all available light drivers lights/rgbled_pwm diff --git a/boards/nxp/fmuk66-v3/rtps.cmake b/boards/nxp/fmuk66-v3/rtps.cmake index 3ca15b7956..252cd6c66e 100644 --- a/boards/nxp/fmuk66-v3/rtps.cmake +++ b/boards/nxp/fmuk66-v3/rtps.cmake @@ -29,6 +29,7 @@ px4_add_board( #imu # all available imu drivers imu/fxas21002c imu/fxos8701cq + imu/invensense/icm20948 # required for ak09916 mag irlock lights # all available light drivers lights/rgbled_pwm diff --git a/boards/nxp/fmuk66-v3/socketcan.cmake b/boards/nxp/fmuk66-v3/socketcan.cmake index f49551b4b1..b6ba521856 100644 --- a/boards/nxp/fmuk66-v3/socketcan.cmake +++ b/boards/nxp/fmuk66-v3/socketcan.cmake @@ -29,6 +29,7 @@ px4_add_board( #imu # all available imu drivers imu/fxas21002c imu/fxos8701cq + imu/invensense/icm20948 # required for ak09916 mag irlock lights # all available light drivers lights/rgbled_pwm diff --git a/boards/nxp/fmurt1062-v1/default.cmake b/boards/nxp/fmurt1062-v1/default.cmake index a2b8cdead9..c00e8e7d73 100644 --- a/boards/nxp/fmurt1062-v1/default.cmake +++ b/boards/nxp/fmurt1062-v1/default.cmake @@ -29,6 +29,7 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 + imu/invensense/icm20948 # required for ak09916 mag #irlock lights # all available light drivers lights/rgbled_pwm diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index 7a6c719a88..65ab6da962 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -33,7 +33,7 @@ px4_add_board( imu/l3gd20 imu/lsm303d imu/invensense/icm20608g - imu/invensense/icm20948 + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/mpu6000 imu/invensense/mpu9250 irlock diff --git a/boards/px4/fmu-v4/cannode.cmake b/boards/px4/fmu-v4/cannode.cmake index fb922141b6..f70382ddc6 100644 --- a/boards/px4/fmu-v4/cannode.cmake +++ b/boards/px4/fmu-v4/cannode.cmake @@ -47,6 +47,7 @@ px4_add_board( #imu/adis16497 imu/invensense/icm20602 imu/invensense/icm20608g + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/mpu9250 #lights/rgbled #lights/rgbled_ncp5623c diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 09fe3655bc..eb3dd1d53e 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -31,6 +31,7 @@ px4_add_board( imu/analog_devices/adis16448 imu/invensense/icm20602 imu/invensense/icm20608g + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/mpu9250 irlock lights # all available light drivers diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 8921963c77..151d218ef7 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -33,6 +33,7 @@ px4_add_board( imu/analog_devices/adis16448 imu/invensense/icm20602 imu/invensense/icm20608g + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/mpu9250 irlock lights # all available light drivers diff --git a/boards/px4/fmu-v5/ctrlalloc.cmake b/boards/px4/fmu-v5/ctrlalloc.cmake index 50380155ee..b470ab17c4 100644 --- a/boards/px4/fmu-v5/ctrlalloc.cmake +++ b/boards/px4/fmu-v5/ctrlalloc.cmake @@ -32,6 +32,7 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 + imu/invensense/icm20948 # required for ak09916 mag irlock lights # all available light drivers lights/rgbled_pwm diff --git a/boards/px4/fmu-v5/debug.cmake b/boards/px4/fmu-v5/debug.cmake index f6e8ebcd52..fec679b476 100644 --- a/boards/px4/fmu-v5/debug.cmake +++ b/boards/px4/fmu-v5/debug.cmake @@ -33,6 +33,7 @@ px4_add_board( #imu/bosch/bmi055 #imu/invensense/icm20602 imu/invensense/icm20689 + #imu/invensense/icm20948 # required for ak09916 mag #irlock lights # all available light drivers lights/rgbled_pwm diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index abc1cef9c6..7a99518dd2 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -32,6 +32,7 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 + imu/invensense/icm20948 # required for ak09916 mag irlock lights # all available light drivers lights/rgbled_pwm diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index c1f354fb5b..4ae506a289 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -29,6 +29,7 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 + imu/invensense/icm20948 # required for ak09916 mag lights # all available light drivers lights/rgbled_pwm magnetometer # all available magnetometer drivers diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index 64bbdf2b84..f6bf0a0af3 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -30,6 +30,7 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 + imu/invensense/icm20948 # required for ak09916 mag irlock lights # all available light drivers lights/rgbled_pwm diff --git a/boards/px4/fmu-v5/optimized.cmake b/boards/px4/fmu-v5/optimized.cmake index 554320c4c5..8d8e7919cf 100644 --- a/boards/px4/fmu-v5/optimized.cmake +++ b/boards/px4/fmu-v5/optimized.cmake @@ -35,6 +35,7 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 + imu/invensense/icm20948 # required for ak09916 mag irlock lights # all available light drivers #lights/rgbled_pwm diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index 78758ae351..9dde786ed0 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -26,6 +26,7 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 + imu/invensense/icm20948 # required for ak09916 mag lights/rgbled lights/rgbled_ncp5623c lights/rgbled_pwm diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index 280510bcb4..5632d4facc 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -31,6 +31,7 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 + imu/invensense/icm20948 # required for ak09916 mag irlock lights # all available light drivers lights/rgbled_pwm diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 49afc15565..6aa24b9fa8 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -35,6 +35,7 @@ px4_add_board( #imu/bosch/bmi055 #imu/invensense/icm20602 imu/invensense/icm20689 + #imu/invensense/icm20948 # required for ak09916 mag #irlock lights # all available light drivers lights/rgbled_pwm diff --git a/boards/px4/fmu-v5/uavcanv0periph.cmake b/boards/px4/fmu-v5/uavcanv0periph.cmake index f943c6a97c..02bf770063 100644 --- a/boards/px4/fmu-v5/uavcanv0periph.cmake +++ b/boards/px4/fmu-v5/uavcanv0periph.cmake @@ -36,6 +36,7 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 + #imu/invensense/icm20948 # required for ak09916 mag #irlock lights # all available light drivers lights/rgbled_pwm diff --git a/boards/px4/fmu-v5/uavcanv1.cmake b/boards/px4/fmu-v5/uavcanv1.cmake index 619fae5e90..670bce6b73 100644 --- a/boards/px4/fmu-v5/uavcanv1.cmake +++ b/boards/px4/fmu-v5/uavcanv1.cmake @@ -32,6 +32,7 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 + imu/invensense/icm20948 # required for ak09916 mag irlock lights # all available light drivers lights/rgbled_pwm diff --git a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake index c653a92969..cbe8b0060f 100644 --- a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake +++ b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake @@ -33,6 +33,7 @@ px4_add_board( imu/analog_devices/adis16448 imu/bosch/bmi088 imu/invensense/icm20602 + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/icm42688p irlock lights # all available light drivers @@ -124,15 +125,15 @@ px4_add_board( work_queue EXAMPLES fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test + #fake_gyro + #fake_magnetometer + #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + #hello + #hwtest # Hardware test #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item + #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + #rover_steering_control # Rover example app + #uuv_example_app + #work_item ) diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index 06a0e5349d..3b475f7722 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -33,6 +33,7 @@ px4_add_board( imu/analog_devices/adis16448 imu/bosch/bmi088 imu/invensense/icm20602 + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/icm42688p irlock lights # all available light drivers @@ -124,15 +125,15 @@ px4_add_board( work_queue EXAMPLES fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test + #fake_gyro + #fake_magnetometer + #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + #hello + #hwtest # Hardware test #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item + #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + #rover_steering_control # Rover example app + #uuv_example_app + #work_item ) diff --git a/boards/px4/fmu-v6u/default.cmake b/boards/px4/fmu-v6u/default.cmake index 9f5ddbba66..e32835bc0e 100644 --- a/boards/px4/fmu-v6u/default.cmake +++ b/boards/px4/fmu-v6u/default.cmake @@ -30,6 +30,7 @@ px4_add_board( #imu # all available imu drivers imu/bosch/bmi088 imu/invensense/icm20602 + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/icm42605 irlock lights # all available light drivers diff --git a/boards/px4/fmu-v6u/test.cmake b/boards/px4/fmu-v6u/test.cmake index 1b76b330ff..6ee6152ede 100644 --- a/boards/px4/fmu-v6u/test.cmake +++ b/boards/px4/fmu-v6u/test.cmake @@ -31,6 +31,7 @@ px4_add_board( #imu # all available imu drivers imu/bosch/bmi088 imu/invensense/icm20602 + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/icm42605 irlock lights # all available light drivers diff --git a/boards/px4/fmu-v6x/default.cmake b/boards/px4/fmu-v6x/default.cmake index 3fa8ac3295..9db0f5d10d 100644 --- a/boards/px4/fmu-v6x/default.cmake +++ b/boards/px4/fmu-v6x/default.cmake @@ -35,6 +35,7 @@ px4_add_board( imu/bosch/bmi088 imu/invensense/icm20602 imu/invensense/icm20649 + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/icm42688p irlock lights # all available light drivers diff --git a/boards/px4/raspberrypi/default.cmake b/boards/px4/raspberrypi/default.cmake index b029484c34..cadd33913c 100644 --- a/boards/px4/raspberrypi/default.cmake +++ b/boards/px4/raspberrypi/default.cmake @@ -22,6 +22,7 @@ px4_add_board( distance_sensor # all available distance sensor drivers gps #imu # all available imu drivers + imu/invensense/icm20948 # required for ak09916 mag imu/invensense/mpu9250 #magnetometer # all available magnetometer drivers magnetometer/hmc5883 diff --git a/src/drivers/imu/invensense/icm20948/CMakeLists.txt b/src/drivers/imu/invensense/icm20948/CMakeLists.txt index 3d4965bed6..b9ea57f5c0 100644 --- a/src/drivers/imu/invensense/icm20948/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm20948/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ px4_add_module( - MODULE drivers__imu__icm20948 + MODULE drivers__imu__invensense__icm20948 MAIN icm20948 COMPILE_FLAGS SRCS @@ -48,3 +48,16 @@ px4_add_module( drivers_gyroscope drivers_magnetometer ) + +px4_add_module( + MODULE drivers__imu__invensense__icm20948_i2c_passthrough + MAIN icm20948_i2c_passthrough + COMPILE_FLAGS + SRCS + ICM20948_I2C_Passthrough.cpp + ICM20948_I2C_Passthrough.hpp + icm20948_i2c_passthrough_main.cpp + InvenSense_ICM20948_registers.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.cpp b/src/drivers/imu/invensense/icm20948/ICM20948.cpp index 3bf358f686..ff91703d82 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.cpp @@ -182,8 +182,8 @@ void ICM20948::RunImpl() ScheduleDelayed(100_ms); } else { - PX4_DEBUG("Reset not complete, check again in 10 ms"); - ScheduleDelayed(10_ms); + PX4_DEBUG("Reset not complete, check again in 100 ms"); + ScheduleDelayed(100_ms); } } @@ -533,10 +533,11 @@ void ICM20948::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits uint16_t ICM20948::FIFOReadCount() { + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); + // read FIFO count uint8_t fifo_count_buf[3] {}; fifo_count_buf[0] = static_cast(Register::BANK_0::FIFO_COUNTH) | DIR_READ; - SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { perf_count(_bad_transfer_perf); @@ -548,9 +549,10 @@ uint16_t ICM20948::FIFOReadCount() bool ICM20948::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) { + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); + FIFOTransferBuffer buffer{}; const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 3, FIFO::SIZE); - SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { perf_count(_bad_transfer_perf); diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.hpp b/src/drivers/imu/invensense/icm20948/ICM20948.hpp index 2dca68c671..2f9c93d6af 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.hpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.hpp @@ -181,9 +181,7 @@ private: WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp new file mode 100644 index 0000000000..a3267f39c3 --- /dev/null +++ b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp @@ -0,0 +1,289 @@ +/**************************************************************************** + * + * 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 "ICM20948_I2C_Passthrough.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +ICM20948_I2C_Passthrough::ICM20948_I2C_Passthrough(I2CSPIBusOption bus_option, int bus, int bus_frequency) : + I2C(DRV_IMU_DEVTYPE_ICM20948, MODULE_NAME, bus, I2C_ADDRESS_DEFAULT, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus) +{ +} + +ICM20948_I2C_Passthrough::~ICM20948_I2C_Passthrough() +{ + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); +} + +int ICM20948_I2C_Passthrough::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool ICM20948_I2C_Passthrough::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +void ICM20948_I2C_Passthrough::print_status() +{ + I2CSPIDriverBase::print_status(); + + PX4_INFO("temperature: %.1f degC", (double)_temperature); + + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); +} + +int ICM20948_I2C_Passthrough::probe() +{ + const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + + if (whoami != WHOAMI) { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + return PX4_ERROR; + } + + return PX4_OK; +} + +void ICM20948_I2C_Passthrough::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // PWR_MGMT_1: Device Reset + RegisterWrite(Register::BANK_0::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(1_ms); + break; + + case STATE::WAIT_FOR_RESET: + + // The reset value is 0x00 for all registers other than the registers below + if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI) && (RegisterRead(Register::BANK_0::PWR_MGMT_1) == 0x41)) { + // Wakeup and reset + RegisterWrite(Register::BANK_0::USER_CTRL, USER_CTRL_BIT::SRAM_RST | USER_CTRL_BIT::I2C_MST_RST); + + // if reset succeeded then configure + _state = STATE::CONFIGURE; + ScheduleDelayed(1_ms); + + } 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); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + _state = STATE::READ; + ScheduleOnInterval(500_ms); + + } 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(10_ms); + } + + break; + + case STATE::READ: { + if (hrt_elapsed_time(&_last_config_check_timestamp) > 1000_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])) { + _last_config_check_timestamp = now; + _checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } + + } else { + // periodically update temperature (~1 Hz) + if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + UpdateTemperature(); + _temperature_update_timestamp = now; + } + } + } + + break; + } +} + +void ICM20948_I2C_Passthrough::SelectRegisterBank(enum REG_BANK_SEL_BIT bank) +{ + if (bank != _last_register_bank) { + // select BANK_0 + uint8_t cmd_bank_sel[2]; + cmd_bank_sel[0] = static_cast(Register::BANK_0::REG_BANK_SEL); + cmd_bank_sel[1] = bank; + + transfer(cmd_bank_sel, 2, nullptr, 0); + + _last_register_bank = bank; + } +} + +bool ICM20948_I2C_Passthrough::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_bank0_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_bank0_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + return success; +} + +template +bool ICM20948_I2C_Passthrough::RegisterCheck(const T ®_cfg) +{ + bool success = true; + + const uint8_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; +} + +template +uint8_t ICM20948_I2C_Passthrough::RegisterRead(T reg) +{ + SelectRegisterBank(reg); + + uint8_t cmd = static_cast(reg); + uint8_t value = 0; + transfer(&cmd, 1, &value, 1); + return value; +} + +template +void ICM20948_I2C_Passthrough::RegisterWrite(T reg, uint8_t value) +{ + SelectRegisterBank(reg); + + uint8_t cmd[2]; + cmd[0] = static_cast(reg); + cmd[1] = value; + transfer(cmd, sizeof(cmd), nullptr, 0); +} + +template +void ICM20948_I2C_Passthrough::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} + +void ICM20948_I2C_Passthrough::UpdateTemperature() +{ + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); + + // read current temperature + uint8_t cmd = static_cast(Register::BANK_0::TEMP_OUT_H); + uint8_t temperature_buf[2] {}; + + if (transfer(&cmd, 1, temperature_buf, 2) != PX4_OK) { + perf_count(_bad_transfer_perf); + return; + } + + const int16_t TEMP_OUT = combine(temperature_buf[0], temperature_buf[1]); + const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET; + + if (PX4_ISFINITE(TEMP_degC)) { + _temperature = TEMP_degC; + } +} diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.hpp b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.hpp new file mode 100644 index 0000000000..c0755a4b7e --- /dev/null +++ b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.hpp @@ -0,0 +1,119 @@ +/**************************************************************************** + * + * 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 ICM20948_I2C_Passthrough.hpp + * + * Driver for the Invensense ICM20948 connected via SPI. + * + */ + +#pragma once + +#include "InvenSense_ICM20948_registers.hpp" + +#include +#include +#include +#include +#include + +using namespace InvenSense_ICM20948; + +class ICM20948_I2C_Passthrough : public device::I2C, public I2CSPIDriver +{ +public: + ICM20948_I2C_Passthrough(I2CSPIBusOption bus_option, int bus, int bus_frequency); + ~ICM20948_I2C_Passthrough() 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: + struct register_bank0_config_t { + Register::BANK_0 reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Reset(); + + bool Configure(); + + void SelectRegisterBank(enum REG_BANK_SEL_BIT bank); + void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } + + template bool RegisterCheck(const T ®_cfg); + + template uint8_t RegisterRead(T reg); + template void RegisterWrite(T reg, uint8_t value); + template void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits); + + void UpdateTemperature(); + + 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}; + hrt_abstime _temperature_update_timestamp{0}; + int _failure_count{0}; + + float _temperature{NAN}; + + enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + READ, + } _state{STATE::RESET}; + + uint8_t _checked_register_bank0{0}; + static constexpr uint8_t size_register_bank0_cfg{3}; + register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { + // Register | Set bits, Clear bits + { Register::BANK_0::USER_CTRL, 0, USER_CTRL_BIT::DMP_EN | USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS }, + { Register::BANK_0::PWR_MGMT_1, 0, PWR_MGMT_1_BIT::DEVICE_RESET | PWR_MGMT_1_BIT::SLEEP }, + { Register::BANK_0::INT_PIN_CFG, INT_PIN_CFG_BIT::BYPASS_EN, 0 }, + }; +}; diff --git a/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp b/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp index ae0c81cb6e..c4a26cce6c 100644 --- a/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp +++ b/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp @@ -54,6 +54,9 @@ static constexpr uint8_t Bit5 = (1 << 5); static constexpr uint8_t Bit6 = (1 << 6); static constexpr uint8_t Bit7 = (1 << 7); +static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0b1101001; + static constexpr uint32_t SPI_SPEED = 7 * 1000 * 1000; // 7 MHz SPI static constexpr uint8_t DIR_READ = 0x80; @@ -97,8 +100,11 @@ enum class BANK_0 : uint8_t { }; enum class BANK_2 : uint8_t { + GYRO_SMPLRT_DIV = 0x00, GYRO_CONFIG_1 = 0x01, + ACCEL_SMPLRT_DIV_2 = 0x11, + ACCEL_CONFIG = 0x14, REG_BANK_SEL = 0x7F, @@ -146,6 +152,7 @@ enum PWR_MGMT_1_BIT : uint8_t { // INT_PIN_CFG enum INT_PIN_CFG_BIT : uint8_t { INT1_ACTL = Bit7, + BYPASS_EN = Bit1, // When asserted, the I2C_MASTER interface pins (ES_CL and ES_DA) will go into ‘bypass mode’ when the I 2 C master interface is disabled. }; // INT_ENABLE_1 @@ -188,7 +195,10 @@ enum REG_BANK_SEL_BIT : uint8_t { //---------------- BANK2 Register bits // GYRO_CONFIG_1 enum GYRO_CONFIG_1_BIT : uint8_t { - // GYRO_FS_SEL[1:0] + // 5:3 GYRO_DLPFCFG[2:0] + GYRO_DLPFCFG = Bit5 | Bit4 | Bit3, // 7 - + + // 2:1 GYRO_FS_SEL[1:0] GYRO_FS_SEL_250_DPS = 0, // 0b00 = ±250 dps GYRO_FS_SEL_500_DPS = Bit1, // 0b01 = ±500 dps GYRO_FS_SEL_1000_DPS = Bit2, // 0b10 = ±1000 dps @@ -199,7 +209,10 @@ enum GYRO_CONFIG_1_BIT : uint8_t { // ACCEL_CONFIG enum ACCEL_CONFIG_BIT : uint8_t { - // ACCEL_FS_SEL[1:0] + // 5:3 ACCEL_DLPFCFG[2:0] + ACCEL_DLPFCFG = Bit5 | Bit4 | Bit3, // 7 - + + // 2:1 ACCEL_FS_SEL[1:0] ACCEL_FS_SEL_2G = 0, // 0b00: ±2g ACCEL_FS_SEL_4G = Bit1, // 0b01: ±4g ACCEL_FS_SEL_8G = Bit2, // 0b10: ±8g diff --git a/src/drivers/imu/invensense/icm20948/icm20948_i2c_passthrough_main.cpp b/src/drivers/imu/invensense/icm20948/icm20948_i2c_passthrough_main.cpp new file mode 100644 index 0000000000..d8f4ffdc07 --- /dev/null +++ b/src/drivers/imu/invensense/icm20948/icm20948_i2c_passthrough_main.cpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * 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 "ICM20948_I2C_Passthrough.hpp" + +#include +#include + +void ICM20948_I2C_Passthrough::print_usage() +{ + PRINT_MODULE_USAGE_NAME("icm20948_i2c_passthrough", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("imu"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x39); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +I2CSPIDriverBase *ICM20948_I2C_Passthrough::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) +{ + ICM20948_I2C_Passthrough *instance = new ICM20948_I2C_Passthrough(iterator.configuredBusOption(), iterator.bus(), + cli.bus_frequency); + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (OK != instance->init()) { + delete instance; + return nullptr; + } + + return instance; +} + +extern "C" int icm20948_i2c_passthrough_main(int argc, char *argv[]) +{ + using ThisDriver = ICM20948_I2C_Passthrough; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = I2C_SPEED; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM20948); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/imu/invensense/mpu9250/mpu9250_i2c_main.cpp b/src/drivers/imu/invensense/mpu9250/mpu9250_i2c_main.cpp index 4712376e0b..ddaec82dff 100644 --- a/src/drivers/imu/invensense/mpu9250/mpu9250_i2c_main.cpp +++ b/src/drivers/imu/invensense/mpu9250/mpu9250_i2c_main.cpp @@ -38,7 +38,7 @@ void MPU9250_I2C::print_usage() { - PRINT_MODULE_USAGE_NAME("mpu9520", "driver"); + PRINT_MODULE_USAGE_NAME("mpu9520_i2c", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("imu"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);