fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

drivers/imu: new Murata SCH16T IMU driver (#22914)

Browse Source 
---------

Co-authored-by: alexklimaj <alex@arkelectron.com>
This commit is contained in:
Jacob Dahl 2024-05-20 12:38:19 -06:00 committed by GitHub
parent 70304fe715
commit e72ecdbefb
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
12 changed files with 976 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ROMFS/px4fmu_common/init.d/rc.sensors
View File

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

1
boards/ark/fmu-v6x/default.px4board
View File

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

2
src/drivers/drv_sensor.h
View File

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

1
src/drivers/imu/Kconfig
View File

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

3
src/drivers/imu/murata/Kconfig Normal file
View File

@ -0,0 +1,3 @@
menu "Murata"
rsource "*/Kconfig"
endmenu #Murata

47
src/drivers/imu/murata/sch16t/CMakeLists.txt Normal file
View File

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

5
src/drivers/imu/murata/sch16t/Kconfig Normal file
View File

@ -0,0 +1,5 @@
menuconfig DRIVERS_IMU_MURATA_SCH16T
bool "SCH16T"
default n
---help---
Enable support for murata SCH16T

115
src/drivers/imu/murata/sch16t/Murata_SCH16T_registers.hpp Normal file
View File

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

516
src/drivers/imu/murata/sch16t/SCH16T.cpp Normal file
View File

@ -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<SCH16T *>(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;
}

149
src/drivers/imu/murata/sch16t/SCH16T.hpp Normal file
View File

@ -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 <px4_platform_common/i2c_spi_buses.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
using namespace Murata_SCH16T;
class SCH16T : public device::SPI, public I2CSPIDriver<SCH16T>
{
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<hrt_abstime> _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};
};

44
src/drivers/imu/murata/sch16t/parameters.c Normal file
View File

@ -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);

87
src/drivers/imu/murata/sch16t/sch16t_main.cpp Normal file
View File

@ -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 <px4_platform_common/module.h>
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;
}