From de107a1f45a31fc2cbffa7cf3488bcc8cabb9e5c Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 30 Apr 2024 14:02:56 -0800 Subject: [PATCH] added debug code --- .../AP_HAL_ChibiOS/hwdef/ARKV6X/hwdef.dat | 30 +++++----- .../AP_InertialSensor_SCH16T.cpp | 59 +++++++++++++------ .../AP_InertialSensor_SCH16T.h | 10 ++-- 3 files changed, 61 insertions(+), 38 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/ARKV6X/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/ARKV6X/hwdef.dat index 4e39d78f7f98b5..fee7f0a8494c3d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/ARKV6X/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/ARKV6X/hwdef.dat @@ -85,7 +85,7 @@ PD9 USART3_RX USART3 # USART6 is for IOMCU PC6 USART6_TX USART6 -PC7 USART6_RX USART6 +# PC7 USART6_RX USART6 # Uncomment this line for carriers boards with an IO MCU # IOMCU_UART USART6 @@ -150,14 +150,16 @@ PF11 SPI5_MOSI SPI5 PG7 FRAM_CS CS # SPI6 - external1 (disabled to save DMA channels) -#PB3 SPI6_SCK SPI6 -#PA6 SPI6_MISO SPI6 -#PG14 SPI6_MOSI SPI6 -#PI10 EXT1_CS CS -#PD11 DRDY_ADIS16507 INPUT GPIO(93) +PB3 SPI6_SCK SPI6 SPEED_VERYLOW +PA6 SPI6_MISO SPI6 SPEED_VERYLOW +PG14 SPI6_MOSI SPI6 SPEED_VERYLOW +PI10 EXT1_CS CS SPEED_VERYLOW +PD11 DRDY_ADIS16507 INPUT GPIO(93) +PF10 nSPI6_RESET_EXTERNAL1 OUTPUT HIGH GPIO(169) +define HAL_nSPI6_RESET_EXTERNAL1 169 # use GPIO(93) for data ready on ADIS16507 -#define ADIS_DRDY_PIN 93 +define ADIS_DRDY_PIN 93 # PWM output pins PI0 TIM5_CH4 TIM5 PWM(1) GPIO(50) @@ -284,7 +286,7 @@ PG0 HW_VER_REV_DRIVE OUTPUT LOW PF9 TIM14_CH1 TIM14 GPIO(77) ALARM # RC input -PI5 TIM8_CH1 TIM8 RCININT PULLDOWN LOW +PC7 TIM3_CH2 TIM3 RCININT PULLDOWN LOW # barometers BARO BMP388 I2C:0:0x76 @@ -298,18 +300,18 @@ COMPASS BMM150 I2C:0:0x10 false ROTATION_NONE # IMU devices for ARKV6X SPIDEV iim42652 SPI1 DEVID1 IMU1_CS MODE3 2*MHZ 8*MHZ SPIDEV icm42688 SPI2 DEVID1 IMU2_CS MODE3 2*MHZ 8*MHZ -SPIDEV icm42688_ext SPI3 DEVID1 IMU3_CS MODE3 2*MHZ 8*MHZ -#SPIDEV adis16507 SPI6 DEVID1 EXT1_CS MODE3 1*MHZ 2*MHZ -#SPIDEV sch16t SPI6 DEVID1 EXT1_CS MODE3 1*MHZ 2*MHZ +SPIDEV sch16t SPI6 DEVID1 EXT1_CS MODE0 1*MHZ 1*MHZ +# SPIDEV icm42688_ext SPI3 DEVID1 IMU3_CS MODE3 2*MHZ 8*MHZ SPIDEV ramtron SPI5 DEVID1 FRAM_CS MODE3 8*MHZ 8*MHZ -# ARKV6X 3 IMUs +# ARKV6X 3 IMUs (max 3 IMU) IMU Invensensev3 SPI:iim42652 ROTATION_ROLL_180_YAW_135 IMU Invensensev3 SPI:icm42688 ROTATION_YAW_45 -IMU Invensensev3 SPI:icm42688_ext ROTATION_ROLL_180_YAW_270 +IMU SCH16T SPI:sch16t ROTATION_NONE + +# IMU Invensensev3 SPI:icm42688_ext ROTATION_ROLL_180_YAW_270 #IMU ADIS1647x SPI:adis16507 ROTATION_NONE ADIS_DRDY_PIN -#IMU SCH16T SPI:sch16t ROTATION_NONE ADIS_DRDY_PIN define HAL_DEFAULT_INS_FAST_SAMPLE 7 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp index d56b77cec51b68..a073a911785b77 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp @@ -16,6 +16,8 @@ #include #include "AP_InertialSensor_SCH16T.h" +#include +#include static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2 MHz SPI serial interface static constexpr uint32_t SAMPLE_INTERVAL_US = 678; // 1500 Hz -- decimation factor 8, F_PRIM/16, 1.475 kHz @@ -85,12 +87,10 @@ extern const AP_HAL::HAL& hal; AP_InertialSensor_SCH16T::AP_InertialSensor_SCH16T(AP_InertialSensor &imu, AP_HAL::OwnPtr _dev, - enum Rotation _rotation, - uint8_t drdy_gpio) + enum Rotation _rotation) : AP_InertialSensor_Backend(imu) , dev(std::move(_dev)) , rotation(_rotation) - , drdy_pin(drdy_gpio) { expected_sample_rate_hz = 1475; accel_scale = radians(1.f / 1600.f); @@ -107,13 +107,13 @@ AP_InertialSensor_SCH16T::AP_InertialSensor_SCH16T(AP_InertialSensor &imu, AP_InertialSensor_Backend * AP_InertialSensor_SCH16T::probe(AP_InertialSensor &imu, AP_HAL::OwnPtr dev, - enum Rotation rotation, - uint8_t drdy_gpio) + enum Rotation rotation) { if (!dev) { return nullptr; } - auto sensor = new AP_InertialSensor_SCH16T(imu, std::move(dev), rotation, drdy_gpio); + + auto sensor = new AP_InertialSensor_SCH16T(imu, std::move(dev), rotation); if (!sensor) { return nullptr; @@ -151,30 +151,49 @@ bool AP_InertialSensor_SCH16T::init() dev->set_speed(AP_HAL::Device::SPEED_LOW); - if (!read_product_id()) { - hal.console->printf("reading product ID failed"); - return false; - } - - configure_registers(); + // if (!read_product_id()) { + // hal.console->printf("reading product ID failed"); + // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "reading product ID failed"); + // return false; + // } return true; } void AP_InertialSensor_SCH16T::run_state_machine() { + WITH_SEMAPHORE(dev->get_semaphore()); + switch (_state) { + case State::PowerOn: { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PowerOn"); + _state = State::Reset; + dev->adjust_periodic_callback(periodic_handle, POWER_ON_TIME); + break; + } + case State::Reset: { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Reset"); failure_count = 0; - hal.console->printf("Resetting (soft)"); - register_write(CTRL_RESET, SPI_SOFT_RESET); + // hal.console->printf("Resetting (soft)"); + // // register_write(CTRL_RESET, SPI_SOFT_RESET); + palClearLine(HAL_nSPI6_RESET_EXTERNAL1); + hal.scheduler->delay(2000); + palSetLine(HAL_nSPI6_RESET_EXTERNAL1); + _state = State::Configure; dev->adjust_periodic_callback(periodic_handle, POWER_ON_TIME); break; } - case State::Configure: { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Configure"); + if (!read_product_id()) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "reading product ID failed"); + dev->adjust_periodic_callback(periodic_handle, POWER_ON_TIME*10); + break; + } + configure_registers(); _state = State::LockConfiguration; dev->adjust_periodic_callback(periodic_handle, POWER_ON_TIME); @@ -182,6 +201,7 @@ void AP_InertialSensor_SCH16T::run_state_machine() } case State::LockConfiguration: { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "LockConfiguration"); hal.console->printf("locking configuration"); read_status_registers(); // Read all status registers once register_write(CTRL_MODE, (EOI | EN_SENSOR)); // Write EOI and EN_SENSOR @@ -192,6 +212,7 @@ void AP_InertialSensor_SCH16T::run_state_machine() } case State::Validate: { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Validate"); read_status_registers(); // Read all status registers twice read_status_registers(); @@ -315,11 +336,11 @@ bool AP_InertialSensor_SCH16T::read_product_id() uint16_t sn_id3 = SPI48_DATA_UINT16(register_read(SN_ID3)); char serial_str[14]; - hal.console->printf(serial_str, 14, "%05d%01X%04X", sn_id2, sn_id1 & 0x000F, sn_id3); - hal.console->printf("Serial:\t %s", serial_str); - hal.console->printf("COMP_ID:\t 0x%0x", comp_id); - hal.console->printf("ASIC_ID:\t 0x%0x", asic_id); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, serial_str, 14, "%05d%01X%04X", sn_id2, sn_id1 & 0x000F, sn_id3); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Serial:\t %s", serial_str); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "COMP_ID:\t 0x%0x", comp_id); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ASIC_ID:\t 0x%0x", asic_id); // SCH16T-K01 - ID hex = 0x0020 // SCH1633-B13 - ID hex = 0x0017 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h index 4a6d17bb5f8d46..1b1775526b5b8d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h @@ -24,8 +24,7 @@ class AP_InertialSensor_SCH16T : public AP_InertialSensor_Backend { public: static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu, AP_HAL::OwnPtr dev, - enum Rotation rotation, - uint8_t drdy_gpio); + enum Rotation rotation); /** * Configure the sensors and start reading routine. @@ -34,10 +33,10 @@ class AP_InertialSensor_SCH16T : public AP_InertialSensor_Backend { bool update() override; private: + AP_InertialSensor_SCH16T(AP_InertialSensor &imu, AP_HAL::OwnPtr dev, - enum Rotation rotation, - uint8_t drdy_gpio); + enum Rotation rotation); struct SensorData { int32_t acc_x; @@ -89,12 +88,13 @@ class AP_InertialSensor_SCH16T : public AP_InertialSensor_Backend { uint8_t calculate_crc8(uint64_t frame); enum class State : uint8_t { + PowerOn, Reset, Configure, LockConfiguration, Validate, Read, - } _state = State::Reset; + } _state = State::PowerOn; RegisterConfig _registers[6]; SensorStatus _sensor_status;