diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index 98e27e7f3c3a..b3fa1853a125 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -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 diff --git a/src/drivers/power_monitor/ina226/CMakeLists.txt b/src/drivers/power_monitor/ina226/CMakeLists.txt index 3c71b8ef84af..c80a2bdc7ca5 100644 --- a/src/drivers/power_monitor/ina226/CMakeLists.txt +++ b/src/drivers/power_monitor/ina226/CMakeLists.txt @@ -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 ) diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp index ace5bae5053d..d499f0233c2f 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -39,10 +39,13 @@ */ #define INA226_RAW // remove this +#include "ina226.h" + #include #include #include +#include #include #include #include @@ -51,181 +54,17 @@ #include #include -/* 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_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 (_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) { - - 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; - } - } - } + if (_initialized) { + + _bus_voltage = read(INA226_REG_BUSVOLTAGE); + _power = read(INA226_REG_POWER); + _current = read(INA226_REG_CURRENT); + _shunt = read(INA226_REG_SHUNTVOLTAGE); + + } else { + init(); + + _bus_voltage = -1.0f; + _power = -1.0f; + _current = -1.0f; + _shunt = -1.0f; + } + + parameter_update_s param_update; + + if (_parameters_sub.copy(¶m_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) { - - /* perform collection */ - if (OK != collect()) { - PX4_DEBUG("collection error"); - /* if error restart the measurement state machine */ - start(); - return; - } + if (_initialized) { + if (_collect_phase) { + + /* 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_trigged; + /* next phase is measurement */ + _collect_phase = !_mode_triggered; - if (_measure_interval > INA226_CONVERSION_INTERVAL) { + 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; + /* schedule a fresh cycle call when we are ready to measure again */ + ScheduleDelayed(_measure_interval - 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 */ - ScheduleDelayed(INA226_CONVERSION_INTERVAL); -} - -void -INA226::print_info() -{ - 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; - } + /* Measurement phase */ - for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) { - if (start_bus(i2c_bus_options[i]) == PX4_OK) { - return PX4_OK; + /* Perform measurement */ + if (OK != measure()) { + perf_count(_measure_errors); } - } - - 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; + /* next phase is collection */ + _collect_phase = true; -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; + /* schedule a fresh cycle call when the measurement is done */ + ScheduleDelayed(INA226_CONVERSION_INTERVAL); } else { - PX4_ERR("driver not running"); - return PX4_ERROR; - } - - 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; + _battery.updateBatteryStatus( + hrt_absolute_time(), + 0.0f, + 0.0f, + false, + false, + 0, + 0.0f, + true + ); + + if (init() != OK) { + ScheduleDelayed(INA226_INIT_RETRY_INTERVAL_US); + } } - - 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[]) +void +INA226::print_info() { - 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) { + if (_initialized) { + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); - case 'b': - i2c_bus = atoi(myoptarg); - break; + printf("poll interval: %u \n", _measure_interval); - 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(); + } else { + PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.", + INA226_INIT_RETRY_INTERVAL_US); } - -out_error: - ina2262_usage(); - return PX4_ERROR; } diff --git a/src/drivers/power_monitor/ina226/ina226.h b/src/drivers/power_monitor/ina226/ina226.h new file mode 100644 index 000000000000..1b29aef055a4 --- /dev/null +++ b/src/drivers/power_monitor/ina226/ina226.h @@ -0,0 +1,201 @@ +#pragma once + + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* Configuration Constants */ +#define INA226_BUS_DEFAULT PX4_I2C_BUS_EXPANSION +#define INA226_BASEADDR 0x41 /* 7-bit address. 8-bit address is 0x41 */ +// 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(); + +}; diff --git a/src/drivers/power_monitor/ina226/ina226_main.cpp b/src/drivers/power_monitor/ina226/ina226_main.cpp new file mode 100644 index 000000000000..5cd6e5023b3a --- /dev/null +++ b/src/drivers/power_monitor/ina226/ina226_main.cpp @@ -0,0 +1,308 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 8c2f0219ba85..dda42a056e7f 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -98,6 +98,11 @@ Battery::Battery(int index, ModuleParams *parent) : updateParams(); } +Battery::~Battery() +{ + orb_unadvertise(_orb_advert); +} + void Battery::reset() { diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index b7297171ed1a..ae9c7b039a8c 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -65,6 +65,8 @@ class Battery : public ModuleParams public: Battery(int index, ModuleParams *parent); + ~Battery(); + /** * Reset all battery stats and report invalid/nothing. */