diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 6261a0ac46..6a1463bbc9 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -77,7 +77,6 @@ #define DRV_IMU_DEVTYPE_MPU9250 0x24 #define DRV_GYR_DEVTYPE_BMI160 0x25 #define DRV_IMU_DEVTYPE_ICM42688P 0x26 - #define DRV_IMU_DEVTYPE_ICM40609D 0x27 #define DRV_RNG_DEVTYPE_MB12XX 0x31 diff --git a/src/drivers/imu/bmi160/bmi160.cpp b/src/drivers/imu/bmi160/bmi160.cpp index b702010b19..b6aa16adc5 100644 --- a/src/drivers/imu/bmi160/bmi160.cpp +++ b/src/drivers/imu/bmi160/bmi160.cpp @@ -49,9 +49,10 @@ const uint8_t BMI160::_checked_registers[BMI160_NUM_CHECKED_REGISTERS] = { BM BMIREG_NV_CONF }; -BMI160::BMI160(int bus, uint32_t device, enum Rotation rotation) : - SPI("BMI160", nullptr, bus, device, SPIDEV_MODE3, BMI160_BUS_SPEED), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(this->get_device_id())), +BMI160::BMI160(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode) : + SPI("BMI160", 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(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), _px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), _accel_reads(perf_alloc(PC_COUNT, "bmi160_accel_read")), @@ -63,15 +64,12 @@ BMI160::BMI160(int bus, uint32_t device, enum Rotation rotation) : _reset_retries(perf_alloc(PC_COUNT, "bmi160_reset_retries")), _duplicates(perf_alloc(PC_COUNT, "bmi160_duplicates")) { - _px4_accel.set_device_type(DRV_ACC_DEVTYPE_BMI160); - _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_BMI160); + _px4_accel.set_device_type(DRV_IMU_DEVTYPE_BMI160); + _px4_gyro.set_device_type(DRV_IMU_DEVTYPE_BMI160); } BMI160::~BMI160() { - /* make sure we are truly inactive */ - stop(); - /* delete the perf counter */ perf_free(_sample_perf); perf_free(_accel_reads); @@ -494,28 +492,12 @@ BMI160::set_gyro_range(unsigned max_dps) void BMI160::start() { - /* make sure we are stopped first */ - stop(); - /* start polling at the specified rate */ ScheduleOnInterval((1_s / BMI160_GYRO_DEFAULT_RATE) - BMI160_TIMER_REDUCTION, 1000); reset(); } -void -BMI160::stop() -{ - ScheduleClear(); -} - -void -BMI160::Run() -{ - /* make another measurement */ - measure(); -} - void BMI160::check_registers(void) { @@ -560,7 +542,7 @@ BMI160::check_registers(void) } void -BMI160::measure() +BMI160::RunImpl() { if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete @@ -690,8 +672,9 @@ BMI160::measure() } void -BMI160::print_info() +BMI160::print_status() { + I2CSPIDriverBase::print_status(); perf_print_counter(_sample_perf); perf_print_counter(_accel_reads); perf_print_counter(_gyro_reads); diff --git a/src/drivers/imu/bmi160/bmi160.hpp b/src/drivers/imu/bmi160/bmi160.hpp index 44b886dfd3..f5b6c88512 100644 --- a/src/drivers/imu/bmi160/bmi160.hpp +++ b/src/drivers/imu/bmi160/bmi160.hpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #define DIR_READ 0x80 @@ -243,33 +243,38 @@ using namespace time_literals; -class BMI160 : public device::SPI, public px4::ScheduledWorkItem +class BMI160 : public device::SPI, public I2CSPIDriver { public: - BMI160(int bus, uint32_t device, enum Rotation rotation); + BMI160(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode); virtual ~BMI160(); - virtual int init(); + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); + int init() override; + + void print_status() override; void print_registers(); // deliberately cause a sensor error void test_error(); + void RunImpl(); + protected: - virtual int probe(); + int probe() override; + void custom_method(const BusCLIArguments &cli) override; private: PX4Accelerometer _px4_accel; PX4Gyroscope _px4_gyro; - uint8_t _whoami; /** whoami result */ + uint8_t _whoami; ///< whoami result unsigned _dlpf_freq; @@ -306,11 +311,6 @@ private: */ void start(); - /** - * Stop automatic measurement. - */ - void stop(); - /** * Reset chip. * @@ -318,13 +318,6 @@ private: */ int reset(); - void Run() override; - - /** - * Fetch measurements from the sensor and update the report buffers. - */ - void measure(); - /** * Read a register from the BMI160 * diff --git a/src/drivers/imu/bmi160/bmi160_main.cpp b/src/drivers/imu/bmi160/bmi160_main.cpp index 61b38cc826..5242ddb3a7 100644 --- a/src/drivers/imu/bmi160/bmi160_main.cpp +++ b/src/drivers/imu/bmi160/bmi160_main.cpp @@ -33,221 +33,108 @@ #include "bmi160.hpp" -/** driver 'main' command */ -extern "C" { __EXPORT int bmi160_main(int argc, char *argv[]); } +#include +#include -/** - * Local functions in support of the shell command. - */ -namespace bmi160 -{ - -BMI160 *g_dev_int; // on internal bus -BMI160 *g_dev_ext; // on external bus - -void start(bool, enum Rotation); -void stop(bool); -void info(bool); -void regdump(bool); -void testerror(bool); -void usage(); - - -/** - * Start the driver. - * - * This function only returns if the driver is up and running - * or failed to detect the sensor. - */ void -start(bool external_bus, enum Rotation rotation) +BMI160::print_usage() { - BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + PRINT_MODULE_USAGE_NAME("bmi160", "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_COMMAND("regdump"); + PRINT_MODULE_USAGE_COMMAND("testerror"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} - if (*g_dev_ptr != nullptr) - /* if already started, the still command succeeded */ - { - errx(0, "already started"); +I2CSPIDriverBase *BMI160::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) +{ + BMI160 *instance = new BMI160(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, + cli.bus_frequency, cli.spi_mode); + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; } - /* create the driver */ - if (external_bus) { -#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI) - *g_dev_ptr = new BMI160(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_BMI, rotation); -#else - errx(0, "External SPI not available"); -#endif - - } else { -#if defined(PX4_SPIDEV_BMI) - *g_dev_ptr = new BMI160(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_BMI, rotation); -#else - errx(0, "No Internal SPI CS"); -#endif + if (OK != instance->init()) { + delete instance; + return nullptr; } - if (*g_dev_ptr == nullptr) { - goto fail; - } - - if (OK != (*g_dev_ptr)->init()) { - goto fail; - } - - exit(0); -fail: - - if (*g_dev_ptr != nullptr) { - delete (*g_dev_ptr); - *g_dev_ptr = nullptr; - } - - errx(1, "driver start failed"); + return instance; } void -stop(bool external_bus) +BMI160::custom_method(const BusCLIArguments &cli) { - BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + switch (cli.custom1) { + case 0: reset(); + break; - if (*g_dev_ptr != nullptr) { - delete *g_dev_ptr; - *g_dev_ptr = nullptr; + case 1: print_registers(); + break; - } else { - /* warn, but not an error */ - warnx("already stopped."); + case 2: test_error(); + break; } - - exit(0); } -/** - * Print a little info about the driver. - */ -void -info(bool external_bus) +extern "C" int bmi160_main(int argc, char *argv[]) { - BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; - - if (*g_dev_ptr == nullptr) { - errx(1, "driver not running"); - } - - printf("state @ %p\n", *g_dev_ptr); - (*g_dev_ptr)->print_info(); - - exit(0); -} - -/** - * Dump the register information - */ -void -regdump(bool external_bus) -{ - BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; - - if (*g_dev_ptr == nullptr) { - errx(1, "driver not running"); - } - - printf("regdump @ %p\n", *g_dev_ptr); - (*g_dev_ptr)->print_registers(); - - exit(0); -} - - -/** - * deliberately produce an error to test recovery - */ -void -testerror(bool external_bus) -{ - BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; - - if (*g_dev_ptr == nullptr) { - errx(1, "driver not running"); - } - - (*g_dev_ptr)->test_error(); - - exit(0); -} - -void -usage() -{ - warnx("missing command: try 'start', 'info', 'stop', 'regdump', 'testerror'"); - warnx("options:"); - warnx(" -X (external bus)"); - warnx(" -R rotation"); -} - -} // namespace - -int -bmi160_main(int argc, char *argv[]) -{ - int myoptind = 1; int ch; - const char *myoptarg = nullptr; - bool external_bus = false; - enum Rotation rotation = ROTATION_NONE; + using ThisDriver = BMI160; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = BMI160_BUS_SPEED; - while ((ch = px4_getopt(argc, argv, "XR:", &myoptind, &myoptarg)) != EOF) { + while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { switch (ch) { - case 'X': - external_bus = true; - break; - case 'R': - rotation = (enum Rotation)atoi(myoptarg); + cli.rotation = (enum Rotation)atoi(cli.optarg()); break; - - default: - bmi160::usage(); - return 0; } } - if (myoptind >= argc) { - bmi160::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_BMI160); - /* - * Start/load the driver. - */ if (!strcmp(verb, "start")) { - bmi160::start(external_bus, rotation); + return ThisDriver::module_start(cli, iterator); } if (!strcmp(verb, "stop")) { - bmi160::stop(external_bus); + return ThisDriver::module_stop(iterator); } - /* - * Print driver information. - */ - if (!strcmp(verb, "info")) { - bmi160::info(external_bus); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + if (!strcmp(verb, "reset")) { + cli.custom1 = 0; + return ThisDriver::module_custom_method(cli, iterator); } - /* - * Print register information. - */ if (!strcmp(verb, "regdump")) { - bmi160::regdump(external_bus); + cli.custom1 = 1; + return ThisDriver::module_custom_method(cli, iterator); } if (!strcmp(verb, "testerror")) { - bmi160::testerror(external_bus); + cli.custom1 = 2; + return ThisDriver::module_custom_method(cli, iterator); } - bmi160::usage(); + ThisDriver::print_usage(); return -1; }