diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index c8d11a4eef..90552c4fff 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -13,7 +13,7 @@ ina226 -X -b 2 -t 2 -k start mpu6000 -R 8 -s -T 20602 start # Internal SPI bus ISM300DLC -ism330dlc start +ism330dlc -s start # Internal SPI bus BMI088 accel bmi088 -A -R 12 -s start diff --git a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp index 30807744eb..c8ca96e8c2 100644 --- a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp +++ b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp @@ -39,9 +39,11 @@ static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) static constexpr uint32_t FIFO_INTERVAL{1000}; // 1000 us / 1000 Hz interval -ISM330DLC::ISM330DLC(int bus, uint32_t device, enum Rotation rotation) : - SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), +ISM330DLC::ISM330DLC(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : + SPI(MODULE_NAME, nullptr, bus, device, spi_mode, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), + _drdy_gpio(drdy_gpio), _px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation), _px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation) { @@ -55,8 +57,6 @@ ISM330DLC::ISM330DLC(int bus, uint32_t device, enum Rotation rotation) : ISM330DLC::~ISM330DLC() { - Stop(); - perf_free(_interval_perf); perf_free(_transfer_perf); perf_free(_fifo_empty_perf); @@ -66,33 +66,47 @@ ISM330DLC::~ISM330DLC() perf_free(_drdy_interval_perf); } +void ISM330DLC::exit_and_cleanup() +{ + if (_drdy_gpio != 0) { + // Disable data ready callback + px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr); + + RegisterWrite(Register::INT1_CTRL, 0); + } + + I2CSPIDriverBase::exit_and_cleanup(); +} + int ISM330DLC::probe() { const uint8_t whoami = RegisterRead(Register::WHO_AM_I); if (whoami != ISM330DLC_WHO_AM_I) { - PX4_WARN("unexpected WHO_AM_I 0x%02x", whoami); + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); return PX4_ERROR; } return PX4_OK; } -bool ISM330DLC::Init() +int ISM330DLC::init() { - if (SPI::init() != PX4_OK) { - PX4_ERR("SPI::init failed"); - return false; + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; } if (!Reset()) { PX4_ERR("reset failed"); - return false; + return PX4_ERROR; } Start(); - return true; + return PX4_OK; } bool ISM330DLC::Reset() @@ -193,41 +207,28 @@ void ISM330DLC::DataReady() void ISM330DLC::Start() { - Stop(); - ResetFIFO(); -#if defined(GPIO_SPI2_DRDY1_ISM330) && false // TODO: enable - // Setup data ready on rising edge - px4_arch_gpiosetevent(GPIO_SPI2_DRDY1_ISM330, true, true, false, &ISM330DLC::DataReadyInterruptCallback, this); + if (_drdy_gpio != 0 && false) { // TODO: enable + // Setup data ready on rising edge + px4_arch_gpiosetevent(_drdy_gpio, true, true, false, &ISM330DLC::DataReadyInterruptCallback, this); - // FIFO threshold level setting - // FIFO_CTRL1: FTH_[7:0] - // FIFO_CTRL2: FTH_[10:8] - const uint8_t fifo_threshold = 12; - RegisterWrite(Register::FIFO_CTRL1, fifo_threshold); + // FIFO threshold level setting + // FIFO_CTRL1: FTH_[7:0] + // FIFO_CTRL2: FTH_[10:8] + const uint8_t fifo_threshold = 12; + RegisterWrite(Register::FIFO_CTRL1, fifo_threshold); - // INT1: FIFO full, overrun, or threshold - RegisterWrite(Register::INT1_CTRL, INT1_CTRL_BIT::INT1_FULL_FLAG | INT1_CTRL_BIT::INT1_FIFO_OVR | - INT1_CTRL_BIT::INT1_FTH); -#else - ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL); -#endif + // INT1: FIFO full, overrun, or threshold + RegisterWrite(Register::INT1_CTRL, INT1_CTRL_BIT::INT1_FULL_FLAG | INT1_CTRL_BIT::INT1_FIFO_OVR | + INT1_CTRL_BIT::INT1_FTH); + + } else { + ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL); + } } -void ISM330DLC::Stop() -{ -#if defined(GPIO_SPI2_DRDY1_ISM330) && false // TODO: enable - // Disable data ready callback - px4_arch_gpiosetevent(GPIO_SPI2_DRDY1_ISM330, false, false, false, nullptr, nullptr); - - RegisterWrite(Register::INT1_CTRL, 0); -#else - ScheduleClear(); -#endif -} - -void ISM330DLC::Run() +void ISM330DLC::RunImpl() { perf_count(_interval_perf); @@ -333,8 +334,9 @@ void ISM330DLC::Run() _px4_accel.updateFIFO(accel); } -void ISM330DLC::PrintInfo() +void ISM330DLC::print_status() { + I2CSPIDriverBase::print_status(); perf_print_counter(_interval_perf); perf_print_counter(_transfer_perf); perf_print_counter(_fifo_empty_perf); diff --git a/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp b/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp index 14c6b4eece..87c6b1ad14 100644 --- a/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp +++ b/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp @@ -48,22 +48,33 @@ #include #include #include -#include +#include using namespace ST_ISM330DLC; -class ISM330DLC : public device::SPI, public px4::ScheduledWorkItem +class ISM330DLC : public device::SPI, public I2CSPIDriver { public: - ISM330DLC(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE); + ISM330DLC(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); ~ISM330DLC() override; - bool Init(); - void Start(); - void Stop(); - bool Reset(); - void PrintInfo(); + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + void print_status() override; + + void RunImpl(); + + int init() override; + + void Start(); + bool Reset(); + +protected: + void custom_method(const BusCLIArguments &cli) override; + void exit_and_cleanup() override; private: // Sensor Configuration @@ -84,8 +95,6 @@ private: static int DataReadyInterruptCallback(int irq, void *context, void *arg); void DataReady(); - void Run() override; - uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); void RegisterSetBits(Register reg, uint8_t setbits); @@ -93,6 +102,8 @@ private: void ResetFIFO(); + const spi_drdy_gpio_t _drdy_gpio; + PX4Accelerometer _px4_accel; PX4Gyroscope _px4_gyro; diff --git a/src/drivers/imu/st/ism330dlc/ism330dlc_main.cpp b/src/drivers/imu/st/ism330dlc/ism330dlc_main.cpp index 42c0da8837..e797b6f815 100644 --- a/src/drivers/imu/st/ism330dlc/ism330dlc_main.cpp +++ b/src/drivers/imu/st/ism330dlc/ism330dlc_main.cpp @@ -34,125 +34,84 @@ #include "ISM330DLC.hpp" #include +#include -namespace ism330dlc +void +ISM330DLC::print_usage() { - -ISM330DLC *g_dev{nullptr}; - -int start(enum Rotation rotation); -int stop(); -int status(); -void usage(); - -int start(enum Rotation rotation) -{ - if (g_dev != nullptr) { - PX4_WARN("already started"); - return 0; - } - - // create the driver - g_dev = new ISM330DLC(PX4_SPI_BUS_SENSORS2, PX4_SPIDEV_ISM330, rotation); // v5x TODO: board manifest - - if (g_dev == nullptr) { - PX4_ERR("driver start failed"); - return -1; - } - - if (!g_dev->Init()) { - PX4_ERR("driver init failed"); - delete g_dev; - g_dev = nullptr; - return -1; - } - - return 0; + PRINT_MODULE_USAGE_NAME("ism330dlc", "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_COMMAND("reset"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -int stop() +I2CSPIDriverBase *ISM330DLC::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - if (g_dev == nullptr) { - PX4_WARN("driver not running"); + ISM330DLC *instance = new ISM330DLC(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, + cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; } - g_dev->Stop(); - delete g_dev; - - return 0; -} - -int reset() -{ - if (g_dev == nullptr) { - PX4_WARN("driver not running"); + if (OK != instance->init()) { + delete instance; + return nullptr; } - return g_dev->Reset(); + return instance; } -int status() +void ISM330DLC::custom_method(const BusCLIArguments &cli) { - if (g_dev == nullptr) { - PX4_WARN("driver not running"); - return -1; - } - - g_dev->PrintInfo(); - - return 0; + Reset(); } -void usage() +extern "C" int ism330dlc_main(int argc, char *argv[]) { - PX4_INFO("missing command: try 'start', 'stop', 'reset', 'status'"); - PX4_INFO("options:"); - PX4_INFO(" -R rotation"); -} + int ch; + using ThisDriver = ISM330DLC; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = ST_ISM330DLC::SPI_SPEED; -} // namespace ism330dlc - -extern "C" __EXPORT int ism330dlc_main(int argc, char *argv[]) -{ - enum Rotation rotation = ROTATION_NONE; - int myoptind = 1; - int ch = 0; - const char *myoptarg = nullptr; - - /* 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: - ism330dlc::usage(); - return 0; } } - if (myoptind >= argc) { - ism330dlc::usage(); + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); return -1; } - const char *verb = argv[myoptind]; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ST_ISM330DLC); if (!strcmp(verb, "start")) { - return ism330dlc::start(rotation); - - } else if (!strcmp(verb, "stop")) { - return ism330dlc::stop(); - - } else if (!strcmp(verb, "status")) { - return ism330dlc::status(); - - } else if (!strcmp(verb, "reset")) { - return ism330dlc::reset(); + return ThisDriver::module_start(cli, iterator); } - ism330dlc::usage(); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } - return 0; + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + if (!strcmp(verb, "reset")) { + return ThisDriver::module_custom_method(cli, iterator); + } + + ThisDriver::print_usage(); + return -1; }