diff --git a/boards/av/x-v1/init/rc.board_sensors b/boards/av/x-v1/init/rc.board_sensors index 6aa46b14ea..d88cd62396 100644 --- a/boards/av/x-v1/init/rc.board_sensors +++ b/boards/av/x-v1/init/rc.board_sensors @@ -5,7 +5,7 @@ adc start -adis16477 -R 8 start +adis16477 -s -R 8 start lps22hb -s start diff --git a/boards/av/x-v1/src/board_config.h b/boards/av/x-v1/src/board_config.h index 107ce3b251..ddaa996371 100644 --- a/boards/av/x-v1/src/board_config.h +++ b/boards/av/x-v1/src/board_config.h @@ -125,7 +125,7 @@ #include /* SPI1 */ -#define PX4_SPIDEV_ADIS16477 PX4_MK_SPI_SEL(0,DRV_GYR_DEVTYPE_ADIS16477) +#define PX4_SPIDEV_ADIS16477 PX4_MK_SPI_SEL(0,DRV_IMU_DEVTYPE_ADIS16477) #define PX4_SENSOR1_BUS_CS_GPIO {GPIO_SPI1_CS1_ADIS16477} /* SPI2 */ diff --git a/boards/av/x-v1/src/spi.cpp b/boards/av/x-v1/src/spi.cpp index 5b74dd47c9..bc468a107a 100644 --- a/boards/av/x-v1/src/spi.cpp +++ b/boards/av/x-v1/src/spi.cpp @@ -37,7 +37,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_GYR_DEVTYPE_ADIS16477, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortJ, GPIO::Pin0}), + initSPIDevice(DRV_IMU_DEVTYPE_ADIS16477, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortJ, GPIO::Pin0}), }), initSPIBusExternal(SPI::Bus::SPI2, { initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin0}), diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index a9539f130a..00fce89452 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -114,9 +114,8 @@ #define DRV_GYR_DEVTYPE_FXAS2100C 0x54 #define DRV_IMU_DEVTYPE_ADIS16448 0x57 #define DRV_BARO_DEVTYPE_LPS22HB 0x58 -#define DRV_ACC_DEVTYPE_ADIS16477 0x59 +#define DRV_IMU_DEVTYPE_ADIS16477 0x59 -#define DRV_GYR_DEVTYPE_ADIS16477 0x60 #define DRV_ACC_DEVTYPE_LSM303AGR 0x61 #define DRV_MAG_DEVTYPE_LSM303AGR 0x62 #define DRV_ACC_DEVTYPE_ADIS16497 0x63 diff --git a/src/drivers/imu/adis16477/ADIS16477.cpp b/src/drivers/imu/adis16477/ADIS16477.cpp index 686d7768e2..8f7c6720e7 100644 --- a/src/drivers/imu/adis16477/ADIS16477.cpp +++ b/src/drivers/imu/adis16477/ADIS16477.cpp @@ -53,31 +53,30 @@ static constexpr uint32_t ADIS16477_DEFAULT_RATE = 1000; using namespace time_literals; -ADIS16477::ADIS16477(int bus, uint32_t device, enum Rotation rotation) : - SPI("ADIS16477", nullptr, bus, device, SPIDEV_MODE3, 1000000), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), +ADIS16477::ADIS16477(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("ADIS16477", 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, "adis16477: read")), - _bad_transfers(perf_alloc(PC_COUNT, "adis16477: bad transfers")) + _bad_transfers(perf_alloc(PC_COUNT, "adis16477: bad transfers")), + _drdy_gpio(drdy_gpio) { #ifdef GPIO_SPI1_RESET_ADIS16477 // Configure hardware reset line px4_arch_configgpio(GPIO_SPI1_RESET_ADIS16477); #endif // GPIO_SPI1_RESET_ADIS16477 - _px4_accel.set_device_type(DRV_ACC_DEVTYPE_ADIS16477); + _px4_accel.set_device_type(DRV_IMU_DEVTYPE_ADIS16477); _px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB - _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16477); + _px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ADIS16477); _px4_gyro.set_scale(math::radians(0.025f)); // gyro 0.025 °/sec/LSB } ADIS16477::~ADIS16477() { - // make sure we are truly inactive - stop(); - // delete the perf counters perf_free(_sample_perf); perf_free(_bad_transfers); @@ -86,11 +85,12 @@ ADIS16477::~ADIS16477() int ADIS16477::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(); @@ -170,18 +170,18 @@ ADIS16477::probe() uint16_t product_id = read_reg16(PROD_ID); if (product_id == PROD_ID_ADIS16477) { - PX4_DEBUG("PRODUCT: %X", product_id); + DEVICE_DEBUG("PRODUCT: %X", product_id); if (self_test_memory() && self_test_sensor()) { return PX4_OK; } 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(); } } @@ -273,27 +273,25 @@ ADIS16477::write_reg16(uint8_t reg, uint16_t value) void ADIS16477::start() { -#ifdef GPIO_SPI1_DRDY1_ADIS16477 - // Setup data ready on rising edge - px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ADIS16477, true, false, true, &ADIS16477::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, &ADIS16477::data_ready_interrupt, this); - // start polling at the specified rate - ScheduleOnInterval((1_s / ADIS16477_DEFAULT_RATE), 10000); -#endif + } else { + // start polling at the specified rate + ScheduleOnInterval((1_s / ADIS16477_DEFAULT_RATE), 10000); + } } void -ADIS16477::stop() +ADIS16477::exit_and_cleanup() { -#ifdef GPIO_SPI1_DRDY1_ADIS16477 - // Disable data ready callback - px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ADIS16477, 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 @@ -308,7 +306,7 @@ ADIS16477::data_ready_interrupt(int irq, void *context, void *arg) } void -ADIS16477::Run() +ADIS16477::RunImpl() { // make another measurement measure(); @@ -384,8 +382,9 @@ ADIS16477::measure() } void -ADIS16477::print_info() +ADIS16477::print_status() { + I2CSPIDriverBase::print_status(); perf_print_counter(_sample_perf); perf_print_counter(_bad_transfers); diff --git a/src/drivers/imu/adis16477/ADIS16477.hpp b/src/drivers/imu/adis16477/ADIS16477.hpp index ab943345a0..ee99a16a55 100644 --- a/src/drivers/imu/adis16477/ADIS16477.hpp +++ b/src/drivers/imu/adis16477/ADIS16477.hpp @@ -45,20 +45,27 @@ #include #include #include -#include +#include -class ADIS16477 : public device::SPI, public px4::ScheduledWorkItem +class ADIS16477 : public device::SPI, public I2CSPIDriver { public: - ADIS16477(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE); + ADIS16477(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 ~ADIS16477(); - virtual int init(); + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); - void print_info(); + int init(); + void print_status() override; + + void RunImpl(); protected: - virtual int probe(); + int probe() override; + void exit_and_cleanup() override; private: @@ -68,6 +75,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 in the ADIS16477, including command byte and interrupt status. struct ADISReport { @@ -91,11 +100,6 @@ private: */ void start(); - /** - * Stop automatic measurement. - */ - void stop(); - /** * Reset chip. * @@ -103,8 +107,6 @@ private: */ int reset(); - void Run() override; - static int data_ready_interrupt(int irq, void *context, void *arg); /** diff --git a/src/drivers/imu/adis16477/adis16477_main.cpp b/src/drivers/imu/adis16477/adis16477_main.cpp index b00443af1b..3b5b94a863 100644 --- a/src/drivers/imu/adis16477/adis16477_main.cpp +++ b/src/drivers/imu/adis16477/adis16477_main.cpp @@ -34,111 +34,74 @@ #include "ADIS16477.hpp" #include +#include -namespace adis16477 +void +ADIS16477::print_usage() { -ADIS16477 *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_ADIS16477) - g_dev = new ADIS16477(PX4_SPI_BUS_SENSOR1, PX4_SPIDEV_ADIS16477, rotation); -#elif defined(PX4_SPI_BUS_EXT) - g_dev = new ADIS16477(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, rotation); -#elif defined(PX4_SPIDEV_EXTERNAL1_1) - g_dev = new ADIS16477(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("adis16477", "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 *ADIS16477::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - if (g_dev == nullptr) { - PX4_WARN("driver not running"); - return -1; + ADIS16477 *instance = new ADIS16477(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 adis16477 - extern "C" int adis16477_main(int argc, char *argv[]) { - enum Rotation rotation = ROTATION_NONE; - int myoptind = 1; - int ch = 0; - const char *myoptarg = nullptr; + int ch; + using ThisDriver = ADIS16477; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = 1000000; - // 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 adis16477::usage(); } } - const char *verb = argv[myoptind]; + const char *verb = cli.optarg(); - if (!strcmp(verb, "start")) { - return adis16477::start(rotation); - - } else if (!strcmp(verb, "stop")) { - return adis16477::stop(); - - } else if (!strcmp(verb, "status")) { - return adis16477::status(); + if (!verb) { + ThisDriver::print_usage(); + return -1; } - return adis16477::usage(); + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ADIS16477); + + 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; }