diff --git a/boards/modalai/fc-v1/init/rc.board_sensors b/boards/modalai/fc-v1/init/rc.board_sensors index 0c02ef9f0f..2bc2646063 100644 --- a/boards/modalai/fc-v1/init/rc.board_sensors +++ b/boards/modalai/fc-v1/init/rc.board_sensors @@ -6,7 +6,8 @@ adc start # Start Digital power monitors -voxlpm -R start +voxlpm -X -b 3 -T VBATT start +voxlpm -X -b 3 -T P5VDC start # Internal SPI bus ICM-20602 mpu6000 -R 6 -s -T 20602 start diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 6329650f8e..295ee89ac8 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -137,6 +137,7 @@ #define DRV_DIST_DEVTYPE_TERARANGER 0x75 #define DRV_DIST_DEVTYPE_VL53LXX 0x76 #define DRV_POWER_DEVTYPE_INA226 0x77 +#define DRV_POWER_DEVTYPE_VOXLPM 0x78 #define DRV_DEVTYPE_UNUSED 0xff diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.cpp b/src/drivers/power_monitor/voxlpm/voxlpm.cpp index 115e7806c8..f08d176e61 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.cpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.cpp @@ -43,15 +43,15 @@ * Address 0x6A - measures battery voltage and current with a 0.0005 ohm sense resistor * Address 0x6B - measures 5VDC ouptut voltage and current */ -VOXLPM::VOXLPM(const char *path, int bus, int address, VOXLPM_CH_TYPE ch_type) : - I2C("voxlpm", path, bus, address, 400000), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(I2C::get_device_id())), +VOXLPM::VOXLPM(I2CSPIBusOption bus_option, const int bus, int bus_frequency, VOXLPM_CH_TYPE ch_type) : + I2C("voxlpm", nullptr, bus, (ch_type == VOXLPM_CH_TYPE_VBATT) ? VOXLPM_LTC2946_ADDR_VBATT : VOXLPM_LTC2946_ADDR_P5VD, + bus_frequency), ModuleParams(nullptr), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")), + _ch_type(ch_type), _battery(1, this) { - _ch_type = ch_type; - if (_ch_type == VOXLPM_CH_TYPE_VBATT) { _rsense = VOXLPM_RSENSE_VBATT; @@ -62,8 +62,6 @@ VOXLPM::VOXLPM(const char *path, int bus, int address, VOXLPM_CH_TYPE ch_type) : VOXLPM::~VOXLPM() { - // make sure we are truly inactive - stop(); perf_free(_sample_perf); } @@ -88,7 +86,7 @@ VOXLPM::init() } void -VOXLPM::print_info() +VOXLPM::print_status() { perf_print_counter(_sample_perf); @@ -108,22 +106,11 @@ VOXLPM::print_info() void VOXLPM::start() { - /* make sure we are stopped first */ - uint32_t last_meas_interval = _meas_interval; - stop(); - _meas_interval = last_meas_interval; - ScheduleOnInterval(_meas_interval, 1000); } void -VOXLPM::stop() -{ - ScheduleClear(); -} - -void -VOXLPM::Run() +VOXLPM::RunImpl() { measure(); } diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.hpp b/src/drivers/power_monitor/voxlpm/voxlpm.hpp index ce6539f8a2..f1a3f93b65 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.hpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.hpp @@ -63,7 +63,7 @@ #include #include -#include +#include #include @@ -144,22 +144,25 @@ enum VOXLPM_CH_TYPE { VOXLPM_CH_TYPE_P5VDC }; -class VOXLPM : public device::I2C, public px4::ScheduledWorkItem, public ModuleParams +class VOXLPM : public device::I2C, public ModuleParams, public I2CSPIDriver { public: - VOXLPM(const char *path, int bus, int address, VOXLPM_CH_TYPE ch_type); + VOXLPM(I2CSPIBusOption bus_option, const int bus, int bus_frequency, VOXLPM_CH_TYPE ch_type); virtual ~VOXLPM(); - virtual int init(); - void print_info(); + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + virtual int init(); + void print_status() override; + + void RunImpl(); private: - unsigned _meas_interval{100000}; // 100ms - void Run() override; void start(); - void stop(); int measure(); + static constexpr unsigned _meas_interval{100000}; // 100ms perf_counter_t _sample_perf; uORB::PublicationMulti _pm_pub_topic{ORB_ID(power_monitor)}; @@ -167,7 +170,7 @@ private: power_monitor_s _pm_status{}; - VOXLPM_CH_TYPE _ch_type; + const VOXLPM_CH_TYPE _ch_type; float _voltage{0.0f}; float _amperage{0.0f}; float _rsense{0.0f}; diff --git a/src/drivers/power_monitor/voxlpm/voxlpm_main.cpp b/src/drivers/power_monitor/voxlpm/voxlpm_main.cpp index 35350c12a9..1306a82787 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm_main.cpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm_main.cpp @@ -36,188 +36,84 @@ #include "voxlpm.hpp" -enum VOXLPM_BUS { - VOXLPM_BUS_I2C_EXTERNAL, - VOXLPM_BUS_I2C_EXTERNAL1, - VOXLPM_BUS_I2C_EXTERNAL2 -}; - -/** - * Local functions in support of the shell command. - */ -namespace voxlpm +I2CSPIDriverBase *VOXLPM::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { -struct voxlpm_chan { - const char *devpath; - uint32_t device; - VOXLPM *dev; -}; + VOXLPM *instance = new VOXLPM(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, + (VOXLPM_CH_TYPE)cli.type); -struct voxlpm_bus_option { - enum VOXLPM_BUS busid; - uint8_t busnum; - bool external; - struct voxlpm_chan vbat; - struct voxlpm_chan vpwr; -} bus_options[] = { -#if defined(PX4_I2C_BUS_EXPANSION) - { - VOXLPM_BUS_I2C_EXTERNAL, PX4_I2C_BUS_EXPANSION, true, - {"/dev/voxlpm_vbat", VOXLPM_LTC2946_ADDR_VBATT, NULL }, - {"/dev/voxlpm_p5vd", VOXLPM_LTC2946_ADDR_P5VD, NULL} - }, -#endif -#if defined(PX4_I2C_BUS_EXPANSION1) - { - VOXLPM_BUS_I2C_EXTERNAL1, PX4_I2C_BUS_EXPANSION1, true, - {"/dev/voxlpm_vbat", VOXLPM_LTC2946_ADDR_VBATT, NULL }, - {"/dev/voxlpm_p5vd", VOXLPM_LTC2946_ADDR_P5VD, NULL} - }, -#endif -#if defined(PX4_I2C_BUS_EXPANSION2) - { - VOXLPM_BUS_I2C_EXTERNAL2, PX4_I2C_BUS_EXPANSION2, true, - {"/dev/voxlpm_vbat", VOXLPM_LTC2946_ADDR_VBATT, NULL }, - {"/dev/voxlpm_p5vd", VOXLPM_LTC2946_ADDR_P5VD, NULL} - }, -#endif -}; -#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) - - -/** - * Start the driver. - */ -bool -start_bus(struct voxlpm_bus_option &bus) -{ - /* assume if we've got the battery channel we have the output channel as well */ - if (bus.vbat.dev != nullptr) { - PX4_ERR("bus option already started"); - exit(1); + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; } - /* create the battery voltage / current channel */ - bus.vbat.dev = new VOXLPM(bus.vbat.devpath, bus.busnum, bus.vbat.device, VOXLPM_CH_TYPE_VBATT); - - if (bus.vbat.dev == nullptr) { - return false; + if (OK != instance->init()) { + delete instance; + return nullptr; } - /* create the 5VDC output / compute current channel */ - bus.vpwr.dev = new VOXLPM(bus.vpwr.devpath, bus.busnum, bus.vpwr.device, VOXLPM_CH_TYPE_P5VDC); - - if (bus.vbat.dev == nullptr) { - return false; - } - - if (bus.vbat.dev->init() != OK || bus.vpwr.dev->init() != OK) { - delete bus.vbat.dev; - bus.vbat.dev = nullptr; - delete bus.vpwr.dev; - bus.vpwr.dev = nullptr; - return false; - } - - return true; + return instance; } void -start(enum VOXLPM_BUS busid) +VOXLPM::print_usage() { - bool started = false; - uint8_t i; + PRINT_MODULE_USAGE_NAME_SIMPLE("voxlpm", "driver"); - for (i = 0; i < NUM_BUS_OPTIONS; i++) { - if (bus_options[i].busid == busid) { - started = start_bus(bus_options[i]); - break; // one bus only... - } - } - - if (!started) { - PX4_WARN("bus option number is %d", i); - PX4_ERR("driver start failed"); - exit(1); - } - - exit(0); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAM_STRING('T', "VBATT", "VBATT|P5VDC", "Type", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -void -info() +extern "C" int +voxlpm_main(int argc, char *argv[]) { - uint8_t i; - - for (i = 0; i < NUM_BUS_OPTIONS; i++) { - if (bus_options[i].vbat.dev) { - bus_options[i].vbat.dev->print_info(); - } - - if (bus_options[i].vpwr.dev) { - bus_options[i].vpwr.dev->print_info(); - } - } - - exit(0); -} - -void -usage() -{ - PRINT_MODULE_USAGE_NAME_SIMPLE("voxlpm", "command"); - - PRINT_MODULE_USAGE_COMMAND_DESCR("start", "start monitoring"); - PRINT_MODULE_USAGE_COMMAND_DESCR("info", "display info");; - PRINT_MODULE_USAGE_COMMAND_DESCR("-X", "PX4_I2C_BUS_EXPANSION"); - PRINT_MODULE_USAGE_COMMAND_DESCR("-T", "PX4_I2C_BUS_EXPANSION1"); - PRINT_MODULE_USAGE_COMMAND_DESCR("-R", "PX4_I2C_BUS_EXPANSION2 (default)"); -} - -} // namespace - -extern "C" __EXPORT int voxlpm_main(int argc, char *argv[]) -{ - int myoptind = 1; int ch; - const char *myoptarg = nullptr; - enum VOXLPM_BUS busid = VOXLPM_BUS_I2C_EXTERNAL2; + using ThisDriver = VOXLPM; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = 400000; + cli.type = VOXLPM_CH_TYPE_VBATT; - while ((ch = px4_getopt(argc, argv, "XTR", &myoptind, &myoptarg)) != EOF) { + while ((ch = cli.getopt(argc, argv, "T:")) != EOF) { switch (ch) { - case 'X': - busid = VOXLPM_BUS_I2C_EXTERNAL; - break; - case 'T': - busid = VOXLPM_BUS_I2C_EXTERNAL1; - break; + if (strcmp(cli.optarg(), "VBATT") == 0) { + cli.type = VOXLPM_CH_TYPE_VBATT; - case 'R': - busid = VOXLPM_BUS_I2C_EXTERNAL2; - break; + } else if (strcmp(cli.optarg(), "P5VDC") == 0) { + cli.type = VOXLPM_CH_TYPE_P5VDC; - default: - voxlpm::usage(); - return 0; + } else { + PX4_ERR("unknown type"); + return -1; + } + + break; } } - if (myoptind >= argc) { - voxlpm::usage(); + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); return -1; } - const char *verb = argv[myoptind]; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_POWER_DEVTYPE_VOXLPM); if (!strcmp(verb, "start")) { - voxlpm::start(busid); + return ThisDriver::module_start(cli, iterator); } - if (!strcmp(verb, "info")) { - voxlpm::info(); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); } - voxlpm::usage(); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); return -1; }