forked from Archive/PX4-Autopilot
Added TI ina226 I2C power monitor (#11755)
This commit is contained in:
David Sidrane
Daniel Agar
parent
d78a842ca8
commit
ed8c6019d5
|
|
@ -44,6 +44,7 @@ px4_add_board(
|
|||
mkblctrl
|
||||
pca9685
|
||||
pmw3901
|
||||
power_monitor/ina226
|
||||
#protocol_splitter
|
||||
pwm_input
|
||||
pwm_out_sim
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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);
|
||||
Loading…
Reference in New Issue