mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-21 21:55:34 +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_ADIS16607 0x5C
|
||||
#define DRV_IMU_DEVTYPE_ADIS1650X 0x5D
|
||||
#define DRV_IMU_DEVTYPE_ADIS1657X 0x5E
|
||||
|
||||
#define DRV_BARO_DEVTYPE_MPC2520 0x5F
|
||||
#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