diff --git a/boards/av/x-v1/init/rc.board_sensors b/boards/av/x-v1/init/rc.board_sensors index d88cd62396..10fc3c162f 100644 --- a/boards/av/x-v1/init/rc.board_sensors +++ b/boards/av/x-v1/init/rc.board_sensors @@ -16,8 +16,7 @@ ms4525_airspeed -T 4515 -I -b 3 start if ! param greater SENS_EN_PMW3901 0 then # try to start adis16497 only if pmw3901 isn't enabled, or parameter doesn't exists - # TODO: this one is external SPI - adis16497 start + adis16497 -S start fi # Possible external compasses diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 00fce89452..6261a0ac46 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -118,8 +118,7 @@ #define DRV_ACC_DEVTYPE_LSM303AGR 0x61 #define DRV_MAG_DEVTYPE_LSM303AGR 0x62 -#define DRV_ACC_DEVTYPE_ADIS16497 0x63 -#define DRV_GYR_DEVTYPE_ADIS16497 0x64 +#define DRV_IMU_DEVTYPE_ADIS16497 0x63 #define DRV_BARO_DEVTYPE_BAROSIM 0x65 #define DRV_GYR_DEVTYPE_BMI088 0x66 #define DRV_BARO_DEVTYPE_BMP388 0x67 diff --git a/src/drivers/imu/adis16497/ADIS16497.cpp b/src/drivers/imu/adis16497/ADIS16497.cpp index bf089bb9f8..515a220f60 100644 --- a/src/drivers/imu/adis16497/ADIS16497.cpp +++ b/src/drivers/imu/adis16497/ADIS16497.cpp @@ -70,29 +70,27 @@ static constexpr uint32_t ADIS16497_DEFAULT_RATE = 1000; using namespace time_literals; -ADIS16497::ADIS16497(int bus, uint32_t device, enum Rotation rotation) : - SPI("ADIS16497", nullptr, bus, device, SPIDEV_MODE3, 5000000), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), +ADIS16497::ADIS16497(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : + SPI("ADIS16497", nullptr, bus, device, spi_mode, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _px4_accel(get_device_id(), ORB_PRIO_MAX, rotation), _px4_gyro(get_device_id(), ORB_PRIO_MAX, rotation), _sample_perf(perf_alloc(PC_ELAPSED, "adis16497: read")), - _bad_transfers(perf_alloc(PC_COUNT, "adis16497: bad transfers")) + _bad_transfers(perf_alloc(PC_COUNT, "adis16497: bad transfers")), + _drdy_gpio(drdy_gpio) { #ifdef GPIO_SPI1_RESET_ADIS16497 // Configure hardware reset line px4_arch_configgpio(GPIO_SPI1_RESET_ADIS16497); #endif // GPIO_SPI1_RESET_ADIS16497 - _px4_accel.set_device_type(DRV_ACC_DEVTYPE_ADIS16497); - - _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16497); + _px4_accel.set_device_type(DRV_IMU_DEVTYPE_ADIS16497); + _px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ADIS16497); } ADIS16497::~ADIS16497() { - // make sure we are truly inactive - stop(); - // delete the perf counters perf_free(_sample_perf); perf_free(_bad_transfers); @@ -101,11 +99,12 @@ ADIS16497::~ADIS16497() int ADIS16497::init() { - // do SPI init (and probe) first - if (SPI::init() != OK) { + int ret = SPI::init(); + + if (ret != OK) { // if probe/setup failed, bail now - PX4_DEBUG("SPI setup failed"); - return PX4_ERROR; + DEVICE_DEBUG("SPI init failed (%i)", ret); + return ret; } start(); @@ -257,17 +256,17 @@ ADIS16497::probe() return PX4_OK; } else { - PX4_ERR("probe attempt %d: reading model id failed, resetting", i); + DEVICE_DEBUG("probe attempt %d: reading model id failed, resetting", i); reset(); } } else { - PX4_ERR("probe attempt %d: self test failed, resetting", i); + DEVICE_DEBUG("probe attempt %d: self test failed, resetting", i); reset(); } } else { - PX4_ERR("probe attempt %d: read product id failed, resetting", i); + DEVICE_DEBUG("probe attempt %d: read product id failed, resetting", i); reset(); } } @@ -380,27 +379,25 @@ ADIS16497::write_reg16(uint8_t reg, uint16_t value) void ADIS16497::start() { -#ifdef GPIO_SPI1_DRDY1_ADIS16497 - // Setup data ready on rising edge - px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ADIS16497, true, false, true, &ADIS16497::data_ready_interrupt, this); -#else - // Make sure we are stopped first - stop(); + if (_drdy_gpio != 0) { + // Setup data ready on rising edge + px4_arch_gpiosetevent(_drdy_gpio, true, false, true, &ADIS16497::data_ready_interrupt, this); - // start polling at the specified rate - ScheduleOnInterval((1_s / ADIS16497_DEFAULT_RATE), 10000); -#endif + } else { + // start polling at the specified rate + ScheduleOnInterval((1_s / ADIS16497_DEFAULT_RATE), 10000); + } } void -ADIS16497::stop() +ADIS16497::exit_and_cleanup() { -#ifdef GPIO_SPI1_DRDY1_ADIS16497 - // Disable data ready callback - px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ADIS16497, false, false, false, nullptr, nullptr); -#else - ScheduleClear(); -#endif + if (_drdy_gpio != 0) { + // Disable data ready callback + px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr); + } + + I2CSPIDriverBase::exit_and_cleanup(); } int @@ -415,7 +412,7 @@ ADIS16497::data_ready_interrupt(int irq, void *context, void *arg) } void -ADIS16497::Run() +ADIS16497::RunImpl() { // make another measurement measure(); @@ -505,8 +502,9 @@ ADIS16497::measure() } void -ADIS16497::print_info() +ADIS16497::print_status() { + I2CSPIDriverBase::print_status(); perf_print_counter(_sample_perf); perf_print_counter(_bad_transfers); diff --git a/src/drivers/imu/adis16497/ADIS16497.hpp b/src/drivers/imu/adis16497/ADIS16497.hpp index ceb0ad5799..a3f6e30473 100644 --- a/src/drivers/imu/adis16497/ADIS16497.hpp +++ b/src/drivers/imu/adis16497/ADIS16497.hpp @@ -45,7 +45,7 @@ #include #include #include -#include +#include // TODO : This is a copy of the NuttX CRC32 table static constexpr uint32_t crc32_tab[] = { @@ -83,18 +83,25 @@ static constexpr uint32_t crc32_tab[] = { 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d }; -class ADIS16497 : public device::SPI, public px4::ScheduledWorkItem +class ADIS16497 : public device::SPI, public I2CSPIDriver { public: - ADIS16497(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE); + ADIS16497(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); virtual ~ADIS16497(); - virtual int init(); + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); - void print_info(); + int init() override; + void print_status() override; + + void RunImpl(); protected: - virtual int probe(); + int probe() override; + void exit_and_cleanup() override; private: @@ -104,6 +111,8 @@ private: perf_counter_t _sample_perf; perf_counter_t _bad_transfers; + const spi_drdy_gpio_t _drdy_gpio; + #pragma pack(push, 1) // Report conversation with the ADIS16497, including command byte. struct ADISReport { @@ -135,11 +144,6 @@ private: */ void start(); - /** - * Stop automatic measurement. - */ - void stop(); - /** * Reset chip. * @@ -147,8 +151,6 @@ private: */ int reset(); - void Run() override; - static int data_ready_interrupt(int irq, void *context, void *arg); /** diff --git a/src/drivers/imu/adis16497/adis16497_main.cpp b/src/drivers/imu/adis16497/adis16497_main.cpp index f64318c170..7f993fa912 100644 --- a/src/drivers/imu/adis16497/adis16497_main.cpp +++ b/src/drivers/imu/adis16497/adis16497_main.cpp @@ -34,107 +34,74 @@ #include "ADIS16497.hpp" #include +#include -namespace adis16497 +void +ADIS16497::print_usage() { -ADIS16497 *g_dev{nullptr}; - -static int start(enum Rotation rotation) -{ - if (g_dev != nullptr) { - PX4_WARN("already started"); - return 0; - } - - // create the driver -#if defined(PX4_SPIDEV_EXTERNAL1_1) - g_dev = new ADIS16497(PX4_SPI_BUS_EXTERNAL1, PX4_SPIDEV_EXTERNAL1_1, rotation); -#else - PX4_ERR("External SPI not available"); - return -1; -#endif - - if (g_dev == nullptr) { - PX4_ERR("driver start failed"); - return -1; - } - - if (g_dev->init() != PX4_OK) { - PX4_ERR("driver init failed"); - delete g_dev; - g_dev = nullptr; - return -1; - } - - return 0; + PRINT_MODULE_USAGE_NAME("adis16497", "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_DEFAULT_COMMANDS(); } -static int stop() +I2CSPIDriverBase *ADIS16497::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - if (g_dev == nullptr) { - PX4_WARN("driver not running"); - return -1; + ADIS16497 *instance = new ADIS16497(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, + cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; } - delete g_dev; - g_dev = nullptr; - - return 0; -} - -static int status() -{ - if (g_dev == nullptr) { - PX4_INFO("driver not running"); - return 0; + if (OK != instance->init()) { + delete instance; + return nullptr; } - g_dev->print_info(); - - return 0; + return instance; } -static int usage() -{ - PX4_INFO("missing command: try 'start', 'stop', 'status'"); - PX4_INFO("options:"); - PX4_INFO(" -R rotation"); - - return 0; -} - -} // namespace adis16497 - extern "C" int adis16497_main(int argc, char *argv[]) { - enum Rotation rotation = ROTATION_NONE; - int myoptind = 1; - int ch = 0; - const char *myoptarg = nullptr; + int ch; + using ThisDriver = ADIS16497; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = 5000000; - // start options - while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { + while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - rotation = (enum Rotation)atoi(myoptarg); + cli.rotation = (enum Rotation)atoi(cli.optarg()); break; - - default: - return adis16497::usage(); } } - const char *verb = argv[myoptind]; + const char *verb = cli.optarg(); - if (!strcmp(verb, "start")) { - return adis16497::start(rotation); - - } else if (!strcmp(verb, "stop")) { - return adis16497::stop(); - - } else if (!strcmp(verb, "status")) { - return adis16497::status(); + if (!verb) { + ThisDriver::print_usage(); + return -1; } - return adis16497::usage(); + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ADIS16497); + + 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; }