diff --git a/boards/omnibus/f4sd/init/rc.board_sensors b/boards/omnibus/f4sd/init/rc.board_sensors index a298315ccd..1055af62e3 100644 --- a/boards/omnibus/f4sd/init/rc.board_sensors +++ b/boards/omnibus/f4sd/init/rc.board_sensors @@ -8,7 +8,7 @@ adc start if ! mpu6000 -R 12 -s start then # some boards such as the Hobbywing XRotor F4 G2 use the ICM-20602 - icm20602 -R 6 start + icm20602 -s -R 6 start #mpu6000 -R 12 -T 20602 -s start fi diff --git a/boards/px4/fmu-v4/init/rc.board_sensors b/boards/px4/fmu-v4/init/rc.board_sensors index cc1ddfc865..8d9f9f027d 100644 --- a/boards/px4/fmu-v4/init/rc.board_sensors +++ b/boards/px4/fmu-v4/init/rc.board_sensors @@ -43,7 +43,7 @@ fi # will prevent the incorrect driver from a successful initialization. # ICM20602 internal SPI bus ICM-20608-G is rotated 90 deg yaw -if ! icm20602 -R 8 start +if ! icm20602 -s -R 8 start then # ICM20608 internal SPI bus ICM-20602-G is rotated 90 deg yaw icm20608g -R 8 start diff --git a/boards/px4/fmu-v4pro/init/rc.board_sensors b/boards/px4/fmu-v4pro/init/rc.board_sensors index a208c8ad76..da5b8b268b 100644 --- a/boards/px4/fmu-v4pro/init/rc.board_sensors +++ b/boards/px4/fmu-v4pro/init/rc.board_sensors @@ -11,7 +11,7 @@ adc start icm20608g -R 8 start # Internal SPI bus ICM-20602 -icm20602 -R 8 start +icm20602 -s -R 8 start # Internal SPI bus mpu9250 mpu9250 -R 2 start diff --git a/boards/px4/fmu-v5/init/rc.board_sensors b/boards/px4/fmu-v5/init/rc.board_sensors index 5576bd107a..d08c600ae3 100644 --- a/boards/px4/fmu-v5/init/rc.board_sensors +++ b/boards/px4/fmu-v5/init/rc.board_sensors @@ -7,14 +7,14 @@ adc start # Internal SPI bus ICM-20602 mpu6000 -R 8 -s -T 20602 start -#icm20602 -R 6 start +#icm20602 -s -R 6 start # Internal SPI bus ICM-20689 mpu6000 -R 8 -s -T 20689 start #icm20689 -R 6 start # new sensor drivers (in testing) -#icm20602 start +#icm20602 -s start #icm20689 start # Internal SPI bus BMI055 accel diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index 07b4265038..a57461aa0c 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -40,13 +40,16 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -ICM20602::ICM20602(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())), +ICM20602::ICM20602(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_VERY_HIGH, rotation), _px4_gyro(get_device_id(), ORB_PRIO_VERY_HIGH, rotation) { set_device_type(DRV_IMU_DEVTYPE_ICM20602); + _px4_accel.set_device_type(DRV_IMU_DEVTYPE_ICM20602); _px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ICM20602); @@ -55,8 +58,6 @@ ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) : ICM20602::~ICM20602() { - Stop(); - perf_free(_transfer_perf); perf_free(_bad_register_perf); perf_free(_bad_transfer_perf); @@ -66,36 +67,35 @@ ICM20602::~ICM20602() perf_free(_drdy_interval_perf); } -bool ICM20602::Init() +int ICM20602::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; } - return Reset(); -} - -void ICM20602::Stop() -{ - // wait until stopped - while (_state.load() != STATE::STOPPED) { - _state.store(STATE::REQUEST_STOP); - ScheduleNow(); - px4_usleep(10); - } + return Reset() ? 0 : -1; } bool ICM20602::Reset() { - _state.store(STATE::RESET); + _state = STATE::RESET; ScheduleClear(); ScheduleNow(); return true; } -void ICM20602::PrintInfo() +void ICM20602::exit_and_cleanup() { + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void ICM20602::print_status() +{ + I2CSPIDriverBase::print_status(); PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us, static_cast(1000000 / _fifo_empty_interval_us)); @@ -116,21 +116,21 @@ int ICM20602::probe() const uint8_t whoami = RegisterRead(Register::WHO_AM_I); if (whoami != WHOAMI) { - PX4_WARN("unexpected WHO_AM_I 0x%02x", whoami); + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); return PX4_ERROR; } return PX4_OK; } -void ICM20602::Run() +void ICM20602::RunImpl() { - switch (_state.load()) { + switch (_state) { case STATE::RESET: // PWR_MGMT_1: Device Reset RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET); _reset_timestamp = hrt_absolute_time(); - _state.store(STATE::WAIT_FOR_RESET); + _state = STATE::WAIT_FOR_RESET; ScheduleDelayed(100); break; @@ -143,14 +143,14 @@ void ICM20602::Run() && (RegisterRead(Register::CONFIG) == 0x80)) { // if reset succeeded then configure - _state.store(STATE::CONFIGURE); + _state = STATE::CONFIGURE; ScheduleNow(); } else { // RESET not complete if (hrt_elapsed_time(&_reset_timestamp) > 10_ms) { PX4_ERR("Reset failed, retrying"); - _state.store(STATE::RESET); + _state = STATE::RESET; ScheduleDelayed(10_ms); } else { @@ -164,7 +164,7 @@ void ICM20602::Run() case STATE::CONFIGURE: if (Configure()) { // if configure succeeded then start reading from FIFO - _state.store(STATE::FIFO_READ); + _state = STATE::FIFO_READ; if (DataReadyInterruptConfigure()) { _data_ready_interrupt_enabled = true; @@ -242,23 +242,13 @@ void ICM20602::Run() } else { // register check failed, force reconfigure PX4_DEBUG("Health check failed, reconfiguring"); - _state.store(STATE::CONFIGURE); + _state = STATE::CONFIGURE; ScheduleNow(); } } } break; - - case STATE::REQUEST_STOP: - DataReadyInterruptDisable(); - ScheduleClear(); - _state.store(STATE::STOPPED); - break; - - case STATE::STOPPED: - // DO NOTHING - break; } } @@ -378,49 +368,21 @@ void ICM20602::DataReady() bool ICM20602::DataReadyInterruptConfigure() { - int ret_setevent = -1; + if (_drdy_gpio == 0) { + return false; + } // Setup data ready on rising edge - // TODO: cleanup horrible DRDY define mess -#if defined(GPIO_DRDY_PORTC_PIN14) - ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, true, false, true, &ICM20602::DataReadyInterruptCallback, - this); -#elif defined(GPIO_SPI1_DRDY1_ICM20602) - ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, - this); -#elif defined(GPIO_SPI1_DRDY4_ICM20602) - ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, - this); -#elif defined(GPIO_SPI1_DRDY1_ICM20602) - ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, - this); -#elif defined(GPIO_DRDY_ICM_2060X) - ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, true, false, true, &ICM20602::DataReadyInterruptCallback, - this); -#endif - - return (ret_setevent == 0); + return px4_arch_gpiosetevent(_drdy_gpio, true, false, true, &ICM20602::DataReadyInterruptCallback, this) == 0; } bool ICM20602::DataReadyInterruptDisable() { - int ret_setevent = -1; + if (_drdy_gpio == 0) { + return false; + } - // Disable data ready callback - // TODO: cleanup horrible DRDY define mess -#if defined(GPIO_DRDY_PORTC_PIN14) - ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, false, false, false, nullptr, nullptr); -#elif defined(GPIO_SPI1_DRDY1_ICM20602) - ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr); -#elif defined(GPIO_SPI1_DRDY4_ICM20602) - ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, false, false, false, nullptr, nullptr); -#elif defined(GPIO_SPI1_DRDY1_ICM20602) - ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr); -#elif defined(GPIO_DRDY_ICM_2060X) - ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, false, false, false, nullptr, nullptr); -#endif - - return (ret_setevent == 0); + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; } bool ICM20602::RegisterCheck(const register_config_t ®_cfg, bool notify) diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp index c844683b2f..edbcd6a8e1 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -49,22 +49,32 @@ #include #include #include -#include +#include using namespace InvenSense_ICM20602; -class ICM20602 : public device::SPI, public px4::ScheduledWorkItem +class ICM20602 : public device::SPI, public I2CSPIDriver { public: - ICM20602(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE); + ICM20602(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); ~ICM20602() 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 RunImpl(); + + int init() override; + void print_status() override; + + void Start(); + bool Reset(); + +protected: + void custom_method(const BusCLIArguments &cli) override; + void exit_and_cleanup() override; private: // Sensor Configuration @@ -88,8 +98,6 @@ private: int probe() override; - void Run() override; - bool Configure(); void ConfigureAccel(); void ConfigureGyro(); @@ -116,6 +124,8 @@ private: void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples); bool ProcessTemperature(const FIFOTransferBuffer &buffer, uint8_t samples); + const spi_drdy_gpio_t _drdy_gpio; + PX4Accelerometer _px4_accel; PX4Gyroscope _px4_gyro; @@ -141,11 +151,9 @@ private: WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - REQUEST_STOP, - STOPPED, }; - px4::atomic _state{STATE::RESET}; + STATE _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1000}; // 1000 us / 1000 Hz transfer interval uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; diff --git a/src/drivers/imu/invensense/icm20602/icm20602_main.cpp b/src/drivers/imu/invensense/icm20602/icm20602_main.cpp index 3a11950c05..47821c7596 100644 --- a/src/drivers/imu/invensense/icm20602/icm20602_main.cpp +++ b/src/drivers/imu/invensense/icm20602/icm20602_main.cpp @@ -34,120 +34,84 @@ #include "ICM20602.hpp" #include +#include -namespace icm20602 +void +ICM20602::print_usage() { -ICM20602 *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_SPI_BUS_SENSORS) - g_dev = new ICM20602(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_ICM_20602, rotation); -#elif defined(PX4_SPI_BUS_SENSORS1) - g_dev = new ICM20602(PX4_SPI_BUS_SENSORS1, PX4_SPIDEV_ICM_20602, rotation); -#endif - - 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("icm20602", "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(); } -static int stop() +I2CSPIDriverBase *ICM20602::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - if (g_dev == nullptr) { - PX4_WARN("driver not running"); - return -1; + ICM20602 *instance = new ICM20602(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; - g_dev = nullptr; - - return 0; -} - -static int reset() -{ - if (g_dev == nullptr) { - PX4_WARN("driver not running"); - return 0; + if (OK != instance->init()) { + delete instance; + return nullptr; } - return g_dev->Reset(); + return instance; } -static int status() +void ICM20602::custom_method(const BusCLIArguments &cli) { - if (g_dev == nullptr) { - PX4_INFO("driver not running"); - return 0; - } - - g_dev->PrintInfo(); - - return 0; + Reset(); } -static int usage() -{ - PX4_INFO("missing command: try 'start', 'stop', 'reset', 'status'"); - PX4_INFO("options:"); - PX4_INFO(" -R rotation"); - - return 0; -} - -} // namespace icm20602 - extern "C" int icm20602_main(int argc, char *argv[]) { - enum Rotation rotation = ROTATION_NONE; - int myoptind = 1; - int ch = 0; - const char *myoptarg = nullptr; + int ch; + using ThisDriver = ICM20602; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = SPI_SPEED; - // 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 icm20602::usage(); } } - const char *verb = argv[myoptind]; + const char *verb = cli.optarg(); - if (!strcmp(verb, "start")) { - return icm20602::start(rotation); - - } else if (!strcmp(verb, "stop")) { - return icm20602::stop(); - - } else if (!strcmp(verb, "status")) { - return icm20602::status(); - - } else if (!strcmp(verb, "reset")) { - return icm20602::reset(); + if (!verb) { + ThisDriver::print_usage(); + return -1; } - return icm20602::usage(); + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM20602); + + 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); + } + + if (!strcmp(verb, "reset")) { + return ThisDriver::module_custom_method(cli, iterator); + } + + ThisDriver::print_usage(); + return -1; }