Skip to content

Commit

Permalink
added debug code
Browse files Browse the repository at this point in the history
  • Loading branch information
@dakejahl
dakejahl committed Apr 30, 2024
1 parent 14aa738 commit de107a1
Show file tree
Hide file tree
Showing 3 changed files with 61 additions and 38 deletions.
30 changes: 16 additions & 14 deletions libraries/AP_HAL_ChibiOS/hwdef/ARKV6X/hwdef.dat
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand All @@ -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

Expand Down
59 changes: 40 additions & 19 deletions libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@
#include <AP_Math/AP_Math.h>

#include "AP_InertialSensor_SCH16T.h"
#include <GCS_MAVLink/GCS.h>
#include <hal.h>

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
Expand Down Expand Up @@ -85,12 +87,10 @@ extern const AP_HAL::HAL& hal;

AP_InertialSensor_SCH16T::AP_InertialSensor_SCH16T(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> _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);
Expand All @@ -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<AP_HAL::Device> 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;
Expand Down Expand Up @@ -151,37 +151,57 @@ 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);
break;
}

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
Expand All @@ -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();

Expand Down Expand Up @@ -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
Expand Down
10 changes: 5 additions & 5 deletions libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,8 +24,7 @@ class AP_InertialSensor_SCH16T : public AP_InertialSensor_Backend {
public:
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation,
uint8_t drdy_gpio);
enum Rotation rotation);

/**
* Configure the sensors and start reading routine.
Expand All @@ -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<AP_HAL::Device> dev,
enum Rotation rotation,
uint8_t drdy_gpio);
enum Rotation rotation);

struct SensorData {
int32_t acc_x;
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit de107a1

Please sign in to comment.