diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake
index 7fef685fd0bd0cb0518f21431db5fbe00aff8040..31dc34d54291d65ef79dfe2eb72c974eea7b09a1 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 b4ae39fe6c8b45e0cef9f04fb7d5ba5bae08a018..fd871cb0df8860c06070b94c4a61f9c8cf5e85f8 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 0000000000000000000000000000000000000000..a64585fa77d66f19261bda7bbd0a7b450fe50527
--- /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 0000000000000000000000000000000000000000..000d4f432f8931de777af9e3a41ef0180a996383
--- /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 0000000000000000000000000000000000000000..45f03a795b212a998ddd19126b49cc470a506668
--- /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 <david_s5@usa.net>
+ *
+ * Driver for the I2C attached INA226
+ */
+#define INA226_RAW // remove this
+#include <px4_config.h>
+#include <px4_getopt.h>
+#include <px4_workqueue.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <perf/perf_counter.h>
+
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/power_monitor.h>
+
+#include <board_config.h>
+
+/* 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 0000000000000000000000000000000000000000..b97a7baf0841fbd610eab37c878dbca449b2b9cf
--- /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);