diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index 9092efd963..08864a841e 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -20,8 +20,8 @@ param set BAT_CNT_V_CURR 0.001 dataman start mpu9250 -R 8 start -lsm9ds1 -R 4 start -lsm9ds1_mag -R 4 start +lsm9ds1 -s -R 4 start +lsm9ds1_mag -s -R 4 start ms5611 start navio_rgbled start diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index e2e139df56..88bb3abf4d 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -20,8 +20,8 @@ param set BAT_CNT_V_CURR 0.001 dataman start mpu9250 -R 8 start -lsm9ds1 -R 4 start -lsm9ds1_mag -R 4 start +lsm9ds1 -s -R 4 start +lsm9ds1_mag -s -R 4 start ms5611 start navio_rgbled start diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index 06a0b81333..7665a6a2a9 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -20,8 +20,8 @@ param set BAT_CNT_V_CURR 0.001 dataman start mpu9250 -R 8 start -lsm9ds1 -R 4 start -lsm9ds1_mag -R 4 start +lsm9ds1 -s -R 4 start +lsm9ds1_mag -s -R 4 start ms5611 start navio_rgbled start diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp index 869caba079..83b62768e5 100644 --- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp +++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp @@ -38,9 +38,10 @@ using namespace ST_LSM9DS1; static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) | lsb; } -LSM9DS1::LSM9DS1(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())), +LSM9DS1::LSM9DS1(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode) : + SPI(MODULE_NAME, 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_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation) { @@ -54,8 +55,6 @@ LSM9DS1::LSM9DS1(int bus, uint32_t device, enum Rotation rotation) : LSM9DS1::~LSM9DS1() { - Stop(); - perf_free(_interval_perf); perf_free(_transfer_perf); perf_free(_fifo_empty_perf); @@ -77,7 +76,6 @@ int LSM9DS1::probe() bool LSM9DS1::Init() { if (SPI::init() != PX4_OK) { - PX4_ERR("SPI::init failed"); return false; } @@ -186,7 +184,7 @@ void LSM9DS1::Stop() ScheduleClear(); } -void LSM9DS1::Run() +void LSM9DS1::RunImpl() { perf_count(_interval_perf); @@ -312,8 +310,9 @@ void LSM9DS1::Run() } } -void LSM9DS1::PrintInfo() +void LSM9DS1::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/lsm9ds1/LSM9DS1.hpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp index b2a0b3cb7f..057fe21034 100644 --- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp +++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp @@ -50,24 +50,30 @@ #include #include #include +#include using ST_LSM9DS1::Register; -class LSM9DS1 : public device::SPI, public px4::ScheduledWorkItem +class LSM9DS1 : public device::SPI, public I2CSPIDriver { public: - LSM9DS1(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE); + LSM9DS1(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode); ~LSM9DS1() override; + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + void print_status(); + bool Init(); void Start(); void Stop(); bool Reset(); - void PrintInfo(); + void RunImpl(); private: int probe() override; - void Run() override; uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); diff --git a/src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp b/src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp index 42b9933aa5..5c39962649 100644 --- a/src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp +++ b/src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp @@ -34,110 +34,75 @@ #include "LSM9DS1.hpp" #include +#include -namespace lsm9ds1 +I2CSPIDriverBase *LSM9DS1::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { + LSM9DS1 *instance = new LSM9DS1(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, + cli.bus_frequency, cli.spi_mode); -LSM9DS1 *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) && defined(PX4_SPIDEV_LSM9DS1_AG) - g_dev = new LSM9DS1(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_LSM9DS1_AG, rotation); -#endif // PX4_SPI_BUS_SENSORS && PX4_SPIDEV_LSM9DS1_AG - - 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; -} - -static int stop() -{ - if (g_dev == nullptr) { - PX4_WARN("driver not running"); + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; } - g_dev->Stop(); - delete g_dev; - - return 0; -} - -static int status() -{ - if (g_dev == nullptr) { - PX4_WARN("driver not running"); - return -1; + if (!instance->Init()) { + delete instance; + return nullptr; } - g_dev->PrintInfo(); - - return 0; + return instance; } -static void usage() +void +LSM9DS1::print_usage() { - PX4_INFO("missing command: try 'start', 'stop', 'status'"); - PX4_INFO("options:"); - PX4_INFO(" -R rotation"); + PRINT_MODULE_USAGE_NAME("lsm9ds1", "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 lsm9ds1 - extern "C" __EXPORT int lsm9ds1_main(int argc, char *argv[]) { - enum Rotation rotation = ROTATION_NONE; - int myoptind = 1; - int ch = 0; - const char *myoptarg = nullptr; + int ch; + using ThisDriver = LSM9DS1; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = ST_LSM9DS1::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: - lsm9ds1::usage(); - return 0; } } - if (myoptind >= argc) { - lsm9ds1::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_LSM9DS1_AG); if (!strcmp(verb, "start")) { - return lsm9ds1::start(rotation); - - } else if (!strcmp(verb, "stop")) { - return lsm9ds1::stop(); + return ThisDriver::module_start(cli, iterator); + } - } else if (!strcmp(verb, "status")) { - return lsm9ds1::status(); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); } - lsm9ds1::usage(); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } - return 0; + ThisDriver::print_usage(); + return -1; } diff --git a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp index 28d47390e8..1f910ea94a 100644 --- a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp +++ b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp @@ -40,9 +40,10 @@ using namespace time_literals; static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) | lsb; } -LSM9DS1_MAG::LSM9DS1_MAG(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())), +LSM9DS1_MAG::LSM9DS1_MAG(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, + int bus_frequency, spi_mode_e spi_mode) : + SPI(MODULE_NAME, nullptr, bus, device, spi_mode, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _px4_mag(get_device_id(), ORB_PRIO_DEFAULT, rotation) { set_device_type(DRV_MAG_DEVTYPE_ST_LSM9DS1_M); @@ -53,8 +54,6 @@ LSM9DS1_MAG::LSM9DS1_MAG(int bus, uint32_t device, enum Rotation rotation) : LSM9DS1_MAG::~LSM9DS1_MAG() { - Stop(); - perf_free(_interval_perf); perf_free(_transfer_perf); perf_free(_data_overrun_perf); @@ -72,7 +71,6 @@ int LSM9DS1_MAG::probe() bool LSM9DS1_MAG::Init() { if (SPI::init() != PX4_OK) { - PX4_ERR("SPI::init failed"); return false; } @@ -159,7 +157,7 @@ void LSM9DS1_MAG::Stop() ScheduleClear(); } -void LSM9DS1_MAG::Run() +void LSM9DS1_MAG::RunImpl() { perf_count(_interval_perf); @@ -204,8 +202,9 @@ void LSM9DS1_MAG::Run() } } -void LSM9DS1_MAG::PrintInfo() +void LSM9DS1_MAG::print_status() { + I2CSPIDriverBase::print_status(); perf_print_counter(_interval_perf); perf_print_counter(_transfer_perf); perf_print_counter(_data_overrun_perf); diff --git a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.hpp b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.hpp index f62b28d3ed..90f64c9893 100644 --- a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.hpp +++ b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.hpp @@ -48,22 +48,30 @@ #include #include #include +#include -class LSM9DS1_MAG : public device::SPI, public px4::ScheduledWorkItem +class LSM9DS1_MAG : public device::SPI, public I2CSPIDriver { public: - LSM9DS1_MAG(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE); + LSM9DS1_MAG(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode); ~LSM9DS1_MAG() override; + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + void print_status(); + bool Init(); void Start(); void Stop(); bool Reset(); - void PrintInfo(); + + void RunImpl(); private: int probe() override; - void Run() override; uint8_t RegisterRead(ST_LSM9DS1_MAG::Register reg); void RegisterWrite(ST_LSM9DS1_MAG::Register reg, uint8_t value); diff --git a/src/drivers/magnetometer/lsm9ds1_mag/lsm9ds1_mag_main.cpp b/src/drivers/magnetometer/lsm9ds1_mag/lsm9ds1_mag_main.cpp index bf2846bbbb..752e4bdb7b 100644 --- a/src/drivers/magnetometer/lsm9ds1_mag/lsm9ds1_mag_main.cpp +++ b/src/drivers/magnetometer/lsm9ds1_mag/lsm9ds1_mag_main.cpp @@ -34,110 +34,75 @@ #include "LSM9DS1_MAG.hpp" #include +#include -namespace lsm9ds1_mag +void +LSM9DS1_MAG::print_usage() { - -LSM9DS1_MAG *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) && defined(PX4_SPIDEV_LSM9DS1_M) - g_dev = new LSM9DS1_MAG(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_LSM9DS1_M, rotation); -#endif // PX4_SPI_BUS_SENSORS && PX4_SPIDEV_LSM9DS1_M - - 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("lsm9ds1_mag", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); + 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 *LSM9DS1_MAG::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - if (g_dev == nullptr) { - PX4_WARN("driver not running"); - } - - g_dev->Stop(); - delete g_dev; + LSM9DS1_MAG *instance = new LSM9DS1_MAG(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), + cli.rotation, cli.bus_frequency, cli.spi_mode); - return 0; -} - -static int status() -{ - if (g_dev == nullptr) { - PX4_WARN("driver not running"); - return -1; + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; } - g_dev->PrintInfo(); + if (!instance->Init()) { + delete instance; + return nullptr; + } - return 0; + return instance; } -static void usage() -{ - PX4_INFO("missing command: try 'start', 'stop', 'status'"); - PX4_INFO("options:"); - PX4_INFO(" -R rotation"); -} - -} // namespace lsm9ds1_mag extern "C" __EXPORT int lsm9ds1_mag_main(int argc, char *argv[]) { - enum Rotation rotation = ROTATION_NONE; - int myoptind = 1; - int ch = 0; - const char *myoptarg = nullptr; + using ThisDriver = LSM9DS1_MAG; + int ch; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = ST_LSM9DS1_MAG::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: - lsm9ds1_mag::usage(); - return 0; } } - if (myoptind >= argc) { - lsm9ds1_mag::usage(); + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); return -1; } - const char *verb = argv[myoptind]; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_ST_LSM9DS1_M); if (!strcmp(verb, "start")) { - return lsm9ds1_mag::start(rotation); - - } else if (!strcmp(verb, "stop")) { - return lsm9ds1_mag::stop(); + return ThisDriver::module_start(cli, iterator); + } - } else if (!strcmp(verb, "status")) { - return lsm9ds1_mag::status(); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); } - lsm9ds1_mag::usage(); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } - return 0; + ThisDriver::print_usage(); + return -1; }