fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

INA226: Integrate with PX4 (#12673)

Browse Source 
* Publish I2C battery data as battery_status
This commit is contained in:
Timothy Scott 2019-12-28 18:30:11 +01:00 committed by Daniel Agar
parent 574b482fdb
commit fc1341208f
7 changed files with 670 additions and 428 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
boards/px4/fmu-v5x/init/rc.board_sensors
View File

@ -6,8 +6,8 @@
adc start
# Start Digital power monitors
ina226 -b 1 start
ina226 -b 2 start
ina226 -b 0 -t 1 -f start
ina226 -b 1 -t 2 -f start
# Internal SPI bus ICM-20602
mpu6000 -R 8 -s -T 20602 start

2
src/drivers/power_monitor/ina226/CMakeLists.txt
View File

@ -36,7 +36,9 @@ px4_add_module(
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
SRCS
ina226_main.cpp
ina226.cpp
DEPENDS
battery
px4_work_queue
)

576
src/drivers/power_monitor/ina226/ina226.cpp
View File

@ -39,10 +39,13 @@
*/
#define INA226_RAW // remove this
#include "ina226.h"
#include <string.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <drivers/device/i2c.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
@ -51,181 +54,17 @@
#include <uORB/topics/power_monitor.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
/* Configuration Constants */
#define INA226_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
#define INA226_BASEADDR 0x41 /* 7-bit address. 8-bit address is 0x41 */
/* 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 */
#define swap16(w) __builtin_bswap16((w))
class INA226 : public device::I2C, px4::ScheduledWorkItem
{
public:
INA226(int bus = INA226_BUS_DEFAULT, int address = INA226_BASEADDR);
virtual ~INA226();
virtual int init();
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
private:
bool _sensor_ok{false};
int _measure_interval{0};
bool _collect_phase{false};
uORB::PublicationMulti<power_monitor_s> _power_monitor_topic{ORB_ID(power_monitor)};
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
int16_t _bus_volatage{0};
int16_t _power{-1};
int16_t _current{-1};
int16_t _shunt{0};
int16_t _cal{0};
bool _mode_trigged{false};
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.0f * _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();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void Run() override;
int measure();
int collect();
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int ina226_main(int argc, char *argv[]);
INA226::INA226(int bus, int address) :
I2C("INA226", nullptr, bus, address, 100000),
INA226::INA226(int battery_index, int bus, int address) :
I2C("INA226", nullptr, i2c_bus_options[bus], address, 100000),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
ModuleParams(nullptr),
index(bus),
_sample_perf(perf_alloc(PC_ELAPSED, "ina226_read")),
_comms_errors(perf_alloc(PC_COUNT, "ina226_com_err"))
_comms_errors(perf_alloc(PC_COUNT, "ina226_com_err")),
_collection_errors(perf_alloc(PC_COUNT, "ina226_collection_err")),
_measure_errors(perf_alloc(PC_COUNT, "ina226_measurement_err")),
_battery(battery_index, this)
{
float fvalue = MAX_CURRENT;
_max_current = fvalue;
@ -251,12 +90,24 @@ INA226::INA226(int bus, int address) :
_config = (uint16_t)value;
}
_mode_trigged = ((_config & INA226_MODE_MASK) >> INA226_MODE_SHIFTS) <=
((INA226_MODE_SHUNT_BUS_TRIG & INA226_MODE_MASK) >>
INA226_MODE_SHIFTS);
_mode_triggered = ((_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;
// We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0
_battery.updateBatteryStatus(
hrt_absolute_time(),
0.0,
0.0,
false,
false, // TODO: selected source?
0,
0.0,
true
);
}
INA226::~INA226()
@ -267,6 +118,8 @@ INA226::~INA226()
/* free perf counters */
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_collection_errors);
perf_free(_measure_errors);
}
int INA226::read(uint8_t address)
@ -313,7 +166,7 @@ INA226::init()
// If we run in continuous mode then start it here
if (!_mode_trigged) {
if (!_mode_triggered) {
ret = write(INA226_REG_CONFIGURATION, _config);
} else {
@ -325,6 +178,17 @@ INA226::init()
start();
_sensor_ok = true;
_initialized = ret == OK;
return ret;
}
int
INA226::force_init()
{
int ret = init();
start();
return ret;
}
@ -361,7 +225,7 @@ INA226::measure()
{
int ret = OK;
if (_mode_trigged) {
if (_mode_triggered) {
ret = write(INA226_REG_CONFIGURATION, _config);
if (ret < 0) {
@ -381,47 +245,70 @@ INA226::collect()
/* read from the sensor */
perf_begin(_sample_perf);
ret = _bus_volatage = read(INA226_REG_BUSVOLTAGE);
if (_initialized) {
if (_bus_volatage >= 0) {
ret = _power = read(INA226_REG_POWER);
_bus_voltage = read(INA226_REG_BUSVOLTAGE);
_power = read(INA226_REG_POWER);
_current = read(INA226_REG_CURRENT);
_shunt = read(INA226_REG_SHUNTVOLTAGE);
if (_power >= 0) {
ret = _current = read(INA226_REG_CURRENT);
} else {
init();
if (_current >= 0) {
ret = _shunt = read(INA226_REG_SHUNTVOLTAGE);
if (_shunt >= 0) {
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 */
_power_monitor_topic.publish(report);
ret = OK;
perf_end(_sample_perf);
return ret;
}
}
}
_bus_voltage = -1.0f;
_power = -1.0f;
_current = -1.0f;
_shunt = -1.0f;
}
parameter_update_s param_update;
if (_parameters_sub.copy(&param_update)) {
// Currently, this INA226 driver doesn't really use ModuleParams. This call to updateParams() is just to
// update the battery, which is registered as a child.
updateParams();
}
// Note: If the power module is connected backwards, then the values of _power, _current, and _shunt will
// be negative but otherwise valid. This isn't important, because why should we support the case where
// the power module is used incorrectly?
if (_bus_voltage >= 0 && _power >= 0 && _current >= 0 && _shunt >= 0) {
_actuators_sub.copy(&_actuator_controls);
/* publish it */
_battery.updateBatteryStatus(
hrt_absolute_time(),
(float) _bus_voltage * INA226_VSCALE,
(float) _current * _current_lsb,
true,
true, // TODO: Determine if this is the selected source
0,
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE],
true
);
ret = OK;
} else {
_battery.updateBatteryStatus(
hrt_absolute_time(),
0.0,
0.0,
false,
false, // TODO: selected source?
0,
0.0,
true
);
ret = -1;
perf_count(_comms_errors);
}
if (ret != OK) {
PX4_DEBUG("error reading from sensor: %d", ret);
}
PX4_DEBUG("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
@ -450,233 +337,70 @@ INA226::stop()
void
INA226::Run()
{
if (_collect_phase) {
if (_initialized) {
if (_collect_phase) {
/* perform collection */
if (OK != collect()) {
PX4_DEBUG("collection error");
/* if error restart the measurement state machine */
start();
return;
/* perform collection */
if (OK != collect()) {
perf_count(_collection_errors);
/* if error restart the measurement state machine */
start();
return;
}
/* next phase is measurement */
_collect_phase = !_mode_triggered;
if (_measure_interval > INA226_CONVERSION_INTERVAL) {
/* schedule a fresh cycle call when we are ready to measure again */
ScheduleDelayed(_measure_interval - INA226_CONVERSION_INTERVAL);
return;
}
}
/* next phase is measurement */
_collect_phase = !_mode_trigged;
/* Measurement phase */
if (_measure_interval > INA226_CONVERSION_INTERVAL) {
/* Perform measurement */
if (OK != measure()) {
perf_count(_measure_errors);
}
/* schedule a fresh cycle call when we are ready to measure again */
ScheduleDelayed(_measure_interval - INA226_CONVERSION_INTERVAL);
return;
/* next phase is collection */
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
ScheduleDelayed(INA226_CONVERSION_INTERVAL);
} else {
_battery.updateBatteryStatus(
hrt_absolute_time(),
0.0f,
0.0f,
false,
false,
0,
0.0f,
true
);
if (init() != OK) {
ScheduleDelayed(INA226_INIT_RETRY_INTERVAL_US);
}
}
/* 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 */
ScheduleDelayed(INA226_CONVERSION_INTERVAL);
}
void
INA226::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
if (_initialized) {
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
printf("poll interval: %u \n", _measure_interval);
}
/**
* Local functions in support of the shell command.
*/
namespace ina226
{
INA226 *g_dev;
int start();
int start_bus(int i2c_bus);
int stop();
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)
{
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;
}
return PX4_OK;
fail:
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;
printf("poll interval: %u \n", _measure_interval);
} else {
PX4_ERR("driver not running");
return PX4_ERROR;
PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.",
INA226_INIT_RETRY_INTERVAL_US);
}
return PX4_OK;
}
/**
* 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);
}
}
/*
* 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;
}

201
src/drivers/power_monitor/ina226/ina226.h Normal file
View File

@ -0,0 +1,201 @@
#pragma once
#include <string.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <drivers/device/i2c.h>
#include <lib/perf/perf_counter.h>
#include <battery/battery.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
/* Configuration Constants */
#define INA226_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
#define INA226_BASEADDR 0x41 /* 7-bit address. 8-bit address is 0x41 */
// If initialization is forced (with the -f flag on the command line), but it fails, the drive will try again to
// connect to the INA226 every this many microseconds
#define INA226_INIT_RETRY_INTERVAL_US 500000
/* 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 */
#define swap16(w) __builtin_bswap16((w))
class INA226 : public device::I2C, px4::ScheduledWorkItem, ModuleParams
{
public:
INA226(int battery_index, int bus = INA226_BUS_DEFAULT, int address = INA226_BASEADDR);
virtual ~INA226();
virtual int init() override;
/**
* Tries to call the init() function. If it fails, then it will schedule to retry again in
* INA226_INIT_RETRY_INTERVAL_US microseconds. It will keep retrying at this interval until initialization succeeds.
*
* @return OK if initialization succeeded on the first try. Negative value otherwise.
*/
int force_init();
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
size_t index;
protected:
virtual int probe() override;
private:
bool _sensor_ok{false};
int _measure_interval{0};
bool _collect_phase{false};
bool _initialized{false};
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _collection_errors;
perf_counter_t _measure_errors;
int16_t _bus_voltage{0};
int16_t _power{-1};
int16_t _current{-1};
int16_t _shunt{0};
int16_t _cal{0};
bool _mode_triggered{false};
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.0f * _current_lsb};
actuator_controls_s _actuator_controls{};
Battery _battery;
uORB::Subscription _actuators_sub{ORB_ID(actuator_controls)};
uORB::Subscription _parameters_sub{ORB_ID(parameter_update)};
/**
* 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();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void Run() override;
int measure();
int collect();
};

308
src/drivers/power_monitor/ina226/ina226_main.cpp Normal file
View File

@ -0,0 +1,308 @@
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <drivers/device/i2c.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <drivers/drv_hrt.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/power_monitor.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include "ina226.h"
#define MAX_I2C_BATTERY_COUNT 2
extern "C" __EXPORT int ina226_main(int argc, char *argv[]);
/**
* Local functions in support of the shell command.
*/
namespace ina226
{
INA226 *g_dev[MAX_I2C_BATTERY_COUNT] = {nullptr};
int start(int i2c_bus, int address);
int start_bus(int i2c_bus, int address, int battery_index, bool force);
int stop(int i2c_bus, int address);
int info(int i2c_bus, int address);
/**
* If a device is already running on the specified bus and address, return the index of that device.
* If not, return the index of the first empty slot (nullptr) in the array.
* @param i2c_bus The bus, as an index in i2c_bus_options
* @param address I2C address of the power monitor
* @return If there is already a device running on the given bus with the given address: Return the index of that device
* If there is not already a device running: Return the index of the first nullptr in the array.
* If there are no empty slots in the array, return -1.
*/
int get_index(int i2c_bus, int address)
{
int first_nullptr = -1;
for (size_t i = 0; i < MAX_I2C_BATTERY_COUNT; i++) {
//PX4_INFO("Checking number %lu", i);
if (g_dev[i] == nullptr) {
if (first_nullptr < 0) {
first_nullptr = i;
}
} else if (g_dev[i]->get_device_bus() == i2c_bus_options[i2c_bus] && g_dev[i]->get_device_address() == address) {
return i;
}
}
return first_nullptr;
}
/**
*
* 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()
{
for (unsigned i2c_bus = 0; i2c_bus < NUM_I2C_BUS_OPTIONS; i2c_bus++) {
int index = get_index(i2c_bus, INA226_BASEADDR);
if (index < 0) {
PX4_ERR("There are already %d instances of INA226 running. No more can be instantiated.",
MAX_I2C_BATTERY_COUNT);
return PX4_ERROR;
} else if (g_dev[index] == nullptr && start_bus(i2c_bus, INA226_BASEADDR, 1, false) == 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 address, int battery_index, bool force)
{
int idx = get_index(i2c_bus, address);
if (idx < 0) {
PX4_ERR("There are already %d instances of INA226 running. No more can be instantiated.",
MAX_I2C_BATTERY_COUNT);
return PX4_ERROR;
}
if (g_dev[idx] != nullptr) {
PX4_ERR("Already started on bus %d, address 0x%02X", i2c_bus, address);
return PX4_ERROR;
}
/* create the driver */
// TODO: Possibly change this to use statically-allocated memory and placement-new?
g_dev[idx] = new INA226(battery_index, i2c_bus, address);
if (g_dev[idx] == nullptr) {
PX4_ERR("Unable to allocate memory for INA226");
goto fail;
}
if (force) {
if (g_dev[idx]->force_init() != OK) {
PX4_INFO("Failed to initialize INA226 on bus %d, but will try again periodically.", i2c_bus);
}
} else if (OK != g_dev[idx]->init()) {
PX4_ERR("Failed to initialize on bus %d, address 0x%02X", i2c_bus, address);
goto fail;
} else {
PX4_INFO("Started INA226 on bus %d", i2c_bus);
}
return PX4_OK;
fail:
if (g_dev[idx] != nullptr) {
delete g_dev[idx];
g_dev[idx] = nullptr;
}
return PX4_ERROR;
}
/**
* Stop the driver
*/
int
stop(int bus, int address)
{
int idx = get_index(bus, address);
if (idx < 0 || g_dev[idx] == nullptr) {
PX4_ERR("Driver not running on bus %d, address 0x%02X", bus, address);
return PX4_ERROR;
} else {
delete g_dev[idx];
g_dev[idx] = nullptr;
return PX4_OK;
}
}
/**
* Print a little info about the driver.
*/
int
info(int bus, int address)
{
bool any_running = false;
for (int i = 0; i < MAX_I2C_BATTERY_COUNT; i++) {
if (g_dev[i] != nullptr) {
any_running = true;
PX4_INFO("Bus %d, address 0x%02X:", (int) g_dev[i]->index, g_dev[i]->get_device_address());
g_dev[i]->print_info();
}
}
if (!any_running) {
PX4_INFO("No devices are running.");
}
return PX4_OK;
}
} /* namespace */
static void
ina2262_usage()
{
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Driver for the INA226 power monitor.
Multiple instances of this driver can run simultaneously, if each instance has a separate bus OR I2C address.
For example, one instance can run on Bus 2, address 0x41, and one can run on Bus 2, address 0x43.
If the INA226 module is not powered, then by default, initialization of the driver will fail. To change this, use
the -f flag. If this flag is set, then if initialization fails, the driver will keep trying to initialize again
every 0.5 seconds. With this flag set, you can plug in a battery after the driver starts, and it will work. Without
this flag set, the battery must be plugged in before starting the driver.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("ina226", "driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start a new instance of the driver");
PRINT_MODULE_USAGE_PARAM_FLAG('a', "If set, try to start the driver on each availabe I2C bus until a module is found", true);
PRINT_MODULE_USAGE_PARAM_FLAG('f', "If initialization fails, keep retrying periodically. Ignored if the -a flag is set. See full driver documentation for more info", true);
PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, NUM_I2C_BUS_OPTIONS - 1, "I2C bus (default: use board-specific bus)", true);
// The module documentation system INSISTS on decimal literals here. So we can't use a #define constant, and
// we can't use hexadecimal.
PRINT_MODULE_USAGE_PARAM_INT('d', 65, 0, UINT8_MAX, "I2C Address (Start with '0x' for hexadecimal)", true);
PRINT_MODULE_USAGE_PARAM_INT('t', 1, 1, 2, "Which battery calibration values should be used (1 or 2)", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop one instance of the driver");
PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, NUM_I2C_BUS_OPTIONS - 1, "I2C bus (default: use board-specific bus)", true);
PRINT_MODULE_USAGE_PARAM_INT('d', 65, 0, UINT8_MAX, "I2C Address (Start with '0x' for hexadecimal)", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Status of every instance of the driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("info", "Status of every instance of the driver");
}
int
ina226_main(int argc, char *argv[])
{
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
bool start_all = false;
bool force = false;
int i2c_bus = INA226_BUS_DEFAULT;
int address = INA226_BASEADDR;
int battery = 1;
while ((ch = px4_getopt(argc, argv, "afb:d:t:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'b':
i2c_bus = atoi(myoptarg);
break;
case 'a':
start_all = true;
break;
case 'f':
force = true;
break;
case 'd':
address = strtol(myoptarg, nullptr, 0);
break;
case 't':
battery = atoi(myoptarg);
break;
default:
PX4_WARN("Unknown option!");
goto out_error;
}
}
if (myoptind >= argc) {
goto out_error;
}
if (address > 255 || address < 0) {
PX4_ERR("Address must be between 0 and 255");
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, address, battery, force);
}
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
return ina226::stop(i2c_bus, address);
}
/*
* Print driver information.
*/
if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
return ina226::info(i2c_bus, address);
}
out_error:
ina2262_usage();
return PX4_ERROR;
}

5
src/lib/battery/battery.cpp
View File

@ -98,6 +98,11 @@ Battery::Battery(int index, ModuleParams *parent) :
updateParams();
}
Battery::~Battery()
{
orb_unadvertise(_orb_advert);
}
void
Battery::reset()
{

2
src/lib/battery/battery.h
View File

@ -65,6 +65,8 @@ class Battery : public ModuleParams
public:
Battery(int index, ModuleParams *parent);
~Battery();
/**
* Reset all battery stats and report invalid/nothing.
*/