fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 10:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
ed8c6019d564c1c8c93cf47a5bfb7a73772fd89f
PX4-Autopilot/src/drivers/power_monitor/ina226/ina226.cpp
T

908 lines
21 KiB
C++
Raw Blame History

/****************************************************************************
*
* 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;
}
Reference in New Issue View Git Blame Copy Permalink