From e72ecdbefb2aaddc5ad7fd6c9171996edf7d0bcb Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Mon, 20 May 2024 12:38:19 -0600 Subject: [PATCH] drivers/imu: new Murata SCH16T IMU driver (#22914) --------- Co-authored-by: alexklimaj --- ROMFS/px4fmu_common/init.d/rc.sensors | 6 + boards/ark/fmu-v6x/default.px4board | 1 + src/drivers/drv_sensor.h | 2 + src/drivers/imu/Kconfig | 1 + src/drivers/imu/murata/Kconfig | 3 + src/drivers/imu/murata/sch16t/CMakeLists.txt | 47 ++ src/drivers/imu/murata/sch16t/Kconfig | 5 + .../murata/sch16t/Murata_SCH16T_registers.hpp | 115 ++++ src/drivers/imu/murata/sch16t/SCH16T.cpp | 516 ++++++++++++++++++ src/drivers/imu/murata/sch16t/SCH16T.hpp | 149 +++++ src/drivers/imu/murata/sch16t/parameters.c | 44 ++ src/drivers/imu/murata/sch16t/sch16t_main.cpp | 87 +++ 12 files changed, 976 insertions(+) create mode 100644 src/drivers/imu/murata/Kconfig create mode 100644 src/drivers/imu/murata/sch16t/CMakeLists.txt create mode 100644 src/drivers/imu/murata/sch16t/Kconfig create mode 100644 src/drivers/imu/murata/sch16t/Murata_SCH16T_registers.hpp create mode 100644 src/drivers/imu/murata/sch16t/SCH16T.cpp create mode 100644 src/drivers/imu/murata/sch16t/SCH16T.hpp create mode 100644 src/drivers/imu/murata/sch16t/parameters.c create mode 100644 src/drivers/imu/murata/sch16t/sch16t_main.cpp diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 490038c608..5410d28bee 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -138,6 +138,12 @@ then adis16507 -S start fi +# SCH16T spi external IMU +if param compare -s SENS_EN_SCH16T 1 +then + sch16t -S start +fi + # Eagle Tree airspeed sensor external I2C if param compare -s SENS_EN_ETSASPD 1 then diff --git a/boards/ark/fmu-v6x/default.px4board b/boards/ark/fmu-v6x/default.px4board index 1209adb9dc..168b17cdfa 100644 --- a/boards/ark/fmu-v6x/default.px4board +++ b/boards/ark/fmu-v6x/default.px4board @@ -20,6 +20,7 @@ CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16507=y +CONFIG_DRIVERS_IMU_MURATA_SCH16T=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 4b1d18e76b..604ab4946b 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -133,6 +133,8 @@ #define DRV_IMU_DEVTYPE_ADIS16477 0x59 #define DRV_IMU_DEVTYPE_ADIS16507 0x5A +#define DRV_IMU_DEVTYPE_SCH16T 0x5B + #define DRV_BARO_DEVTYPE_MPC2520 0x5F #define DRV_BARO_DEVTYPE_LPS22HB 0x60 diff --git a/src/drivers/imu/Kconfig b/src/drivers/imu/Kconfig index 3b084a5801..ee0978d39c 100644 --- a/src/drivers/imu/Kconfig +++ b/src/drivers/imu/Kconfig @@ -8,6 +8,7 @@ menu "IMU" select DRIVERS_IMU_ANALOG_DEVICES_ADIS16470 select DRIVERS_IMU_BOSCH_BMI055 select DRIVERS_IMU_BOSCH_BMI088 + select DRIVERS_IMU_MURATA_SCH16T select DRIVERS_IMU_NXP_FXAS21002C select DRIVERS_IMU_NXP_FXOS8701CQ select DRIVERS_IMU_INVENSENSE_ICM20602 diff --git a/src/drivers/imu/murata/Kconfig b/src/drivers/imu/murata/Kconfig new file mode 100644 index 0000000000..36d4534078 --- /dev/null +++ b/src/drivers/imu/murata/Kconfig @@ -0,0 +1,3 @@ +menu "Murata" +rsource "*/Kconfig" +endmenu #Murata diff --git a/src/drivers/imu/murata/sch16t/CMakeLists.txt b/src/drivers/imu/murata/sch16t/CMakeLists.txt new file mode 100644 index 0000000000..73b90a12fb --- /dev/null +++ b/src/drivers/imu/murata/sch16t/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2022 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. +# +############################################################################ + +px4_add_module( + MODULE drivers__imu__murata__sch16t + MAIN sch16t + COMPILE_FLAGS + SRCS + SCH16T.cpp + SCH16T.hpp + sch16t_main.cpp + Murata_SCH16T_registers.hpp + DEPENDS + drivers_accelerometer + drivers_gyroscope + px4_work_queue + ) diff --git a/src/drivers/imu/murata/sch16t/Kconfig b/src/drivers/imu/murata/sch16t/Kconfig new file mode 100644 index 0000000000..ef0e906edd --- /dev/null +++ b/src/drivers/imu/murata/sch16t/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_MURATA_SCH16T + bool "SCH16T" + default n + ---help--- + Enable support for murata SCH16T diff --git a/src/drivers/imu/murata/sch16t/Murata_SCH16T_registers.hpp b/src/drivers/imu/murata/sch16t/Murata_SCH16T_registers.hpp new file mode 100644 index 0000000000..07d84dcb36 --- /dev/null +++ b/src/drivers/imu/murata/sch16t/Murata_SCH16T_registers.hpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +#pragma once + +#include + +namespace Murata_SCH16T +{ +static constexpr uint32_t SPI_SPEED = 5 * 1000 * 1000; // 5 MHz SPI serial interface +static constexpr uint32_t SAMPLE_INTERVAL_US = 678; // 1500 Hz -- decimation factor 8, F_PRIM/16, 1.475 kHz +static constexpr uint16_t EOI = (1 << 1); // End of Initialization +static constexpr uint16_t EN_SENSOR = (1 << 0); // Enable RATE and ACC measurement +static constexpr uint16_t DRY_DRV_EN = (1 << 5); // Enables Data ready function +static constexpr uint16_t FILTER_BYPASS = (0b111); // Bypass filter +static constexpr uint16_t RATE_300DPS_1475HZ = 0b0'001'001'011'011'011; // Gyro XYZ range 300 deg/s @ 1475Hz +static constexpr uint16_t ACC12_8G_1475HZ = 0b0'001'001'011'011'011; // Acc XYZ range 8 G and 1475 update rate +static constexpr uint16_t ACC3_26G = (0b000 << 0); +static constexpr uint16_t SPI_SOFT_RESET = (0b1010); + +// Data registers +#define RATE_X1 0x01 // 20 bit +#define RATE_Y1 0x02 // 20 bit +#define RATE_Z1 0x03 // 20 bit +#define ACC_X1 0x04 // 20 bit +#define ACC_Y1 0x05 // 20 bit +#define ACC_Z1 0x06 // 20 bit +#define ACC_X3 0x07 // 20 bit +#define ACC_Y3 0x08 // 20 bit +#define ACC_Z3 0x09 // 20 bit +#define RATE_X2 0x0A // 20 bit +#define RATE_Y2 0x0B // 20 bit +#define RATE_Z2 0x0C // 20 bit +#define ACC_X2 0x0D // 20 bit +#define ACC_Y2 0x0E // 20 bit +#define ACC_Z2 0x0F // 20 bit +#define TEMP 0x10 // 16 bit +// Status registers +#define STAT_SUM 0x14 // 16 bit +#define STAT_SUM_SAT 0x15 // 16 bit +#define STAT_COM 0x16 // 16 bit +#define STAT_RATE_COM 0x17 // 16 bit +#define STAT_RATE_X 0x18 // 16 bit +#define STAT_RATE_Y 0x19 // 16 bit +#define STAT_RATE_Z 0x1A // 16 bit +#define STAT_ACC_X 0x1B // 16 bit +#define STAT_ACC_Y 0x1C // 16 bit +#define STAT_ACC_Z 0x1D // 16 bit +// Control registers +#define CTRL_FILT_RATE 0x25 // 9 bit +#define CTRL_FILT_ACC12 0x26 // 9 bit +#define CTRL_FILT_ACC3 0x27 // 9 bit +#define CTRL_RATE 0x28 // 15 bit +#define CTRL_ACC12 0x29 // 15 bit +#define CTRL_ACC3 0x2A // 3 bit +#define CTRL_USER_IF 0x33 // 16 bit +#define CTRL_ST 0x34 // 13 bit +#define CTRL_MODE 0x35 // 4 bit +#define CTRL_RESET 0x36 // 4 bit +// Misc registers +#define ASIC_ID 0x3B // 12 bit +#define COMP_ID 0x3C // 16 bit +#define SN_ID1 0x3D // 16 bit +#define SN_ID2 0x3E // 16 bit +#define SN_ID3 0x3F // 16 bit + +// STAT_SUM_SAT bits +#define STAT_SUM_SAT_RSVD (1 << 15) +#define STAT_SUM_SAT_RATE_X1 (1 << 14) +#define STAT_SUM_SAT_RATE_Y1 (1 << 13) +#define STAT_SUM_SAT_RATE_Z1 (1 << 12) +#define STAT_SUM_SAT_ACC_X1 (1 << 11) +#define STAT_SUM_SAT_ACC_Y1 (1 << 10) +#define STAT_SUM_SAT_ACC_Z1 (1 << 9) +#define STAT_SUM_SAT_ACC_X3 (1 << 8) +#define STAT_SUM_SAT_ACC_Y3 (1 << 7) +#define STAT_SUM_SAT_ACC_Z3 (1 << 6) +#define STAT_SUM_SAT_RATE_X2 (1 << 5) +#define STAT_SUM_SAT_RATE_Y2 (1 << 4) +#define STAT_SUM_SAT_RATE_Z2 (1 << 3) +#define STAT_SUM_SAT_ACC_X2 (1 << 2) +#define STAT_SUM_SAT_ACC_Y2 (1 << 1) +#define STAT_SUM_SAT_ACC_Z2 (1 << 0) + +} // namespace Murata_SCH16T diff --git a/src/drivers/imu/murata/sch16t/SCH16T.cpp b/src/drivers/imu/murata/sch16t/SCH16T.cpp new file mode 100644 index 0000000000..5a682bfe23 --- /dev/null +++ b/src/drivers/imu/murata/sch16t/SCH16T.cpp @@ -0,0 +1,516 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "SCH16T.hpp" + +using namespace time_literals; + +#define SPI48_DATA_INT32(a) (((int32_t)(((a) << 4) & 0xfffff000UL)) >> 12) +#define SPI48_DATA_UINT32(a) ((uint32_t)(((a) >> 8) & 0x000fffffUL)) +#define SPI48_DATA_UINT16(a) ((uint16_t)(((a) >> 8) & 0x0000ffffUL)) + +static constexpr uint32_t POWER_ON_TIME = 250_ms; + +SCH16T::SCH16T(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation), + _drdy_gpio(config.drdy_gpio) +{ + if (_drdy_gpio != 0) { + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); + } + +#if defined(SPI6_nRESET_EXTERNAL1) + _hardware_reset_available = true; +#endif +} + +SCH16T::~SCH16T() +{ + perf_free(_reset_perf); + perf_free(_bad_transfer_perf); + perf_free(_perf_crc_bad); + perf_free(_perf_frame_bad); + perf_free(_drdy_missed_perf); +} + +int SCH16T::init() +{ + px4_usleep(POWER_ON_TIME); + + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; + } + + Reset(); + + return PX4_OK; +} + +int SCH16T::probe() +{ + if (hrt_absolute_time() < POWER_ON_TIME) { + PX4_WARN("Required Power-On Start-Up Time %" PRIu32 " ms", POWER_ON_TIME); + } + + RegisterRead(COMP_ID); + uint16_t comp_id = SPI48_DATA_UINT16(RegisterRead(ASIC_ID)); + uint16_t asic_id = SPI48_DATA_UINT16(RegisterRead(ASIC_ID)); + + RegisterRead(SN_ID1); + uint16_t sn_id1 = SPI48_DATA_UINT16(RegisterRead(SN_ID2)); + uint16_t sn_id2 = SPI48_DATA_UINT16(RegisterRead(SN_ID3)); + uint16_t sn_id3 = SPI48_DATA_UINT16(RegisterRead(SN_ID3)); + + char serial_str[14]; + snprintf(serial_str, 14, "%05d%01X%04X", sn_id2, sn_id1 & 0x000F, sn_id3); + + PX4_INFO("Serial:\t %s", serial_str); + PX4_INFO("COMP_ID:\t 0x%0x", comp_id); + PX4_INFO("ASIC_ID:\t 0x%0x", asic_id); + + // SCH16T-K01 - ID hex = 0x0020 + // SCH1633-B13 - ID hex = 0x0017 + bool success = asic_id == 0x20 && comp_id == 0x17; + + return success ? PX4_OK : PX4_ERROR; +} + +void SCH16T::Reset() +{ + if (_drdy_gpio) { + DataReadyInterruptDisable(); + } + + ScheduleClear(); + + _state = STATE::RESET_INIT; + ScheduleNow(); +} + +void SCH16T::ResetSpi6(bool reset) +{ +#if defined(SPI6_RESET) + SPI6_RESET(reset); +#endif +} + +void SCH16T::exit_and_cleanup() +{ + if (_drdy_gpio) { + DataReadyInterruptDisable(); + } + + I2CSPIDriverBase::exit_and_cleanup(); +} + +void SCH16T::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_reset_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_perf_crc_bad); + perf_print_counter(_perf_frame_bad); + perf_print_counter(_drdy_missed_perf); +} + +void SCH16T::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET_INIT: { + perf_count(_reset_perf); + + _failure_count = 0; + + if (_hardware_reset_available) { + PX4_INFO("Resetting (hard)"); + ResetSpi6(true); + _state = STATE::RESET_HARD; + ScheduleDelayed(2_ms); + + } else { + PX4_INFO("Resetting (soft)"); + SoftwareReset(); + _state = STATE::CONFIGURE; + ScheduleDelayed(POWER_ON_TIME); + } + + break; + } + + case STATE::RESET_HARD: { + if (_hardware_reset_available) { + ResetSpi6(false); + } + + _state = STATE::CONFIGURE; + ScheduleDelayed(POWER_ON_TIME); + break; + } + + case STATE::CONFIGURE: { + Configure(); + + _state = STATE::LOCK_CONFIGURATION; + ScheduleDelayed(POWER_ON_TIME); + break; + } + + case STATE::LOCK_CONFIGURATION: { + ReadStatusRegisters(); // Read all status registers once + RegisterWrite(CTRL_MODE, (EOI | EN_SENSOR)); // Write EOI and EN_SENSOR + + _state = STATE::VALIDATE; + ScheduleDelayed(5_ms); + break; + } + + case STATE::VALIDATE: { + ReadStatusRegisters(); // Read all status registers twice + ReadStatusRegisters(); + + // Check that registers are configured properly and that the sensor status is OK + if (ValidateSensorStatus() && ValidateRegisterConfiguration()) { + _state = STATE::READ; + + if (_drdy_gpio) { + DataReadyInterruptConfigure(); + ScheduleDelayed(100_ms); // backup schedule as a watchdog timeout + + } else { + ScheduleOnInterval(SAMPLE_INTERVAL_US, SAMPLE_INTERVAL_US); + } + + } else { + _state = STATE::RESET_INIT; + ScheduleDelayed(100_ms); + } + + break; + } + + case STATE::READ: { + hrt_abstime timestamp_sample = now; + + if (_drdy_gpio) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < SAMPLE_INTERVAL_US) { + timestamp_sample = drdy_timestamp_sample; + + } else { + perf_count(_drdy_missed_perf); + } + + // push backup schedule back + ScheduleDelayed(SAMPLE_INTERVAL_US * 2); + } + + // Collect the data + SensorData data = {}; + + if (ReadData(&data)) { + _px4_accel.set_temperature(float(data.temp) / 100.f); // Temperature signal sensitivity is 100 LSB/°C + _px4_gyro.set_temperature(float(data.temp) / 100.f); + _px4_accel.update(timestamp_sample, data.acc_x, data.acc_y, data.acc_z); + _px4_gyro.update(timestamp_sample, data.gyro_x, data.gyro_y, data.gyro_z); + + if (_failure_count > 0) { + _failure_count--; + } + + } else { + perf_count(_bad_transfer_perf); + _failure_count++; + } + + // Reset if successive failures + if (_failure_count > 10) { + PX4_INFO("Failure count high, resetting"); + Reset(); + return; + } + + break; + } + + default: + break; + } // end switch/case +} + +bool SCH16T::ReadData(SensorData *data) +{ + uint64_t temp = 0; + uint64_t gyro_x = 0; + uint64_t gyro_y = 0; + uint64_t gyro_z = 0; + uint64_t acc_x = 0; + uint64_t acc_y = 0; + uint64_t acc_z = 0; + + // Data registers are 20bit 2s complement + RegisterRead(TEMP); + temp = RegisterRead(STAT_SUM_SAT); + _sensor_status.saturation = SPI48_DATA_UINT16(RegisterRead(RATE_X2)); + gyro_x = RegisterRead(RATE_Y2); + gyro_y = RegisterRead(RATE_Z2); + + // Check if ACC2 is saturated, if so, use ACC3 + if ((_sensor_status.saturation & STAT_SUM_SAT_ACC_X2) || (_sensor_status.saturation & STAT_SUM_SAT_ACC_Y2) + || (_sensor_status.saturation & STAT_SUM_SAT_ACC_Z2)) { + gyro_z = RegisterRead(ACC_X3); + acc_x = RegisterRead(ACC_Y3); + acc_y = RegisterRead(ACC_Z3); + acc_z = RegisterRead(TEMP); + _px4_accel.set_scale(1.f / 1600.f); + _px4_accel.set_range(260.f); + + } else { + gyro_z = RegisterRead(ACC_X2); + acc_x = RegisterRead(ACC_Y2); + acc_y = RegisterRead(ACC_Z2); + acc_z = RegisterRead(TEMP); + _px4_accel.set_scale(1.f / 3200.f); + _px4_accel.set_range(163.4f); + } + + static constexpr uint64_t MASK48_ERROR = 0x001E00000000UL; + uint64_t values[] = { gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z, temp }; + + for (auto v : values) { + // Check for frame errors + if (v & MASK48_ERROR) { + perf_count(_perf_frame_bad); + return false; + } + + // Validate the CRC + if (uint8_t(v & 0xff) != CalculateCRC8(v)) { + perf_count(_perf_crc_bad); + return false; + } + } + + // Data registers are 20bit 2s complement + data->acc_x = SPI48_DATA_INT32(acc_x); + data->acc_y = SPI48_DATA_INT32(acc_y); + data->acc_z = SPI48_DATA_INT32(acc_z); + data->gyro_x = SPI48_DATA_INT32(gyro_x); + data->gyro_y = SPI48_DATA_INT32(gyro_y); + data->gyro_z = SPI48_DATA_INT32(gyro_z); + // Temperature data is always 16 bits wide. Drop 4 LSBs as they are not used. + data->temp = SPI48_DATA_INT32(temp) >> 4; + + // Conver to PX4 coordinate system (FLU to FRD) + data->acc_x = data->acc_x; + data->acc_y = -data->acc_y; + data->acc_z = -data->acc_z; + data->gyro_x = data->gyro_x; + data->gyro_y = -data->gyro_y; + data->gyro_z = -data->gyro_z; + + return true; +} + +void SCH16T::Configure() +{ + for (auto &r : _registers) { + RegisterWrite(r.addr, r.value); + } + + RegisterWrite(CTRL_USER_IF, DRY_DRV_EN); // Enable data ready + RegisterWrite(CTRL_MODE, EN_SENSOR); // Enable the sensor + + // NOTE: we use ACC3 for the higher range. The DRDY frequency adjusts to whichever register bank is + // being sampled from (decimated vs interpolated outputs). RATE_XYZ2 is decimated and RATE_XYZ1 is interpolated. + _px4_gyro.set_range(math::radians(327.68f)); // +-/ 300°/sec calibrated range, 327.68°/sec electrical headroom (20bit) + _px4_gyro.set_scale(math::radians(1.f / 1600.f)); // scaling 1600 LSB/°/sec -> rad/s per LSB + + // ACC12 range is 163.4 m/s^2, 3200 LSB/(m/s^2), ACC3 range is 260 m/s^2, 1600 LSB/(m/s^2) + _px4_accel.set_range(163.4f); + _px4_accel.set_scale(1.f / 3200.f); +} + +bool SCH16T::ValidateRegisterConfiguration() +{ + bool success = true; + + for (auto &r : _registers) { + RegisterRead(r.addr); // double read, wasteful but makes the code cleaner, not high rate so doesn't matter anyway + auto value = SPI48_DATA_UINT16(RegisterRead(r.addr)); + + if (value != r.value) { + PX4_INFO("Register 0x%0x misconfigured: 0x%0x", r.addr, value); + success = false; + } + } + + return success; +} + +void SCH16T::ReadStatusRegisters() +{ + RegisterRead(STAT_SUM); + _sensor_status.summary = SPI48_DATA_UINT16(RegisterRead(STAT_SUM_SAT)); + _sensor_status.saturation = SPI48_DATA_UINT16(RegisterRead(STAT_COM)); + _sensor_status.common = SPI48_DATA_UINT16(RegisterRead(STAT_RATE_COM)); + _sensor_status.rate_common = SPI48_DATA_UINT16(RegisterRead(STAT_RATE_X)); + _sensor_status.rate_x = SPI48_DATA_UINT16(RegisterRead(STAT_RATE_Y)); + _sensor_status.rate_y = SPI48_DATA_UINT16(RegisterRead(STAT_RATE_Z)); + _sensor_status.rate_z = SPI48_DATA_UINT16(RegisterRead(STAT_ACC_X)); + _sensor_status.acc_x = SPI48_DATA_UINT16(RegisterRead(STAT_ACC_Y)); + _sensor_status.acc_y = SPI48_DATA_UINT16(RegisterRead(STAT_ACC_Z)); + _sensor_status.acc_z = SPI48_DATA_UINT16(RegisterRead(STAT_ACC_Z)); +} + +bool SCH16T::ValidateSensorStatus() +{ + auto &s = _sensor_status; + uint16_t values[] = { s.summary, s.saturation, s.common, s.rate_common, s.rate_x, s.rate_y, s.rate_z, s.acc_x, s.acc_y, s.acc_z }; + + for (auto v : values) { + if (v != 0xFFFF) { + PX4_INFO("Sensor status failed"); + return false; + } + } + + return true; +} + +void SCH16T::SoftwareReset() +{ + RegisterWrite(CTRL_RESET, SPI_SOFT_RESET); +} + +uint64_t SCH16T::RegisterRead(uint8_t addr) +{ + uint64_t frame = {}; + frame |= uint64_t(addr) << 38; // Target address offset + frame |= uint64_t(1) << 35; // FrameType: SPI48BF + frame |= uint64_t(CalculateCRC8(frame)); + + return TransferSpiFrame(frame); +} + +// Non-data registers are the only writable ones and are 16 bit or less +void SCH16T::RegisterWrite(uint8_t addr, uint16_t value) +{ + uint64_t frame = {}; + frame |= uint64_t(1) << 37; // Write bit + frame |= uint64_t(addr) << 38; // Target address offset + frame |= uint64_t(1) << 35; // FrameType: SPI48BF + frame |= uint64_t(value) << 8; + frame |= uint64_t(CalculateCRC8(frame)); + + // We don't care about the return frame on a write + (void)TransferSpiFrame(frame); +} + +// The SPI protocol (SafeSPI) is 48bit out-of-frame. This means read return frames will be received on the next transfer. +uint64_t SCH16T::TransferSpiFrame(uint64_t frame) +{ + set_frequency(SPI_SPEED); + + uint16_t buf[3]; + + for (int index = 0; index < 3; index++) { + buf[3 - index - 1] = (frame >> (index << 4)) & 0xFFFF; + } + + transferhword(buf, buf, 3); + +#if defined(DEBUG_BUILD) + PX4_INFO("TransferSpiFrame: 0x%llx", frame); + + PX4_INFO("RECEIVED"); + + for (auto r : buf) { + PX4_INFO("%u", r); + } + +#endif + + uint64_t value = {}; + + for (int index = 0; index < 3; index++) { + value |= (uint64_t)buf[index] << ((3 - index - 1) << 4); + } + + return value; +} + +int SCH16T::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void SCH16T::DataReady() +{ + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); +} + +bool SCH16T::DataReadyInterruptConfigure() +{ + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, true, false, false, &DataReadyInterruptCallback, this) == 0; +} + +bool SCH16T::DataReadyInterruptDisable() +{ + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +uint8_t SCH16T::CalculateCRC8(uint64_t frame) +{ + uint64_t data = frame & 0xFFFFFFFFFF00LL; + uint8_t crc = 0xFF; + + for (int i = 47; i >= 0; i--) { + uint8_t data_bit = data >> i & 0x01; + crc = crc & 0x80 ? (uint8_t)((crc << 1) ^ 0x2F) ^ data_bit : (uint8_t)(crc << 1) | data_bit; + } + + return crc; +} diff --git a/src/drivers/imu/murata/sch16t/SCH16T.hpp b/src/drivers/imu/murata/sch16t/SCH16T.hpp new file mode 100644 index 0000000000..6e390d70b7 --- /dev/null +++ b/src/drivers/imu/murata/sch16t/SCH16T.hpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +#pragma once + +#include "Murata_SCH16T_registers.hpp" + +#include +#include +#include + +using namespace Murata_SCH16T; + +class SCH16T : public device::SPI, public I2CSPIDriver +{ +public: + SCH16T(const I2CSPIDriverConfig &config); + ~SCH16T() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + struct SensorData { + int32_t acc_x; + int32_t acc_y; + int32_t acc_z; + int32_t gyro_x; + int32_t gyro_y; + int32_t gyro_z; + int32_t temp; + }; + + struct SensorStatus { + uint16_t summary; + uint16_t saturation; + uint16_t common; + uint16_t rate_common; + uint16_t rate_x; + uint16_t rate_y; + uint16_t rate_z; + uint16_t acc_x; + uint16_t acc_y; + uint16_t acc_z; + }; + + struct RegisterConfig { + RegisterConfig(uint16_t a, uint16_t v) + : addr(a) + , value(v) + {}; + uint8_t addr; + uint16_t value; + }; + + int probe() override; + void exit_and_cleanup() override; + + bool ValidateSensorStatus(); + bool ValidateRegisterConfiguration(); + void Reset(); + void ResetSpi6(bool reset); + uint8_t CalculateCRC8(uint64_t frame); + + bool ReadData(SensorData *data); + void ReadStatusRegisters(); + + void Configure(); + void SoftwareReset(); + + void RegisterWrite(uint8_t addr, uint16_t value); + uint64_t RegisterRead(uint8_t addr); + uint64_t TransferSpiFrame(uint64_t frame); + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); +private: + PX4Accelerometer _px4_accel; + PX4Gyroscope _px4_gyro; + + SensorStatus _sensor_status{}; + + int _failure_count{0}; + + px4::atomic _drdy_timestamp_sample{0}; + const spi_drdy_gpio_t _drdy_gpio; + bool _hardware_reset_available{false}; + + enum class STATE : uint8_t { + RESET_INIT, + RESET_HARD, + CONFIGURE, + LOCK_CONFIGURATION, + VALIDATE, + READ, + } _state{STATE::RESET_INIT}; + + RegisterConfig _registers[6] = { + RegisterConfig(CTRL_FILT_RATE, FILTER_BYPASS), // Bypass filter + RegisterConfig(CTRL_FILT_ACC12, FILTER_BYPASS), // Bypass filter + RegisterConfig(CTRL_FILT_ACC3, FILTER_BYPASS), // Bypass filter + RegisterConfig(CTRL_RATE, RATE_300DPS_1475HZ), // +/- 300 deg/s, 1600 LSB/(deg/s) -- default, Decimation 8, 1475Hz + RegisterConfig(CTRL_ACC12, ACC12_8G_1475HZ), // +/- 80 m/s^2, 3200 LSB/(m/s^2) -- default, Decimation 8, 1475Hz + RegisterConfig(CTRL_ACC3, ACC3_26G) // +/- 260 m/s^2, 1600 LSB/(m/s^2) -- default + }; + + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC8 bad"))}; + perf_counter_t _perf_frame_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": Frame bad"))}; + perf_counter_t _drdy_missed_perf{nullptr}; + +}; diff --git a/src/drivers/imu/murata/sch16t/parameters.c b/src/drivers/imu/murata/sch16t/parameters.c new file mode 100644 index 0000000000..205b2f020e --- /dev/null +++ b/src/drivers/imu/murata/sch16t/parameters.c @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +/** + * Murata SCH16T IMU (external SPI) + * + * @reboot_required true + * @min 0 + * @max 1 + * @group Sensors + * @value 0 Disabled + * @value 1 Enabled + */ +PARAM_DEFINE_INT32(SENS_EN_SCH16T, 0); diff --git a/src/drivers/imu/murata/sch16t/sch16t_main.cpp b/src/drivers/imu/murata/sch16t/sch16t_main.cpp new file mode 100644 index 0000000000..a5ae892c99 --- /dev/null +++ b/src/drivers/imu/murata/sch16t/sch16t_main.cpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "SCH16T.hpp" + +#include + +void SCH16T::print_usage() +{ + PRINT_MODULE_USAGE_NAME("sch16t", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("imu"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int sch16t_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = SCH16T; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = SPI_SPEED; + cli.spi_mode = SPIDEV_MODE0; + + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optArg()); + break; + } + } + + const char *verb = cli.optArg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_SCH16T); + + 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; +}