diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index cc6d2d713c82..453fde9b83e9 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -21,6 +21,23 @@ fi # Begin Optional drivers # ############################################################################### +# ADIS16448 spi external IMU +if param compare -s SENS_EN_ADIS164X 1 +then + if param compare -s SENS_OR_ADIS164X 0 + then + adis16448 -S start + fi + if param compare -s SENS_OR_ADIS164X 4 + then + adis16448 -S start -R 4 + fi + + # add a sleep to make sure the mag is the priority one + usleep 1000000 +fi + + if param compare -s SENS_EN_BATT 1 then batt_smbus start -X @@ -108,19 +125,6 @@ then vl53l1x start -X fi -# ADIS16448 spi external IMU -if param compare -s SENS_EN_ADIS164X 1 -then - if param compare -s SENS_OR_ADIS164X 0 - then - adis16448 -S start - fi - if param compare -s SENS_OR_ADIS164X 4 - then - adis16448 -S start -R 4 - fi -fi - # Hall effect sensors si7210 # Potentially remove the -k option if possible and improve the startup if possible if param greater CAL_AV_AOA_ID -1 @@ -213,5 +217,3 @@ fi ############################################################################### # End Optional drivers # ############################################################################### - -sensors start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 38af11804b81..baaa966b232b 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -332,6 +332,8 @@ else # # board sensors: rc.sensors # + . ${R}etc/init.d/rc.sensors + set BOARD_RC_SENSORS ${R}etc/init.d/rc.board_sensors if [ -f $BOARD_RC_SENSORS ] then @@ -340,7 +342,7 @@ else fi unset BOARD_RC_SENSORS - . ${R}etc/init.d/rc.sensors + sensors start if param compare -s BAT1_SOURCE 2 then diff --git a/boards/px4/fmu-v2/src/spi.cpp b/boards/px4/fmu-v2/src/spi.cpp index ce80e1cee487..e28958d3baba 100644 --- a/boards/px4/fmu-v2/src/spi.cpp +++ b/boards/px4/fmu-v2/src/spi.cpp @@ -159,8 +159,8 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}) }), initSPIBusExternal(SPI::Bus::SPI4, { - initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}), initSPIConfigExternal(SPI::CS{GPIO::PortE, GPIO::Pin4}), + initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}), }), }), diff --git a/boards/px4/fmu-v3/src/spi.cpp b/boards/px4/fmu-v3/src/spi.cpp index ce80e1cee487..e28958d3baba 100644 --- a/boards/px4/fmu-v3/src/spi.cpp +++ b/boards/px4/fmu-v3/src/spi.cpp @@ -159,8 +159,8 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}) }), initSPIBusExternal(SPI::Bus::SPI4, { - initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}), initSPIConfigExternal(SPI::CS{GPIO::PortE, GPIO::Pin4}), + initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}), }), }), diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp index 821a6c3dc709..00dc49e72380 100644 --- a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp +++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp @@ -105,6 +105,7 @@ ADIS16448::ADIS16448(I2CSPIBusOption bus_option, int bus, uint32_t device, enum _px4_gyro(get_device_id(), rotation), _px4_mag(get_device_id(), rotation) { + _debug_enabled = true; } ADIS16448::~ADIS16448() @@ -160,12 +161,12 @@ int ADIS16448::probe() } for (int attempt = 0; attempt < 3; attempt++) { - const uint16_t PROD_ID = RegisterRead(Register::PROD_ID); + const uint16_t PROD_ID = RegisterReadVerified(Register::PROD_ID); if (PROD_ID == Product_identification) { - const uint16_t SERIAL_NUM = RegisterRead(Register::SERIAL_NUM); - const uint16_t LOT_ID1 = RegisterRead(Register::LOT_ID1); - const uint16_t LOT_ID2 = RegisterRead(Register::LOT_ID2); + const uint16_t SERIAL_NUM = RegisterReadVerified(Register::SERIAL_NUM); + const uint16_t LOT_ID1 = RegisterReadVerified(Register::LOT_ID1); + const uint16_t LOT_ID2 = RegisterReadVerified(Register::LOT_ID2); PX4_INFO("Serial Number: 0x%02x, Lot ID1: 0x%02x ID2: 0x%02x", SERIAL_NUM, LOT_ID1, LOT_ID2); @@ -197,7 +198,7 @@ void ADIS16448::RunImpl() case STATE::WAIT_FOR_RESET: if (_self_test_passed) { - if ((RegisterRead(Register::PROD_ID) == Product_identification)) { + if ((RegisterReadVerified(Register::PROD_ID) == Product_identification)) { // if reset succeeded then configure _state = STATE::CONFIGURE; ScheduleNow(); @@ -224,7 +225,7 @@ void ADIS16448::RunImpl() break; case STATE::SELF_TEST_CHECK: { - const uint16_t MSC_CTRL = RegisterRead(Register::MSC_CTRL); + const uint16_t MSC_CTRL = RegisterReadVerified(Register::MSC_CTRL); if (MSC_CTRL & MSC_CTRL_BIT::Internal_self_test) { // self test not finished, check again @@ -244,7 +245,7 @@ void ADIS16448::RunImpl() bool test_passed = true; - const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT); + const uint16_t DIAG_STAT = RegisterReadVerified(Register::DIAG_STAT); if (DIAG_STAT & DIAG_STAT_BIT::Self_test_diagnostic_error_flag) { PX4_ERR("self test failed"); @@ -477,7 +478,7 @@ void ADIS16448::RunImpl() bool ADIS16448::Configure() { - const uint16_t LOT_ID1 = RegisterRead(Register::LOT_ID1); + const uint16_t LOT_ID1 = RegisterReadVerified(Register::LOT_ID1); // Only enable CRC-16 for verified lots (HACK to support older ADIS16448AMLZ with no explicit detection) if (LOT_ID1 == 0x1824) { @@ -603,7 +604,7 @@ bool ADIS16448::RegisterCheck(const register_config_t ®_cfg) { bool success = true; - const uint16_t reg_value = RegisterRead(reg_cfg.reg); + const uint16_t reg_value = RegisterReadVerified(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); @@ -628,10 +629,27 @@ uint16_t ADIS16448::RegisterRead(Register reg) transferhword(cmd, nullptr, 1); px4_udelay(SPI_STALL_PERIOD); transferhword(nullptr, cmd, 1); + px4_udelay(SPI_STALL_PERIOD); return cmd[0]; } +uint16_t ADIS16448::RegisterReadVerified(Register reg) +{ + // 3 attempts + for (int i = 0; i < 3; i++) { + uint16_t read1 = RegisterRead(reg); + uint16_t read2 = RegisterRead(reg); + + if (read1 == read2) { + return read1; + } + } + + // failed + return 0; +} + void ADIS16448::RegisterWrite(Register reg, uint16_t value) { set_frequency(SPI_SPEED); @@ -643,11 +661,12 @@ void ADIS16448::RegisterWrite(Register reg, uint16_t value) transferhword(cmd, nullptr, 1); px4_udelay(SPI_STALL_PERIOD); transferhword(cmd + 1, nullptr, 1); + px4_udelay(SPI_STALL_PERIOD); } void ADIS16448::RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits) { - const uint16_t orig_val = RegisterRead(reg); + const uint16_t orig_val = RegisterReadVerified(reg); uint16_t val = (orig_val & ~clearbits) | setbits; diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp index 6a86f5ae23c0..e3d4cbf0cb5b 100644 --- a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp +++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp @@ -94,6 +94,7 @@ class ADIS16448 : public device::SPI, public I2CSPIDriver bool RegisterCheck(const register_config_t ®_cfg); uint16_t RegisterRead(Register reg); + uint16_t RegisterReadVerified(Register reg); void RegisterWrite(Register reg, uint16_t value); void RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits); diff --git a/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp b/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp index b16059afbf49..d671cde77f8a 100644 --- a/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp +++ b/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp @@ -65,7 +65,7 @@ namespace Analog_Devices_ADIS16448 static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2 MHz SPI serial interface static constexpr uint32_t SPI_SPEED_BURST = 1 * 1000 * 1000; // 1 MHz SPI serial interface for burst read -static constexpr uint32_t SPI_STALL_PERIOD = 10; // 9 us Stall period between data +static constexpr uint32_t SPI_STALL_PERIOD = 11; // 9 us Stall period between data static constexpr uint16_t DIR_WRITE = 0x80; diff --git a/src/drivers/imu/analog_devices/adis16448/CMakeLists.txt b/src/drivers/imu/analog_devices/adis16448/CMakeLists.txt index ed488387efbb..b32976987eeb 100644 --- a/src/drivers/imu/analog_devices/adis16448/CMakeLists.txt +++ b/src/drivers/imu/analog_devices/adis16448/CMakeLists.txt @@ -36,7 +36,7 @@ px4_add_module( MAIN adis16448 COMPILE_FLAGS ${MAX_CUSTOM_OPT_LEVEL} - #-DDEBUG_BUILD + -DDEBUG_BUILD SRCS ADIS16448.cpp ADIS16448.hpp