From 5324c37d04e18d15a911d5818e05ff4d19e1bf2f Mon Sep 17 00:00:00 2001 From: Travis Bottalico Date: Tue, 8 Oct 2019 13:19:54 -0700 Subject: [PATCH] Add support for voxlpm (I2C power monitor) --- .../power_monitor/voxlpm/CMakeLists.txt | 43 ++++ src/drivers/power_monitor/voxlpm/voxlpm.cpp | 236 ++++++++++++++++++ src/drivers/power_monitor/voxlpm/voxlpm.hpp | 179 +++++++++++++ .../power_monitor/voxlpm/voxlpm_main.cpp | 223 +++++++++++++++++ 4 files changed, 681 insertions(+) create mode 100644 src/drivers/power_monitor/voxlpm/CMakeLists.txt create mode 100644 src/drivers/power_monitor/voxlpm/voxlpm.cpp create mode 100644 src/drivers/power_monitor/voxlpm/voxlpm.hpp create mode 100644 src/drivers/power_monitor/voxlpm/voxlpm_main.cpp diff --git a/src/drivers/power_monitor/voxlpm/CMakeLists.txt b/src/drivers/power_monitor/voxlpm/CMakeLists.txt new file mode 100644 index 0000000000..3d35bc2589 --- /dev/null +++ b/src/drivers/power_monitor/voxlpm/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__voxlpm + MAIN voxlpm + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + SRCS + voxlpm.cpp + voxlpm_main.cpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.cpp b/src/drivers/power_monitor/voxlpm/voxlpm.cpp new file mode 100644 index 0000000000..6cb7b063bf --- /dev/null +++ b/src/drivers/power_monitor/voxlpm/voxlpm.cpp @@ -0,0 +1,236 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file voxlpm.cpp + * Driver for the VOXL Power Management unit + */ + +#include "voxlpm.hpp" + +/* + * The VOXLPM has two LTC2946 ICs on it. + * 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())), + _sample_perf(perf_alloc(PC_ELAPSED, "voxlpm: sample")), + _bat_pub_topic(nullptr), + _pm_pub_topic(nullptr), + _voltage(0.0f), + _amperage(0.0f) +{ + _ch_type = ch_type; + + if (_ch_type == VOXLPM_CH_TYPE_VBATT) { + _rsense = VOXLPM_RSENSE_VBATT; + + } else { + _rsense = VOXLPM_RSENSE_5VOUT; + } +} + +VOXLPM::~VOXLPM() +{ + // make sure we are truly inactive + stop(); + perf_free(_sample_perf); +} + +int +VOXLPM::init() +{ + int ret = PX4_ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) { + return ret; + } + + write_reg(DEFAULT_CTRLA_REG_VAL, VOXLPM_LTC2946_CTRLA_REG); + write_reg(DEFAULT_CTRLB_REG_VAL, VOXLPM_LTC2946_CTRLB_REG); + + _battery.reset(&_bat_status); + + start(); + + return PX4_OK; +} + +void +VOXLPM::print_info() +{ + perf_print_counter(_sample_perf); + + if (_ch_type == VOXLPM_CH_TYPE_VBATT) { + printf("- type: BATT\n"); + + } else { + printf("- type: P5VDC\n"); + } + + printf(" - voltage: %9.2f VDC \n", (double)_voltage); + printf(" - current: %9.2f ADC \n", (double)_amperage); + printf(" - rsense: %9.6f ohm \n", (double)_rsense); + printf(" - meas interval: %u us \n", _meas_interval); +} + +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() +{ + measure(); +} + +int +VOXLPM::measure() +{ + _voltage = 0.0f; + _amperage = 0.0f; + + uint8_t vraw[2]; + uint8_t iraw[2]; + + perf_begin(_sample_perf); + + hrt_abstime tnow = hrt_absolute_time(); + + int curr_read_ret = read_reg_buf(VOXLPM_LTC2946_DELTA_SENSE_MSB_REG, iraw, sizeof(iraw)); // 0x14 + int volt_read_ret = read_reg_buf(VOXLPM_LTC2946_VIN_MSB_REG, vraw, sizeof(vraw)); // 0x1E + + if ((volt_read_ret == 0) && (curr_read_ret == 0)) { + uint16_t volt16 = (((uint16_t)vraw[0]) << 8) | vraw[1]; // MSB first + volt16 >>= 4; // data is 12 bit and left-aligned + _voltage = (volt16 / VOXLPM_LTC2946_RESOLUTION) * VOXLPM_LTC2946_VFS_SENSE; + + uint16_t curr16 = (((uint16_t)iraw[0]) << 8) | iraw[1]; // MSB first + curr16 >>= 4; // data is 12 bit and left-aligned + _amperage = curr16 / VOXLPM_LTC2946_RESOLUTION * VOXLPM_LTC2946_VFS_DELTA_SENSE / _rsense; + + switch (_ch_type) { + case VOXLPM_CH_TYPE_VBATT: { + _battery.updateBatteryStatus(tnow, _voltage, _amperage, true, true, 0, 0, false, &_bat_status); + + if (_bat_pub_topic != nullptr) { + orb_publish(ORB_ID(battery_status), _bat_pub_topic, &_bat_status); + + } else { + _bat_pub_topic = orb_advertise(ORB_ID(battery_status), &_bat_status); + } + } + + // fallthrough + + case VOXLPM_CH_TYPE_P5VDC: { + memset(&_pm_status, 0x00, sizeof(_pm_status)); + _pm_status.timestamp = tnow; + _pm_status.voltage_v = (float) _voltage; + _pm_status.current_a = (float) _amperage; + + //_pm_pub_topic.power_w = (float) _power * _power_lsb; + + if (_pm_pub_topic != nullptr) { + orb_publish(ORB_ID(power_monitor), _pm_pub_topic, &_pm_status); + + } else { + _pm_pub_topic = orb_advertise(ORB_ID(power_monitor), &_pm_status); + } + } + break; + + } + + } else { + switch (_ch_type) { + case VOXLPM_CH_TYPE_VBATT: { + _battery.updateBatteryStatus(tnow, 0.0, 0.0, true, true, 0, 0, false, &_bat_status); + + if (_bat_pub_topic != nullptr) { + orb_publish(ORB_ID(battery_status), _bat_pub_topic, &_bat_status); + + } else { + _bat_pub_topic = orb_advertise(ORB_ID(battery_status), &_bat_status); + } + } + break; + + default: + break; + } + } + + perf_end(_sample_perf); + + return OK; +} + +uint8_t +VOXLPM::read_reg(uint8_t addr) +{ + uint8_t cmd[2] = { (uint8_t)(addr), 0}; + transfer(&cmd[0], 1, &cmd[1], 1); + + return cmd[1]; +} + +int +VOXLPM::read_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len) +{ + const uint8_t cmd = (uint8_t)(addr); + return transfer(&cmd, sizeof(cmd), buf, len); +} + +int +VOXLPM::write_reg(uint8_t value, uint8_t addr) +{ + uint8_t cmd[2] = { (uint8_t)(addr), value}; + return transfer(cmd, sizeof(cmd), nullptr, 0); +} diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.hpp b/src/drivers/power_monitor/voxlpm/voxlpm.hpp new file mode 100644 index 0000000000..5d068efa4b --- /dev/null +++ b/src/drivers/power_monitor/voxlpm/voxlpm.hpp @@ -0,0 +1,179 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file voxlpm.hpp + * + * Shared defines for the voxlpm (QTY2 LTC2946) driver. + * + * This is roughly what's goin on: + * + * +~~~~~~~~~~~~~~+ + * VBATT -----| RSENSE_VBATT | ----------+---------------------> VBATT TO ESCS + * | +~~~~~~~~~~~~~~+ | + * | | +--------+------+ + * +----+ +----+ | 5V REGULATOR | + * | | +--------+------+ + * | | | +~~~~~~~~~~~~~~+ + * | | +---| RSENSE_5VOUT |---> 5VDC TO COMPUTE + * | | | +~~~~~~~~~~~~~~+ + * | | | | + * V| |A V| |A + * ################# ################# + * # LTC2946, 0x6a # # LTC2946, 0x6b # + * ################# ################# + * + * Publishes: Publishes: + * - ORB_ID(battery_status) + * - ORB_ID(power_monitor) - ORB_ID(power_monitor) + * + */ +#pragma once + +#include +#include + +#include + +#include + +#include +#include +#include + +/* + * Note that these are unshifted addresses. + */ +#define VOXLPM_LTC2946_ADDR_VBATT 0x6a // 0x6a = 0xd4 >> 1 +#define VOXLPM_LTC2946_ADDR_P5VD 0x6b // 0x6b = 0xd6 >> 1 + +#define VOXLPM_LTC2946_CTRLA_REG 0x00 +#define VOXLPM_LTC2946_CTRLB_REG 0x01 + +#define VOXLPM_LTC2946_POWER_MSB2_REG 0x05 +#define VOXLPM_LTC2946_CTRLB_MSG1_REG 0x06 +#define VOXLPM_LTC2946_CTRLB_LSB_REG 0x07 + +#define VOXLPM_LTC2946_DELTA_SENSE_MSB_REG 0x14 +#define VOXLPM_LTC2946_DELTA_SENSE_LSB_REG 0x15 + +#define VOXLPM_LTC2946_VIN_MSB_REG 0x1E +#define VOXLPM_LTC2946_VIN_LSB_REG 0x1F + +/* + * CTRLA (Address 0x00 - LTC2946_CTRLA_REG) + * + * 7 - [ADIN Configuration] + * 0 --> ADIN Measured with Respect to GND + * 6:5 - [Offset Calibratoin Configuration] + * 00 --> Every Conversion + * 4:3 - [Voltage Selection] + * 11 --> SENSE+ + * 2:0 - [Channel Configuration] + * 000 --> Alternate Voltage, Current Measurement + */ +#define DEFAULT_CTRLA_REG_VAL 0x18 + +/* + * CTRLB (Address 0x01 - LTC2946_CTRLA_REG) + * + * 7 - [!ALERT Clear Enable ] + * 0 --> Disable + * 6 - [Shutdown] + * 0 --> Power-Up + * 5 - [Cleared on Read Control] + * 0 --> Registers Not Affected by Reading + * 4 - [Stuck Bus Timeout Auto Wake-Up] + * 0 --> Disable + * 3:2 - [Enable Accumulation] + * 00 --> Accumulate + * 1:0 - [Auto-Reset Mode/Reset] + * 01 --> Enable Auto-Reset + */ +#define DEFAULT_CTRLB_REG_VAL 0x01 + +/* 12 bits */ +#define VOXLPM_LTC2946_RESOLUTION 4095.0f + +/* VFS Full-Scale Voltage, SENSE+ */ +#define VOXLPM_LTC2946_VFS_SENSE 102.4f + +/* VFS Full-Scale Voltage, delta sense */ +#define VOXLPM_LTC2946_VFS_DELTA_SENSE 0.1024f + +/* Power sense resistor for battery current */ +#define VOXLPM_RSENSE_VBATT 0.0005f + +/* Power sense resistor for 5VDC output current */ +#define VOXLPM_RSENSE_5VOUT 0.005f + +enum VOXLPM_CH_TYPE { + VOXLPM_CH_TYPE_VBATT = 0, + VOXLPM_CH_TYPE_P5VDC +}; + +class VOXLPM : public device::I2C, public px4::ScheduledWorkItem +{ +public: + VOXLPM(const char *path, int bus, int address, VOXLPM_CH_TYPE ch_type); + virtual ~VOXLPM(); + + virtual int init(); + void print_info(); + +private: + unsigned _meas_interval{100000}; // 100ms + void Run() override; + void start(); + void stop(); + int measure(); + + perf_counter_t _sample_perf; + + orb_advert_t _bat_pub_topic; + orb_advert_t _pm_pub_topic; + + struct battery_status_s _bat_status; + struct power_monitor_s _pm_status; + + VOXLPM_CH_TYPE _ch_type; + float _voltage; + float _amperage; + float _rsense; + + Battery _battery; + + uint8_t read_reg(uint8_t addr); + int read_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len); + int write_reg(uint8_t value, uint8_t addr); +}; diff --git a/src/drivers/power_monitor/voxlpm/voxlpm_main.cpp b/src/drivers/power_monitor/voxlpm/voxlpm_main.cpp new file mode 100644 index 0000000000..aecd617e2d --- /dev/null +++ b/src/drivers/power_monitor/voxlpm/voxlpm_main.cpp @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include + +#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 +{ +struct voxlpm_chan { + const char *devpath; + uint32_t device; + VOXLPM *dev; +}; + +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); + } + + /* 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; + } + + /* 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; +} + +void +start(enum VOXLPM_BUS busid) +{ + bool started = false; + uint8_t i; + + 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); +} + +void +info() +{ + 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; + + while ((ch = px4_getopt(argc, argv, "XTR", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'X': + busid = VOXLPM_BUS_I2C_EXTERNAL; + break; + + case 'T': + busid = VOXLPM_BUS_I2C_EXTERNAL1; + break; + + case 'R': + busid = VOXLPM_BUS_I2C_EXTERNAL2; + break; + + default: + voxlpm::usage(); + return 0; + } + } + + if (myoptind >= argc) { + voxlpm::usage(); + return -1; + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + voxlpm::start(busid); + } + + if (!strcmp(verb, "info")) { + voxlpm::info(); + } + + voxlpm::usage(); + return -1; +}