diff --git a/boards/airmind/mindpx-v2/init/rc.board_sensors b/boards/airmind/mindpx-v2/init/rc.board_sensors index 650fb2579e..6dbec0811c 100644 --- a/boards/airmind/mindpx-v2/init/rc.board_sensors +++ b/boards/airmind/mindpx-v2/init/rc.board_sensors @@ -15,7 +15,7 @@ qmc5883 -I -R 12 start mpu6000 -s -R 8 start mpu9250 -s -R 8 start -lsm303d -R 10 start +lsm303d -s -R 10 start l3gd20 -R 14 start # Internal SPI diff --git a/boards/px4/fmu-v2/init/rc.board_sensors b/boards/px4/fmu-v2/init/rc.board_sensors index b7d3afca17..1332f2ba43 100644 --- a/boards/px4/fmu-v2/init/rc.board_sensors +++ b/boards/px4/fmu-v2/init/rc.board_sensors @@ -73,8 +73,8 @@ then # external L3GD20H is rotated 180 degrees yaw l3gd20 -X -R 4 start - # external LSM303D is rotated 270 degrees yaw - lsm303d -X -R 6 start + # (external) LSM303D is rotated 270 degrees yaw + lsm303d -s -R 6 start if [ $BOARD_FMUV3 = 20 ] then @@ -106,7 +106,7 @@ else fi l3gd20 start - lsm303d start + lsm303d -s start fi unset BOARD_FMUV3 diff --git a/boards/px4/fmu-v3/init/rc.board_sensors b/boards/px4/fmu-v3/init/rc.board_sensors index 1c6c8d5bbe..7b32e26a78 100644 --- a/boards/px4/fmu-v3/init/rc.board_sensors +++ b/boards/px4/fmu-v3/init/rc.board_sensors @@ -81,8 +81,8 @@ then # external L3GD20H is rotated 180 degrees yaw l3gd20 -X -R 4 start - # external LSM303D is rotated 270 degrees yaw - lsm303d -X -R 6 start + # (external) LSM303D is rotated 270 degrees yaw + lsm303d -s -R 6 start if [ $BOARD_FMUV3 = 20 ] then @@ -114,7 +114,7 @@ else fi l3gd20 start - lsm303d start + lsm303d -s start fi # External RM3100 diff --git a/src/drivers/imu/lsm303d/LSM303D.cpp b/src/drivers/imu/lsm303d/LSM303D.cpp index 7ee2bd85ae..c3f4305c31 100644 --- a/src/drivers/imu/lsm303d/LSM303D.cpp +++ b/src/drivers/imu/lsm303d/LSM303D.cpp @@ -53,9 +53,10 @@ static constexpr uint8_t _checked_registers[] = { ADDR_CTRL_REG7 }; -LSM303D::LSM303D(int bus, uint32_t device, enum Rotation rotation) : - SPI("LSM303D", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), +LSM303D::LSM303D(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode) : + SPI("LSM303D", 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_DEFAULT, rotation), _px4_mag(get_device_id(), ORB_PRIO_LOW, rotation), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d: acc_read")), @@ -71,9 +72,6 @@ LSM303D::LSM303D(int bus, uint32_t device, enum Rotation rotation) : LSM303D::~LSM303D() { - // make sure we are truly inactive - stop(); - // delete the perf counter perf_free(_accel_sample_perf); perf_free(_mag_sample_perf); @@ -89,8 +87,8 @@ LSM303D::init() int ret = SPI::init(); if (ret != OK) { - PX4_ERR("SPI init failed (%i)", ret); - return PX4_ERROR; + DEVICE_DEBUG("SPI init failed (%i)", ret); + return ret; } reset(); @@ -407,21 +405,12 @@ LSM303D::mag_set_samplerate(unsigned frequency) void LSM303D::start() { - // make sure we are stopped first - stop(); - // start polling at the specified rate ScheduleOnInterval(_call_accel_interval - LSM303D_TIMER_REDUCTION); } void -LSM303D::stop() -{ - ScheduleClear(); -} - -void -LSM303D::Run() +LSM303D::RunImpl() { // make another accel measurement measureAccelerometer(); @@ -581,8 +570,9 @@ LSM303D::measureMagnetometer() } void -LSM303D::print_info() +LSM303D::print_status() { + I2CSPIDriverBase::print_status(); perf_print_counter(_accel_sample_perf); perf_print_counter(_mag_sample_perf); perf_print_counter(_bad_registers); diff --git a/src/drivers/imu/lsm303d/LSM303D.hpp b/src/drivers/imu/lsm303d/LSM303D.hpp index f8a7a9c67b..20268d3310 100644 --- a/src/drivers/imu/lsm303d/LSM303D.hpp +++ b/src/drivers/imu/lsm303d/LSM303D.hpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include #include @@ -138,30 +138,28 @@ */ #define LSM303D_TIMER_REDUCTION 200 -extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } - -class LSM303D : public device::SPI, public px4::ScheduledWorkItem +class LSM303D : public device::SPI, public I2CSPIDriver { public: - LSM303D(int bus, uint32_t device, enum Rotation rotation); - virtual ~LSM303D(); + LSM303D(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode); + ~LSM303D() override; + + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + void RunImpl(); int init() override; - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); + void print_status() override; protected: - virtual int probe(); - + int probe() override; private: - void Run() override; - void start(); - void stop(); void reset(); /** diff --git a/src/drivers/imu/lsm303d/lsm303d_main.cpp b/src/drivers/imu/lsm303d/lsm303d_main.cpp index 91e1764341..692690b0c5 100644 --- a/src/drivers/imu/lsm303d/lsm303d_main.cpp +++ b/src/drivers/imu/lsm303d/lsm303d_main.cpp @@ -39,144 +39,74 @@ #include "LSM303D.hpp" #include - -/** - * Local functions in support of the shell command. - */ -namespace lsm303d -{ - -LSM303D *g_dev; - -void start(bool external_bus, enum Rotation rotation, unsigned range); -void info(); -void usage(); - -/** - * Start the driver. - * - * This function call only returns once the driver is - * up and running or failed to detect the sensor. - */ -void -start(bool external_bus, enum Rotation rotation, unsigned range) -{ - if (g_dev != nullptr) { - errx(0, "already started"); - } - - /* create the driver */ - if (external_bus) { -#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_ACCEL_MAG) - g_dev = new LSM303D(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_ACCEL_MAG, rotation); -#else - errx(0, "External SPI not available"); -#endif - - } else { - g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_ACCEL_MAG, rotation); - } - - if (g_dev == nullptr) { - PX4_ERR("failed instantiating LSM303D obj"); - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; - } - - exit(0); -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - errx(1, "driver start failed"); -} - -/** - * Print a little info about the driver. - */ -void -info() -{ - if (g_dev == nullptr) { - errx(1, "driver not running\n"); - } - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); -} +#include void -usage() +LSM303D::print_usage() { - PX4_INFO("missing command: try 'start', 'info'"); - PX4_INFO("options:"); - PX4_INFO(" -X (external bus)"); - PX4_INFO(" -R rotation"); + PRINT_MODULE_USAGE_NAME("lsm303d", "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(); } -} // namespace - -int -lsm303d_main(int argc, char *argv[]) +I2CSPIDriverBase *LSM303D::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - bool external_bus = false; - enum Rotation rotation = ROTATION_NONE; - int accel_range = 8; + LSM303D *instance = new LSM303D(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, + cli.bus_frequency, cli.spi_mode); - int myoptind = 1; + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (OK != instance->init()) { + delete instance; + return nullptr; + } + + return instance; +} + +extern "C" int lsm303d_main(int argc, char *argv[]) +{ int ch; - const char *myoptarg = nullptr; + using ThisDriver = LSM303D; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = 11 * 1000 * 1000; - /* jump over start/off/etc and look at options first */ - while ((ch = px4_getopt(argc, argv, "XR:a:", &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; - - case 'a': - accel_range = atoi(myoptarg); - break; - - default: - lsm303d::usage(); - exit(0); } } - if (myoptind >= argc) { - lsm303d::usage(); - exit(0); + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; } - const char *verb = argv[myoptind]; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_ACC_DEVTYPE_LSM303D); - /* - * Start/load the driver. - - */ if (!strcmp(verb, "start")) { - lsm303d::start(external_bus, rotation, accel_range); + return ThisDriver::module_start(cli, iterator); } - /* - * Print driver information. - */ - if (!strcmp(verb, "info")) { - lsm303d::info(); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); } - errx(1, "unrecognized command, try 'start', info'"); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; }