diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 7fef685fd0..31dc34d542 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -44,6 +44,7 @@ px4_add_board( mkblctrl pca9685 pmw3901 + power_monitor/ina226 #protocol_splitter pwm_input pwm_out_sim diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index b4ae39fe6c..fd871cb0df 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -88,6 +88,7 @@ set(msg_files position_setpoint.msg position_setpoint_triplet.msg power_button_state.msg + power_monitor.msg pwm_input.msg qshell_req.msg qshell_retval.msg diff --git a/msg/power_monitor.msg b/msg/power_monitor.msg new file mode 100644 index 0000000000..a64585fa77 --- /dev/null +++ b/msg/power_monitor.msg @@ -0,0 +1,15 @@ +# power monitor message + +uint64 timestamp # Time since system start (microseconds) + +float32 voltage_v # Voltage in volts, 0 if unknown +float32 current_a # Current in amperes, -1 if unknown +float32 power_w # power in watts, -1 if unknown +int16 rconf +int16 rsv +int16 rbv +int16 rp +int16 rc +int16 rcal +int16 me +int16 al diff --git a/src/drivers/power_monitor/ina226/CMakeLists.txt b/src/drivers/power_monitor/ina226/CMakeLists.txt new file mode 100644 index 0000000000..000d4f432f --- /dev/null +++ b/src/drivers/power_monitor/ina226/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# 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__ina226 + MAIN ina226 + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + SRCS + ina226.cpp + ) diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp new file mode 100644 index 0000000000..45f03a795b --- /dev/null +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -0,0 +1,907 @@ +/**************************************************************************** + * + * 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 ina226.cpp + * @author David Sidrane + * + * Driver for the I2C attached INA226 + */ +#define INA226_RAW // remove this +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include + +#include + +/* Configuration Constants */ + +#define INA226_BUS_DEFAULT PX4_I2C_BUS_EXPANSION +#define INA226_BASEADDR 0x41 /* 7-bit address. 8-bit address is 0x41 */ +#define INA226_DEVICE_PATH "/dev/ina226" + +/* INA226 Registers addresses */ + +#define INA226_REG_CONFIGURATION (0x00) +#define INA226_REG_SHUNTVOLTAGE (0x01) +#define INA226_REG_BUSVOLTAGE (0x02) +#define INA226_REG_POWER (0x03) +#define INA226_REG_CURRENT (0x04) +#define INA226_REG_CALIBRATION (0x05) +#define INA226_REG_MASKENABLE (0x06) +#define INA226_REG_ALERTLIMIT (0x07) +#define INA226_MFG_ID (0xfe) +#define INA226_MFG_DIEID (0xff) + +#define INA226_MFG_ID_TI (0x5449) // TI +#define INA226_MFG_DIE (0x2260) // INA2260 + +/* INA226 Configuration Register */ + +#define INA226_MODE_SHIFTS (0) +#define INA226_MODE_MASK (7 << INA226_MODE_SHIFTS) +#define INA226_MODE_SHUTDOWN (0 << INA226_MODE_SHIFTS) +#define INA226_MODE_SHUNT_TRIG (1 << INA226_MODE_SHIFTS) +#define INA226_MODE_BUS_TRIG (2 << INA226_MODE_SHIFTS) +#define INA226_MODE_SHUNT_BUS_TRIG (3 << INA226_MODE_SHIFTS) +#define INA226_MODE_ADC_OFF (4 << INA226_MODE_SHIFTS) +#define INA226_MODE_SHUNT_CONT (5 << INA226_MODE_SHIFTS) +#define INA226_MODE_BUS_CONT (6 << INA226_MODE_SHIFTS) +#define INA226_MODE_SHUNT_BUS_CONT (7 << INA226_MODE_SHIFTS) + +#define INA226_VSHCT_SHIFTS (3) +#define INA226_VSHCT_MASK (7 << INA226_VSHCT_SHIFTS) +#define INA226_VSHCT_140US (0 << INA226_VSHCT_SHIFTS) +#define INA226_VSHCT_204US (1 << INA226_VSHCT_SHIFTS) +#define INA226_VSHCT_332US (2 << INA226_VSHCT_SHIFTS) +#define INA226_VSHCT_588US (3 << INA226_VSHCT_SHIFTS) +#define INA226_VSHCT_1100US (4 << INA226_VSHCT_SHIFTS) +#define INA226_VSHCT_2116US (5 << INA226_VSHCT_SHIFTS) +#define INA226_VSHCT_4156US (6 << INA226_VSHCT_SHIFTS) +#define INA226_VSHCT_8244US (7 << INA226_VSHCT_SHIFTS) + +#define INA226_VBUSCT_SHIFTS (6) +#define INA226_VBUSCT_MASK (7 << INA226_VBUSCT_SHIFTS) +#define INA226_VBUSCT_140US (0 << INA226_VBUSCT_SHIFTS) +#define INA226_VBUSCT_204US (1 << INA226_VBUSCT_SHIFTS) +#define INA226_VBUSCT_332US (2 << INA226_VBUSCT_SHIFTS) +#define INA226_VBUSCT_588US (3 << INA226_VBUSCT_SHIFTS) +#define INA226_VBUSCT_1100US (4 << INA226_VBUSCT_SHIFTS) +#define INA226_VBUSCT_2116US (5 << INA226_VBUSCT_SHIFTS) +#define INA226_VBUSCT_4156US (6 << INA226_VBUSCT_SHIFTS) +#define INA226_VBUSCT_8244US (7 << INA226_VBUSCT_SHIFTS) + +#define INA226_AVERAGES_SHIFTS (9) +#define INA226_AVERAGES_MASK (7 << INA226_AVERAGES_SHIFTS) +#define INA226_AVERAGES_1 (0 << INA226_AVERAGES_SHIFTS) +#define INA226_AVERAGES_4 (1 << INA226_AVERAGES_SHIFTS) +#define INA226_AVERAGES_16 (2 << INA226_AVERAGES_SHIFTS) +#define INA226_AVERAGES_64 (3 << INA226_AVERAGES_SHIFTS) +#define INA226_AVERAGES_128 (4 << INA226_AVERAGES_SHIFTS) +#define INA226_AVERAGES_256 (5 << INA226_AVERAGES_SHIFTS) +#define INA226_AVERAGES_512 (6 << INA226_AVERAGES_SHIFTS) +#define INA226_AVERAGES_1024 (7 << INA226_AVERAGES_SHIFTS) + +#define INA226_CONFIG (INA226_MODE_SHUNT_BUS_CONT | INA226_VSHCT_588US | INA226_VBUSCT_588US | INA226_AVERAGES_64) + +#define INA226_RST (1 << 15) + +/* INA226 Enable / Mask Register */ + +#define INA226_LEN (1 << 0) +#define INA226_APOL (1 << 1) +#define INA226_OVF (1 << 2) +#define INA226_CVRF (1 << 3) +#define INA226_AFF (1 << 4) + +#define INA226_CNVR (1 << 10) +#define INA226_POL (1 << 11) +#define INA226_BUL (1 << 12) +#define INA226_BOL (1 << 13) +#define INA226_SUL (1 << 14) +#define INA226_SOL (1 << 15) + +#define INA226_CONVERSION_INTERVAL (100000-7) /* 100 ms / 10 Hz */ +#define MAX_CURRENT 164.0f /* 164 Amps */ +#define DN_MAX 32768.0f /* 2^15 */ +#define INA226_CONST 0.00512f /* is an internal fixed value used to ensure scaling is maintained properly */ +#define INA226_SHUNT 0.0005f /* Shunt is 500 uOhm */ +#define INA226_VSCALE 0.00125f /* LSB of voltage is 1.25 mV */ + + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +#define swap16(w) __builtin_bswap16((w)) + + +class INA226 : public device::I2C +{ +public: + INA226(int bus = INA226_BUS_DEFAULT, int address = INA226_BASEADDR); + virtual ~INA226(); + + virtual int init(); + + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + work_s _work{}; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _class_instance; + int _orb_class_instance; + + orb_advert_t _power_monitor_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + + int16_t _bus_volatage; + int16_t _power; + int16_t _current; + int16_t _shunt; + int16_t _cal; + bool _mode_trigged; + + float _max_current = MAX_CURRENT; + float _rshunt = INA226_SHUNT; + uint16_t _config = INA226_CONFIG; + float _current_lsb = _max_current / DN_MAX; + float _power_lsb = 25 * _current_lsb; + + /** + * Test whetpower_monitorhe device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to read or write. + * @return . + */ + int read(uint8_t address); + int write(uint8_t address, uint16_t data); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults INA226_MIN_DISTANCE + * and INA226_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int ina226_main(int argc, char *argv[]); + +INA226::INA226(int bus, int address) : + I2C("INA226", INA226_DEVICE_PATH, bus, address, 100000), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _class_instance(-1), + _orb_class_instance(-1), + _power_monitor_topic(nullptr), + _sample_perf(perf_alloc(PC_ELAPSED, "ina226_read")), + _comms_errors(perf_alloc(PC_COUNT, "ina226_com_err")), + _bus_volatage(0), + _power(-1), + _current(-1), + _shunt(0), + _cal(0), + _mode_trigged(false) +{ + + float fvalue = MAX_CURRENT; + _max_current = fvalue; + param_t ph = param_find("INA226_CURRENT"); + + if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) { + _max_current = fvalue; + } + + fvalue = INA226_SHUNT; + _rshunt = fvalue; + ph = param_find("INA226_SHUNT"); + + if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) { + _rshunt = fvalue; + } + + ph = param_find("INA226_CONFIG"); + int32_t value = INA226_CONFIG; + _config = (uint16_t)value; + + if (ph != PARAM_INVALID && param_get(ph, &value) == PX4_OK) { + _config = (uint16_t)value; + } + + _mode_trigged = ((_config & INA226_MODE_MASK) >> INA226_MODE_SHIFTS) <= + ((INA226_MODE_SHUNT_BUS_TRIG & INA226_MODE_MASK) >> + INA226_MODE_SHIFTS); + + _current_lsb = _max_current / DN_MAX; + _power_lsb = 25 * _current_lsb; +} + +INA226::~INA226() +{ + /* make sure we are truly inactive */ + + stop(); + + if (_class_instance != -1) { + unregister_class_devname(INA226_DEVICE_PATH, _class_instance); + } + + /* free perf counters */ + + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int INA226::read(uint8_t address) +{ + union { + uint16_t reg; + uint8_t b[2] = {}; + } data; + + int ret = transfer(&address, 1, &data.b[0], sizeof(data.b)); + + if (OK != ret) { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + return -1; + } + + return swap16(data.reg); +} + +int INA226::write(uint8_t address, uint16_t value) +{ + uint8_t data[3] = {address, ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)}; + return transfer(data, sizeof(data), nullptr, 0); +} + +int +INA226::init() +{ + int ret = PX4_ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) { + return ret; + } + + write(INA226_REG_CONFIGURATION, INA226_RST); + + _cal = INA226_CONST / (_current_lsb * INA226_SHUNT); + + if (write(INA226_REG_CALIBRATION, _cal) < 0) { + return -3; + } + + // If we run in continuous mode then start it here + + if (!_mode_trigged) { + ret = write(INA226_REG_CONFIGURATION, _config); + } + + set_device_address(INA226_BASEADDR); /* set I2c Address */ + + _class_instance = register_class_devname(INA226_DEVICE_PATH); + + struct power_monitor_s report = {}; + _power_monitor_topic = orb_advertise_multi(ORB_ID(power_monitor), &report, + &_orb_class_instance, ORB_PRIO_LOW); + + if (_power_monitor_topic == nullptr) { + PX4_ERR("failed to create power_monitor object"); + } + + ret = measure(); + _sensor_ok = true; + + return ret; +} + +int +INA226::probe() +{ + int value = read(INA226_MFG_ID); + + if (value < 0) { + perf_count(_comms_errors); + } + + if (value != INA226_MFG_ID_TI) { + PX4_DEBUG("probe mfgid %d", value); + return -1; + } + + value = read(INA226_MFG_DIEID); + + if (value < 0) { + perf_count(_comms_errors); + } + + if (value != INA226_MFG_DIE) { + PX4_DEBUG("probe die id %d", value); + return -1; + } + + return OK; +} +int +INA226::ioctl(device::file_t *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default polling rate */ + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(INA226_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + + } + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + int ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(INA226_CONVERSION_INTERVAL)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +int +INA226::measure() +{ + int ret = OK; + + if (_mode_trigged) { + ret = write(INA226_REG_CONFIGURATION, _config); + + if (ret < 0) { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + } + } + + return ret; +} + +int +INA226::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + perf_begin(_sample_perf); + + ret = _bus_volatage = read(INA226_REG_BUSVOLTAGE); + + if (_bus_volatage >= 0) { + ret = _power = read(INA226_REG_POWER); + + if (_power >= 0) { + ret = _current = read(INA226_REG_CURRENT); + + if (_current >= 0) { + ret = _shunt = read(INA226_REG_SHUNTVOLTAGE); + + if (_shunt >= 0) { + + struct power_monitor_s report; + report.timestamp = hrt_absolute_time(); + report.voltage_v = (float) _bus_volatage * INA226_VSCALE; + report.current_a = (float) _current * _current_lsb; + report.power_w = (float) _power * _power_lsb; +#if defined(INA226_RAW) + report.rconf = read(INA226_REG_CONFIGURATION); + report.rsv = read(INA226_REG_SHUNTVOLTAGE); + report.rbv = read(INA226_REG_BUSVOLTAGE); + report.rp = read(INA226_REG_POWER); + report.rc = read(INA226_REG_CURRENT); + report.rcal = read(INA226_REG_CALIBRATION); + report.me = read(INA226_REG_MASKENABLE); + report.al = read(INA226_REG_ALERTLIMIT); +#endif + + /* publish it, if we are the primary */ + if (_power_monitor_topic != nullptr) { + orb_publish(ORB_ID(power_monitor), _power_monitor_topic, &report); + } + + ret = OK; + perf_end(_sample_perf); + return ret; + } + } + } + } + + PX4_DEBUG("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; +} + +void +INA226::start() +{ + + /* reset the report ring and state machine */ + _collect_phase = false; + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&INA226::cycle_trampoline, this, 5); +} + +void +INA226::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +INA226::cycle_trampoline(void *arg) +{ + + INA226 *dev = (INA226 *)arg; + + dev->cycle(); + +} + +void +INA226::cycle() +{ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + PX4_DEBUG("collection error"); + /* if error restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = !_mode_trigged; + + + if (_measure_ticks > USEC2TICK(INA226_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&INA226::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(INA226_CONVERSION_INTERVAL)); + return; + } + } + + /* Measurement phase */ + + /* Perform measurement */ + if (OK != measure()) { + PX4_DEBUG("measure error ina226"); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&INA226::cycle_trampoline, + this, + USEC2TICK(INA226_CONVERSION_INTERVAL)); +} + +void +INA226::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + printf("poll interval: %u ticks\n", _measure_ticks); +} + +/** + * Local functions in support of the shell command. + */ +namespace ina226 +{ + +INA226 *g_dev; + +int start(); +int start_bus(int i2c_bus); +int stop(); +int test(); +int info(); + +/** + * + * Attempt to start driver on all available I2C busses. + * + * This function will return as soon as the first sensor + * is detected on one of the available busses or if no + * sensors are detected. + * + */ +int +start() +{ + if (g_dev != nullptr) { + PX4_ERR("already started"); + return PX4_ERROR; + } + + for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) { + if (start_bus(i2c_bus_options[i]) == PX4_OK) { + return PX4_OK; + } + } + + return PX4_ERROR; +} + +/** + * Start the driver on a specific bus. + * + * This function only returns if the sensor is up and running + * or could not be detected successfully. + */ +int +start_bus(int i2c_bus) +{ + int fd = -1; + + if (g_dev != nullptr) { + PX4_ERR("already started"); + return PX4_ERROR; + } + + /* create the driver */ + g_dev = new INA226(i2c_bus); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = px4_open(INA226_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + px4_close(fd); + return PX4_OK; + +fail: + + if (fd >= 0) { + px4_close(fd); + } + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + return PX4_ERROR; +} + +/** + * Stop the driver + */ +int +stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + PX4_ERR("driver not running"); + return PX4_ERROR; + } + + return PX4_OK; +} + +int +test() +{ + int h = orb_subscribe(ORB_ID(power_monitor)); + struct power_monitor_s mon; + bool done = false; + + while (!done) { + + bool updated = false; + orb_check(h, &updated); + + if (updated) { + orb_copy(ORB_ID(power_monitor), h, &mon); + print_message(mon); + } + + for (int k = 0; k < 5; k++) { + char c; + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + ret = read(0, &c, 1); + + if (ret) { + done = true; + } + + return 1; + } + + switch (c) { + case 0x03: // ctrl-c + case 0x1b: // esc + case 'c': + case 'q': + done = true; + break; + } + } + + px4_usleep(250000); + } + + orb_unsubscribe(h); + return 0; + +} +/** + * Print a little info about the driver. + */ +int +info() +{ + if (g_dev == nullptr) { + PX4_ERR("driver poll restart failed"); + return PX4_ERROR; + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + return PX4_OK; +} + +} /* namespace */ + + +static void +ina2262_usage() +{ + PX4_INFO("usage: ina226 command [options]"); + PX4_INFO("options:"); + PX4_INFO("\t-b --bus i2cbus (%d)", INA226_BUS_DEFAULT); + PX4_INFO("\t-a --all"); + PX4_INFO("command:"); + PX4_INFO("\tstart|stop|test|info"); +} + +int +ina226_main(int argc, char *argv[]) +{ + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + bool start_all = false; + + int i2c_bus = INA226_BUS_DEFAULT; + + while ((ch = px4_getopt(argc, argv, "ab:R:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + + case 'b': + i2c_bus = atoi(myoptarg); + break; + + case 'a': + start_all = true; + break; + + default: + PX4_WARN("Unknown option!"); + goto out_error; + } + } + + if (myoptind >= argc) { + goto out_error; + } + + /* + * Start/load the driver. + */ + if (!strcmp(argv[myoptind], "start")) { + if (start_all) { + return ina226::start(); + + } else { + return ina226::start_bus(i2c_bus); + } + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[myoptind], "test")) { + return ina226::test(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[myoptind], "stop")) { + return ina226::stop(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { + return ina226::info(); + } + +out_error: + ina2262_usage(); + return PX4_ERROR; +} diff --git a/src/drivers/power_monitor/ina226/ina226_params.c b/src/drivers/power_monitor/ina226/ina226_params.c new file mode 100644 index 0000000000..b97a7baf08 --- /dev/null +++ b/src/drivers/power_monitor/ina226/ina226_params.c @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + + +/** + * INA226 Power Monitor Config + * + * @group Sensors + * @unit u + * @min 0 + * @max 65535 + * @decimal 1 + * @increment 1 +*/ +PARAM_DEFINE_INT32(INA226_CONFIG, 18139); + +/** + * INA226 Power Monitor Max Current + * + * @group Sensors + * @min 0.1 + * @max 200.0 + * @decimal 2 + * @increment 0.1 + */ +PARAM_DEFINE_FLOAT(INA226_CURRENT, 164.0f); + +/** + * INA226 Power Monitor Shunt + * + * @group Sensors + * @min 0.000000001 + * @max 0.1 + * @decimal 10 + * @increment .000000001 + */ +PARAM_DEFINE_FLOAT(INA226_SHUNT, 0.0005f);