mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-02 03:36:48 +08:00
feat(drivers/imu): add Analog Devices ADIS1657x IMU driver (#27239)
This commit is contained in:
Stolniceanu Iustin
committed by
GitHub
GitHub
parent
f3f2606d68
commit
a3d3c8bc20
@@ -137,6 +137,7 @@
|
|||||||
#define DRV_IMU_DEVTYPE_SCH16T 0x5B
|
#define DRV_IMU_DEVTYPE_SCH16T 0x5B
|
||||||
#define DRV_IMU_DEVTYPE_ADIS16607 0x5C
|
#define DRV_IMU_DEVTYPE_ADIS16607 0x5C
|
||||||
#define DRV_IMU_DEVTYPE_ADIS1650X 0x5D
|
#define DRV_IMU_DEVTYPE_ADIS1650X 0x5D
|
||||||
|
#define DRV_IMU_DEVTYPE_ADIS1657X 0x5E
|
||||||
|
|
||||||
#define DRV_BARO_DEVTYPE_MPC2520 0x5F
|
#define DRV_BARO_DEVTYPE_MPC2520 0x5F
|
||||||
#define DRV_BARO_DEVTYPE_LPS22HB 0x60
|
#define DRV_BARO_DEVTYPE_LPS22HB 0x60
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,146 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2026 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 "Analog_Devices_ADIS1657x_registers.hpp"
|
||||||
|
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <drivers/device/spi.h>
|
||||||
|
#include <lib/conversion/rotation.h>
|
||||||
|
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||||
|
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||||
|
#include <perf/perf_counter.h>
|
||||||
|
#include <px4_platform_common/atomic.h>
|
||||||
|
#include <px4_platform_common/i2c_spi_buses.h>
|
||||||
|
|
||||||
|
using namespace Analog_Devices_ADIS1657x;
|
||||||
|
|
||||||
|
// 32-bit burst frame: cmd(2) + 17 words(34) = 36 bytes
|
||||||
|
struct __attribute__((packed)) adis1657x_burst {
|
||||||
|
uint8_t cmd[2];
|
||||||
|
uint16_t diag_stat;
|
||||||
|
uint16_t x_gyro_lsb;
|
||||||
|
uint16_t x_gyro_msb;
|
||||||
|
uint16_t y_gyro_lsb;
|
||||||
|
uint16_t y_gyro_msb;
|
||||||
|
uint16_t z_gyro_lsb;
|
||||||
|
uint16_t z_gyro_msb;
|
||||||
|
uint16_t x_accel_lsb;
|
||||||
|
uint16_t x_accel_msb;
|
||||||
|
uint16_t y_accel_lsb;
|
||||||
|
uint16_t y_accel_msb;
|
||||||
|
uint16_t z_accel_lsb;
|
||||||
|
uint16_t z_accel_msb;
|
||||||
|
uint16_t temp;
|
||||||
|
uint16_t data_cntr;
|
||||||
|
uint16_t timestamp_upr;
|
||||||
|
uint16_t spi_chksum;
|
||||||
|
};
|
||||||
|
|
||||||
|
static_assert(sizeof(adis1657x_burst) == 36, "burst frame size mismatch");
|
||||||
|
|
||||||
|
class ADIS1657x : public device::SPI, public I2CSPIDriver<ADIS1657x>
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
ADIS1657x(const I2CSPIDriverConfig &config);
|
||||||
|
~ADIS1657x() override;
|
||||||
|
|
||||||
|
static void print_usage();
|
||||||
|
|
||||||
|
static int selected_dec_rate;
|
||||||
|
static int selected_filt_size;
|
||||||
|
|
||||||
|
int init() override;
|
||||||
|
void print_status() override;
|
||||||
|
void RunImpl();
|
||||||
|
|
||||||
|
protected:
|
||||||
|
int probe() override;
|
||||||
|
void exit_and_cleanup() override;
|
||||||
|
|
||||||
|
private:
|
||||||
|
PX4Accelerometer _px4_accel;
|
||||||
|
PX4Gyroscope _px4_gyro;
|
||||||
|
|
||||||
|
perf_counter_t _sample_perf;
|
||||||
|
perf_counter_t _bad_transfers;
|
||||||
|
perf_counter_t _reset_perf;
|
||||||
|
perf_counter_t _drdy_missed_perf;
|
||||||
|
|
||||||
|
const spi_drdy_gpio_t _drdy_gpio;
|
||||||
|
|
||||||
|
int _dec_rate;
|
||||||
|
int _filt_size;
|
||||||
|
|
||||||
|
float _gyro_scale{0.0f};
|
||||||
|
float _accel_scale{0.0f};
|
||||||
|
float _sample_rate_hz{1000.0f};
|
||||||
|
uint16_t _prod_id{0};
|
||||||
|
uint16_t _last_data_cntr{0};
|
||||||
|
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
|
||||||
|
int32_t _drdy_count{0};
|
||||||
|
bool _data_ready_interrupt_enabled{false};
|
||||||
|
|
||||||
|
int _failure_count{0};
|
||||||
|
int _configure_retries{0};
|
||||||
|
|
||||||
|
enum class STATE : uint8_t {
|
||||||
|
RESET,
|
||||||
|
WAIT_FOR_RESET,
|
||||||
|
CONFIGURE,
|
||||||
|
READ,
|
||||||
|
} _state{STATE::RESET};
|
||||||
|
|
||||||
|
// SPI register access
|
||||||
|
uint16_t read_reg16(Register reg);
|
||||||
|
bool write_reg16(Register reg, uint16_t val);
|
||||||
|
bool write_reg16_verified(Register reg, uint16_t val, int retries = 3);
|
||||||
|
|
||||||
|
// Burst read
|
||||||
|
bool read_burst32(adis1657x_burst &burst);
|
||||||
|
bool validate_checksum(const adis1657x_burst &burst);
|
||||||
|
|
||||||
|
void print_error_flags(uint16_t diag_stat);
|
||||||
|
bool self_test();
|
||||||
|
void Reset();
|
||||||
|
bool Configure();
|
||||||
|
void start();
|
||||||
|
int measure(hrt_abstime timestamp_sample);
|
||||||
|
|
||||||
|
// DRDY interrupt handling
|
||||||
|
void DataReady();
|
||||||
|
bool DataReadyInterruptConfigure();
|
||||||
|
bool DataReadyInterruptDisable();
|
||||||
|
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
|
||||||
|
};
|
||||||
@@ -0,0 +1,151 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2026 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file Analog_Devices_ADIS1657x_registers.hpp
|
||||||
|
*
|
||||||
|
* Analog Devices ADIS16575/ADIS16576/ADIS16577 registers.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <cstdint>
|
||||||
|
|
||||||
|
static constexpr uint16_t Bit0 = (1 << 0);
|
||||||
|
static constexpr uint16_t Bit1 = (1 << 1);
|
||||||
|
static constexpr uint16_t Bit2 = (1 << 2);
|
||||||
|
static constexpr uint16_t Bit3 = (1 << 3);
|
||||||
|
static constexpr uint16_t Bit4 = (1 << 4);
|
||||||
|
static constexpr uint16_t Bit5 = (1 << 5);
|
||||||
|
static constexpr uint16_t Bit6 = (1 << 6);
|
||||||
|
static constexpr uint16_t Bit7 = (1 << 7);
|
||||||
|
static constexpr uint16_t Bit8 = (1 << 8);
|
||||||
|
static constexpr uint16_t Bit9 = (1 << 9);
|
||||||
|
static constexpr uint16_t Bit10 = (1 << 10);
|
||||||
|
static constexpr uint16_t Bit11 = (1 << 11);
|
||||||
|
static constexpr uint16_t Bit12 = (1 << 12);
|
||||||
|
static constexpr uint16_t Bit13 = (1 << 13);
|
||||||
|
static constexpr uint16_t Bit14 = (1 << 14);
|
||||||
|
static constexpr uint16_t Bit15 = (1 << 15);
|
||||||
|
|
||||||
|
namespace Analog_Devices_ADIS1657x
|
||||||
|
{
|
||||||
|
|
||||||
|
|
||||||
|
static constexpr uint32_t SPI_SPEED = 2'000'000;
|
||||||
|
static constexpr uint32_t SPI_STALL_PERIOD = 5;
|
||||||
|
|
||||||
|
// Timing constants (suffix indicates unit: _MS = milliseconds, _US = microseconds)
|
||||||
|
static constexpr uint32_t SW_RESET_MS = 350;
|
||||||
|
static constexpr uint32_t RST_PULSE_US = 100;
|
||||||
|
static constexpr uint32_t DEC_RATE_UPDATE_US = 30;
|
||||||
|
static constexpr uint32_t FILT_UPDATE_US = 30;
|
||||||
|
static constexpr uint32_t MSC_CTRL_UPDATE_US = 200;
|
||||||
|
static constexpr uint32_t RETRY_DELAY_US = 100'000;
|
||||||
|
|
||||||
|
// Internal clock
|
||||||
|
static constexpr uint32_t INT_CLK_HZ = 2000;
|
||||||
|
|
||||||
|
// Temperature scale
|
||||||
|
static constexpr float TEMP_SCALE = 0.1f; // 0.1 C/LSB
|
||||||
|
|
||||||
|
// Burst read command
|
||||||
|
static constexpr uint8_t BURST_CMD_MSB = 0x68;
|
||||||
|
static constexpr uint8_t BURST_CMD_LSB = 0x00;
|
||||||
|
|
||||||
|
// Product IDs
|
||||||
|
static constexpr uint16_t ADIS16575_PROD_ID = 0x40BF;
|
||||||
|
static constexpr uint16_t ADIS16576_PROD_ID = 0x40C0;
|
||||||
|
static constexpr uint16_t ADIS16577_PROD_ID = 0x40C1;
|
||||||
|
|
||||||
|
enum class Register : uint8_t {
|
||||||
|
DIAG_STAT = 0x02,
|
||||||
|
FILT_CTRL = 0x5C,
|
||||||
|
RNG_MDL = 0x5E,
|
||||||
|
MSC_CTRL = 0x60,
|
||||||
|
DEC_RATE = 0x64,
|
||||||
|
GLOB_CMD = 0x68,
|
||||||
|
PROD_ID = 0x72,
|
||||||
|
};
|
||||||
|
|
||||||
|
// MSC_CTRL register bits
|
||||||
|
enum MSC_CTRL_BIT : uint16_t {
|
||||||
|
MSC_CTRL_DR_POL = Bit0, // Data ready polarity
|
||||||
|
MSC_CTRL_POP_EN = Bit6, // Point of percussion alignment enable
|
||||||
|
MSC_CTRL_GSEN_EN = Bit7, // G sensitivity enable
|
||||||
|
MSC_CTRL_OUT_SEL = Bit8, // Output select (0=gyro/accel, 1=delta angle/velocity)
|
||||||
|
MSC_CTRL_BURST_32 = Bit9, // 32-bit burst mode enable
|
||||||
|
};
|
||||||
|
|
||||||
|
// POP_EN: factory-calibrated correction for IMU offset from vehicle CoM — does not conflict with EKF2.
|
||||||
|
// GSEN_EN: factory-calibrated gyro sensitivity to linear acceleration — does not conflict with EKF2.
|
||||||
|
static constexpr uint16_t MSC_CTRL_DEFAULT = MSC_CTRL_DR_POL | MSC_CTRL_POP_EN | MSC_CTRL_GSEN_EN;
|
||||||
|
|
||||||
|
// GLOB_CMD register bits
|
||||||
|
enum GLOB_CMD_BIT : uint16_t {
|
||||||
|
GLOB_CMD_SENSOR_SELF_TEST = Bit2,
|
||||||
|
GLOB_CMD_SW_RESET = Bit7,
|
||||||
|
};
|
||||||
|
|
||||||
|
// DIAG_STAT register bits
|
||||||
|
enum DIAG_STAT_BIT : uint16_t {
|
||||||
|
DIAG_STAT_MICROCONTROLLER_FAULT = Bit15,
|
||||||
|
DIAG_STAT_Z_ACCEL_FAILURE = Bit13,
|
||||||
|
DIAG_STAT_Y_ACCEL_FAILURE = Bit12,
|
||||||
|
DIAG_STAT_X_ACCEL_FAILURE = Bit11,
|
||||||
|
DIAG_STAT_Z_GYRO_FAILURE = Bit10,
|
||||||
|
DIAG_STAT_Y_GYRO_FAILURE = Bit9,
|
||||||
|
DIAG_STAT_X_GYRO_FAILURE = Bit8,
|
||||||
|
DIAG_STAT_SYNC_DPLL_UNLOCK = Bit7,
|
||||||
|
DIAG_STAT_MEMORY_ERROR = Bit6,
|
||||||
|
DIAG_STAT_SELF_TEST_ERR = Bit5,
|
||||||
|
DIAG_STAT_POWER_SUPPLY_MON = Bit4,
|
||||||
|
DIAG_STAT_SPI_COMM_ERROR = Bit3,
|
||||||
|
DIAG_STAT_FLASH_UPDATE_FAILURE = Bit2,
|
||||||
|
DIAG_STAT_DATAPATH_OVERRUN = Bit1,
|
||||||
|
DIAG_STAT_SENSOR_INIT_FAILURE = Bit0,
|
||||||
|
};
|
||||||
|
|
||||||
|
// Self-test duration (datasheet t_ST)
|
||||||
|
static constexpr uint32_t SELF_TEST_TIME_MS = 19;
|
||||||
|
|
||||||
|
// Filter flush delay: >= 1/(2000 Hz) to allow one output cycle between bursts
|
||||||
|
static constexpr uint32_t FILT_FLUSH_DELAY_US = 1100;
|
||||||
|
|
||||||
|
// RNG_MDL gyro range bits [3:2]
|
||||||
|
static constexpr uint16_t RNG_MDL_GYRO_MASK = 0x0C;
|
||||||
|
static constexpr uint16_t RNG_MDL_450DPS = 0x04; // bits[3:2] = 01
|
||||||
|
static constexpr uint16_t RNG_MDL_2000DPS = 0x0C; // bits[3:2] = 11
|
||||||
|
|
||||||
|
} // namespace Analog_Devices_ADIS1657x
|
||||||
@@ -0,0 +1,46 @@
|
|||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# Copyright (c) 2026 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__analog_devices__adis1657x
|
||||||
|
MAIN adis1657x
|
||||||
|
SRCS
|
||||||
|
ADIS1657x.cpp
|
||||||
|
ADIS1657x.hpp
|
||||||
|
Analog_Devices_ADIS1657x_registers.hpp
|
||||||
|
adis1657x_main.cpp
|
||||||
|
DEPENDS
|
||||||
|
px4_work_queue
|
||||||
|
drivers_accelerometer
|
||||||
|
drivers_gyroscope
|
||||||
|
)
|
||||||
@@ -0,0 +1,7 @@
|
|||||||
|
menuconfig DRIVERS_IMU_ANALOG_DEVICES_ADIS1657X
|
||||||
|
bool "adis1657x (ADIS16575 / ADIS16576 / ADIS16577)"
|
||||||
|
default n
|
||||||
|
---help---
|
||||||
|
Enable support for the Analog Devices ADIS1657x IMU family.
|
||||||
|
Supported variants: ADIS16575-2/3, ADIS16576-2/3, ADIS16577-2/3.
|
||||||
|
The variant is picked automatically at runtime.
|
||||||
@@ -0,0 +1,120 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2026 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 "ADIS1657x.hpp"
|
||||||
|
|
||||||
|
#include <px4_platform_common/getopt.h>
|
||||||
|
#include <px4_platform_common/module.h>
|
||||||
|
|
||||||
|
void
|
||||||
|
ADIS1657x::print_usage()
|
||||||
|
{
|
||||||
|
PRINT_MODULE_USAGE_NAME("adis1657x", "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_PARAM_INT('r', 0, 0, 1999,
|
||||||
|
"Decimation rate: sample_rate = 2000/(dec_rate+1). 0=2000Hz 1=1000Hz 3=500Hz 19=100Hz",
|
||||||
|
true);
|
||||||
|
PRINT_MODULE_USAGE_PARAM_INT('F', 0, 0, 6,
|
||||||
|
"Hardware Bartlett FIR filter tap size: 0=bypass 1-6=increasing filter strength",
|
||||||
|
true);
|
||||||
|
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||||
|
}
|
||||||
|
|
||||||
|
extern "C" int adis1657x_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
int ch;
|
||||||
|
using ThisDriver = ADIS1657x;
|
||||||
|
BusCLIArguments cli{false, true};
|
||||||
|
cli.default_spi_frequency = SPI_SPEED;
|
||||||
|
|
||||||
|
int dec_rate = 0;
|
||||||
|
int filt_size = 0;
|
||||||
|
|
||||||
|
while ((ch = cli.getOpt(argc, argv, "R:r:F:")) != EOF) {
|
||||||
|
switch (ch) {
|
||||||
|
case 'R':
|
||||||
|
cli.rotation = (enum Rotation)atoi(cli.optArg());
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 'r':
|
||||||
|
dec_rate = atoi(cli.optArg());
|
||||||
|
|
||||||
|
if (dec_rate < 0 || dec_rate > 1999) {
|
||||||
|
PX4_ERR("Invalid dec_rate %d (valid: 0-1999)", dec_rate);
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 'F':
|
||||||
|
filt_size = atoi(cli.optArg());
|
||||||
|
|
||||||
|
if (filt_size < 0 || filt_size > 6) {
|
||||||
|
PX4_ERR("Invalid filt_size %d (valid: 0-6)", filt_size);
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const char *verb = cli.optArg();
|
||||||
|
|
||||||
|
if (!verb) {
|
||||||
|
ThisDriver::print_usage();
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
ThisDriver::selected_dec_rate = dec_rate;
|
||||||
|
ThisDriver::selected_filt_size = filt_size;
|
||||||
|
|
||||||
|
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ADIS1657X);
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user