diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 1013f2781b..413d7c4c8b 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -143,7 +143,8 @@ #define DRV_DIST_DEVTYPE_TERARANGER 0x75 #define DRV_DIST_DEVTYPE_VL53L0X 0x76 #define DRV_POWER_DEVTYPE_INA226 0x77 -#define DRV_POWER_DEVTYPE_VOXLPM 0x78 +#define DRV_POWER_DEVTYPE_INA228 0x78 +#define DRV_POWER_DEVTYPE_VOXLPM 0x79 #define DRV_LED_DEVTYPE_RGBLED 0x7a #define DRV_LED_DEVTYPE_RGBLED_NCP5623C 0x7b diff --git a/src/drivers/power_monitor/ina228/CMakeLists.txt b/src/drivers/power_monitor/ina228/CMakeLists.txt new file mode 100644 index 0000000000..32af49b463 --- /dev/null +++ b/src/drivers/power_monitor/ina228/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2021 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__ina228 + MAIN ina228 + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + SRCS + ina228_main.cpp + ina228.cpp + DEPENDS + battery + px4_work_queue + ) diff --git a/src/drivers/power_monitor/ina228/ina228.cpp b/src/drivers/power_monitor/ina228/ina228.cpp new file mode 100644 index 0000000000..d6540887c0 --- /dev/null +++ b/src/drivers/power_monitor/ina228/ina228.cpp @@ -0,0 +1,419 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 ina228.cpp + * @author David Sidrane + * + * Driver for the I2C attached INA228 + */ + +#include "ina228.h" + + +INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) : + I2C(config), + ModuleParams(nullptr), + I2CSPIDriver(config), + _sample_perf(perf_alloc(PC_ELAPSED, "ina228_read")), + _comms_errors(perf_alloc(PC_COUNT, "ina228_com_err")), + _collection_errors(perf_alloc(PC_COUNT, "ina228_collection_err")), + _measure_errors(perf_alloc(PC_COUNT, "ina228_measurement_err")), + _battery(battery_index, this, INA228_SAMPLE_INTERVAL_US) +{ + float fvalue = MAX_CURRENT; + _max_current = fvalue; + param_t ph = param_find("INA228_CURRENT"); + + if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) { + _max_current = fvalue; + } + + _range = _max_current > (MAX_CURRENT - 1.0f) ? INA228_ADCRANGE_HIGH : INA228_ADCRANGE_LOW; + + fvalue = INA228_SHUNT; + _rshunt = fvalue; + ph = param_find("INA228_SHUNT"); + + if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) { + _rshunt = fvalue; + } + + ph = param_find("INA228_CONFIG"); + int32_t value = INA228_ADCCONFIG; + _config = (uint16_t)value; + + if (ph != PARAM_INVALID && param_get(ph, &value) == PX4_OK) { + _config = (uint16_t)value; + } + + _mode_triggered = ((_config & INA228_MODE_MASK) >> INA228_MODE_SHIFTS) <= + ((INA228_MODE_TEMP_SHUNT_BUS_TRIG & INA228_MODE_MASK) >> + INA228_MODE_SHIFTS); + + _current_lsb = _max_current / DN_MAX; + _power_lsb = 3.2f * _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, + battery_status_s::BATTERY_SOURCE_POWER_MODULE, + 0, + 0.0 + ); +} + +INA228::~INA228() +{ + /* free perf counters */ + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_collection_errors); + perf_free(_measure_errors); +} + +int INA228::read(uint8_t address, int16_t &data) +{ + // read desired little-endian value via I2C + int16_t received_bytes; + const int ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes)); + + if (ret == PX4_OK) { + data = swap16(received_bytes); + + } else { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + } + + return ret; +} + +int INA228::read(uint8_t address, int32_t &data) +{ + // read desired 24 bit value via I2C + int32_t received_bytes{0}; + const int ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes) - 1); + + if (ret == PX4_OK) { + data = swap32(received_bytes) >> ((32 - 24) + 4); + + } else { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + } + + return ret; +} + +int INA228::read(uint8_t address, int64_t &data) +{ + // read desired 40 bit little-endian value via I2C + int64_t received_bytes{0}; + const int ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes) - 3); + + if (ret == PX4_OK) { + data = swap64(received_bytes); + + } else { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + } + + return ret; +} + +int INA228::read(uint8_t address, uint16_t &data) +{ + // read desired little-endian value via I2C + uint16_t received_bytes; + const int ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes)); + + if (ret == PX4_OK) { + data = swap16(received_bytes); + + } else { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + } + + return ret; +} + +int INA228::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 INA228::write(uint8_t address, int16_t value) +{ + uint8_t data[3] = {address, ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)}; + return transfer(data, sizeof(data), nullptr, 0); +} + +int INA228::write(uint8_t address, int32_t value) +{ + uint8_t data[4] = {address, ((uint8_t)((value & 0xff0000) >> 16)), ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)}; + return transfer(data, sizeof(data), nullptr, 0); +} + +int INA228::write(uint8_t address, int64_t value) +{ + uint8_t data[6] = {address, ((uint8_t)((value & 0xff000000) >> 32)), ((uint8_t)((value & 0xff0000) >> 24)), ((uint8_t)((value & 0xff00) >> 16)), ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)}; + return transfer(data, sizeof(data), nullptr, 0); +} + +int +INA228::init() +{ + int ret = PX4_ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != PX4_OK) { + return ret; + } + + write(INA228_REG_CONFIG, (uint16_t)(INA228_RST_RESET | _range)); + + _cal = INA228_CONST * _current_lsb * _rshunt; + + if (_range == INA228_ADCRANGE_LOW) { + _cal *= 4; + } + + if (write(INA228_REG_SHUNTCAL, _cal) < 0) { + return -3; + } + + // Set the CONFIG for max I + + write(INA228_REG_CONFIG, (uint16_t) _range); + + // If we run in continuous mode then start it here + + + if (!_mode_triggered) { + + ret = write(INA228_REG_ADCCONFIG, _config); + + } else { + ret = PX4_OK; + } + + start(); + _sensor_ok = true; + + _initialized = ret == PX4_OK; + return ret; +} + +int +INA228::force_init() +{ + int ret = init(); + + start(); + + return ret; +} + +int +INA228::probe() +{ + uint16_t value{0}; + + if (read(INA228_MANUFACTURER_ID, value) != PX4_OK || value != INA228_MFG_ID_TI) { + PX4_DEBUG("probe mfgid %d", value); + return -1; + } + + if (read(INA228_DEVICE_ID, value) != PX4_OK || INA228_DEVICEID(value) != INA228_MFG_DIE) { + PX4_DEBUG("probe die id %d", value); + return -1; + } + + return PX4_OK; +} + +int +INA228::measure() +{ + int ret = PX4_OK; + + if (_mode_triggered) { + ret = write(INA228_REG_ADCCONFIG, _config); + + if (ret < 0) { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + } + } + + return ret; +} + +int +INA228::collect() +{ + perf_begin(_sample_perf); + + if (_parameter_update_sub.updated()) { + // Read from topic to clear updated flag + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); + + updateParams(); + } + + // read from the sensor + // Note: If the power module is connected backwards, then the values of _power, _current, and _shunt will be negative but otherwise valid. + bool success{true}; + success = success && (read(INA228_REG_VSBUS, _bus_voltage) == PX4_OK); + // success = success && (read(INA228_REG_POWER, _power) == PX4_OK); + success = success && (read(INA228_REG_CURRENT, _current) == PX4_OK); + //success = success && (read(INA228_REG_VSHUNT, _shunt) == PX4_OK); + + if (!success) { + PX4_DEBUG("error reading from sensor"); + _bus_voltage = _power = _current = _shunt = 0; + } + + _actuators_sub.copy(&_actuator_controls); + + _battery.updateBatteryStatus( + hrt_absolute_time(), + (float) _bus_voltage * INA228_VSCALE, + (float) _current * _current_lsb, + success, + battery_status_s::BATTERY_SOURCE_POWER_MODULE, + 0, + _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] + ); + + perf_end(_sample_perf); + + if (success) { + return PX4_OK; + + } else { + return PX4_ERROR; + } +} + +void +INA228::start() +{ + ScheduleClear(); + + /* reset the report ring and state machine */ + _collect_phase = false; + + _measure_interval = INA228_CONVERSION_INTERVAL; + + /* schedule a cycle to start things */ + ScheduleDelayed(5); +} + +void +INA228::RunImpl() +{ + if (_initialized) { + if (_collect_phase) { + /* perform collection */ + if (collect() != PX4_OK) { + perf_count(_collection_errors); + /* if error restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = !_mode_triggered; + + if (_measure_interval > INA228_CONVERSION_INTERVAL) { + /* schedule a fresh cycle call when we are ready to measure again */ + ScheduleDelayed(_measure_interval - INA228_CONVERSION_INTERVAL); + return; + } + } + + /* Measurement phase */ + + /* Perform measurement */ + if (measure() != PX4_OK) { + perf_count(_measure_errors); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + ScheduleDelayed(INA228_CONVERSION_INTERVAL); + + } else { + _battery.updateBatteryStatus( + hrt_absolute_time(), + 0.0f, + 0.0f, + false, + battery_status_s::BATTERY_SOURCE_POWER_MODULE, + 0, + 0.0f + ); + + if (init() != PX4_OK) { + ScheduleDelayed(INA228_INIT_RETRY_INTERVAL_US); + } + } +} + +void +INA228::print_status() +{ + I2CSPIDriverBase::print_status(); + + if (_initialized) { + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + + printf("poll interval: %u \n", _measure_interval); + + } else { + PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.", + INA228_INIT_RETRY_INTERVAL_US / 1000); + } +} diff --git a/src/drivers/power_monitor/ina228/ina228.h b/src/drivers/power_monitor/ina228/ina228.h new file mode 100644 index 0000000000..5629ab858e --- /dev/null +++ b/src/drivers/power_monitor/ina228/ina228.h @@ -0,0 +1,384 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 ina228.h + * + */ + +#pragma once + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +/* Configuration Constants */ +#define INA228_BASEADDR 0x45 /* 7-bit address. 8-bit address is 0x45 */ +// If initialization is forced (with the -f flag on the command line), but it fails, the drive will try again to +// connect to the INA228 every this many microseconds +#define INA228_INIT_RETRY_INTERVAL_US 500000 + +/* INA228 Registers addresses */ +#define INA228_REG_CONFIG (0x00) +#define INA228_REG_ADCCONFIG (0x01) +#define INA228_REG_SHUNTCAL (0x02) +#define INA228_REG_SHUNTTEMPCO (0x03) +#define INA228_REG_VSHUNT (0x04) +#define INA228_REG_VSBUS (0x05) +#define INA228_REG_DIETEMP (0x06) +#define INA228_REG_CURRENT (0x07) +#define INA228_REG_POWER (0x08) +#define INA228_REG_ENERGY (0x09) +#define INA228_REG_CHARGE (0x0a) +#define INA228_REG_DIAG_ALRT (0x0b) +#define INA228_REG_SOVL (0x0c) +#define INA228_REG_SUVL (0x0d) +#define INA228_REG_BOVL (0x0e) +#define INA228_REG_BUVL (0x0f) +#define INA228_REG_TEMP_LIMIT (0x10) +#define INA228_REG_TPWR_LIMIT (0x11) +#define INA228_MANUFACTURER_ID (0x3e) +#define INA228_DEVICE_ID (0x3f) + +#define INA228_MFG_ID_TI (0x5449) // TI +#define INA228_MFG_DIE (0x228) // INA228 + +/* INA228 Configuration (CONFIG) 16-bit Register (Address = 0h) [reset = 0h] */ +#define INA228_ADCRANGE_SHIFTS (4) +#define INA228_ADCRANGE_MASK (1 << INA228_ADCRANGE_SHIFTS) +# define INA228_ADCRANGE_LOW (1 << INA228_ADCRANGE_SHIFTS) // ± 40.96 mV +# define INA228_ADCRANGE_HIGH (0 << INA228_ADCRANGE_SHIFTS) // ±163.84 mV +#define INA228_TEMPCOMP_SHIFTS (5) +#define INA228_TEMPCOMP_MASK (1 << INA228_TEMPCOMP_SHIFTS) +# define INA228_TEMPCOMP_ENABLE (1 << INA228_TEMPCOMP_SHIFTS) +# define INA228_TEMPCOMP_DISABLE (0 << INA228_TEMPCOMP_SHIFTS) + +#define INA228_CONVDLY_SHIFTS (6) +#define INA228_CONVDLY_MASK (0xff << INA228_CONVDLY_SHIFTS) +# define INA228_CONVDLY2MS(n) ((n) << INA228_CONVDLY_SHIFTS) + +#define INA228_RSTACC_SHIFTS (14) +#define INA228_RSTACC_MASK (1 << INA228_RSTACC_SHIFTS) +# define INA228_RSTACC_CLEAR (1 << INA228_RSTACC_SHIFTS) +# define INA228_RSTACC_NORMAL (0 << INA228_RSTACC_SHIFTS) + +#define INA228_RST_SHIFTS (15) +#define INA228_RST_MASK (1 << INA228_RST_SHIFTS) +# define INA228_RST_RESET (1 << INA228_RST_SHIFTS) +# define INA228_RST_NORMAL (0 << INA228_RST_SHIFTS) + +/* INA228 ADC Configuration (ADC_CONFIG) 16-bit Register (Address = 1h) [reset = FB68h] */ + +#define INA228_MODE_SHIFTS (12) +#define INA228_MODE_MASK (0xf << INA228_MODE_SHIFTS) +#define INA228_MODE_SHUTDOWN_TRIG (0 << INA228_MODE_SHIFTS) +#define INA228_MODE_BUS_TRIG (1 << INA228_MODE_SHIFTS) +#define INA228_MODE_SHUNT_TRIG (2 << INA228_MODE_SHIFTS) +#define INA228_MODE_SHUNT_BUS_TRIG (3 << INA228_MODE_SHIFTS) +#define INA228_MODE_TEMP_TRIG (4 << INA228_MODE_SHIFTS) +#define INA228_MODE_TEMP_BUS_TRIG (5 << INA228_MODE_SHIFTS) +#define INA228_MODE_TEMP_SHUNT_TRIG (6 << INA228_MODE_SHIFTS) +#define INA228_MODE_TEMP_SHUNT_BUS_TRIG (7 << INA228_MODE_SHIFTS) + +#define INA228_MODE_SHUTDOWN_CONT (8 << INA228_MODE_SHIFTS) +#define INA228_MODE_BUS_CONT (9 << INA228_MODE_SHIFTS) +#define INA228_MODE_SHUNT_CONT (10 << INA228_MODE_SHIFTS) +#define INA228_MODE_SHUNT_BUS_CONT (11 << INA228_MODE_SHIFTS) +#define INA228_MODE_TEMP_CONT (12 << INA228_MODE_SHIFTS) +#define INA228_MODE_TEMP_BUS_CONT (13 << INA228_MODE_SHIFTS) +#define INA228_MODE_TEMP_SHUNT_CONT (14 << INA228_MODE_SHIFTS) +#define INA228_MODE_TEMP_SHUNT_BUS_CONT (15 << INA228_MODE_SHIFTS) + +#define INA228_VBUSCT_SHIFTS (9) +#define INA228_VBUSCT_MASK (7 << INA228_VBUSCT_SHIFTS) +#define INA228_VBUSCT_50US (0 << INA228_VBUSCT_SHIFTS) +#define INA228_VBUSCT_84US (1 << INA228_VBUSCT_SHIFTS) +#define INA228_VBUSCT_150US (2 << INA228_VBUSCT_SHIFTS) +#define INA228_VBUSCT_280US (3 << INA228_VBUSCT_SHIFTS) +#define INA228_VBUSCT_540US (4 << INA228_VBUSCT_SHIFTS) +#define INA228_VBUSCT_1052US (5 << INA228_VBUSCT_SHIFTS) +#define INA228_VBUSCT_2074US (6 << INA228_VBUSCT_SHIFTS) +#define INA228_VBUSCT_4170US (7 << INA228_VBUSCT_SHIFTS) + +#define INA228_VSHCT_SHIFTS (6) +#define INA228_VSHCT_MASK (7 << INA228_VSHCT_SHIFTS) +#define INA228_VSHCT_50US (0 << INA228_VSHCT_SHIFTS) +#define INA228_VSHCT_84US (1 << INA228_VSHCT_SHIFTS) +#define INA228_VSHCT_150US (2 << INA228_VSHCT_SHIFTS) +#define INA228_VSHCT_280US (3 << INA228_VSHCT_SHIFTS) +#define INA228_VSHCT_540US (4 << INA228_VSHCT_SHIFTS) +#define INA228_VSHCT_1052US (5 << INA228_VSHCT_SHIFTS) +#define INA228_VSHCT_2074US (6 << INA228_VSHCT_SHIFTS) +#define INA228_VSHCT_4170US (7 << INA228_VSHCT_SHIFTS) + +#define INA228_VTCT_SHIFTS (3) +#define INA228_VTCT_MASK (7 << INA228_VTCT_SHIFTS) +#define INA228_VTCT_50US (0 << INA228_VTCT_SHIFTS) +#define INA228_VTCT_84US (1 << INA228_VTCT_SHIFTS) +#define INA228_VTCT_150US (2 << INA228_VTCT_SHIFTS) +#define INA228_VTCT_280US (3 << INA228_VTCT_SHIFTS) +#define INA228_VTCT_540US (4 << INA228_VTCT_SHIFTS) +#define INA228_VTCT_1052US (5 << INA228_VTCT_SHIFTS) +#define INA228_VTCT_2074US (6 << INA228_VTCT_SHIFTS) +#define INA228_VTCT_4170US (7 << INA228_VTCT_SHIFTS) + +#define INA228_AVERAGES_SHIFTS (0) +#define INA228_AVERAGES_MASK (7 << INA228_AVERAGES_SHIFTS) +#define INA228_AVERAGES_1 (0 << INA228_AVERAGES_SHIFTS) +#define INA228_AVERAGES_4 (1 << INA228_AVERAGES_SHIFTS) +#define INA228_AVERAGES_16 (2 << INA228_AVERAGES_SHIFTS) +#define INA228_AVERAGES_64 (3 << INA228_AVERAGES_SHIFTS) +#define INA228_AVERAGES_128 (4 << INA228_AVERAGES_SHIFTS) +#define INA228_AVERAGES_256 (5 << INA228_AVERAGES_SHIFTS) +#define INA228_AVERAGES_512 (6 << INA228_AVERAGES_SHIFTS) +#define INA228_AVERAGES_1024 (7 << INA228_AVERAGES_SHIFTS) + +#define INA228_ADCCONFIG (INA228_MODE_TEMP_SHUNT_BUS_CONT | INA228_VBUSCT_540US | INA228_VSHCT_540US | INA228_VTCT_540US |INA228_AVERAGES_64) + +/* INA228 Shunt Calibration (SHUNT_CAL) 16-bit Register (Address = 2h) [reset = 1000h] */ + +#define INA228_CURRLSB_SHIFTS (0) +#define INA228_CURRLSB_MASK (0x7fff << INA228_CURRLSB_SHIFTS) + +/* INA228 Shunt Temperature Coefficient (SHUNT_TEMPCO) 16-bit Register (Address = 3h) [reset = 0h] */ + +#define INA228_TEMPCO_SHIFTS (0) +#define INA228_TEMPCO_MASK (0x1fff << INA228_TEMPCO_SHIFTS) + +/* INA228 Shunt Voltage Measurement (VSHUNT) 24-bit Register (Address = 4h) [reset = 0h] */ + +#define INA228_VSHUNT_SHIFTS (4) +#define INA228_VSHUNT_MASK (UINT32_C(0xffffff) << INA228_VSHUNT_SHIFTS) + +/* INA228 Bus Voltage Measurement (VBUS) 24-bit Register (Address = 5h) [reset = 0h] */ + +#define INA228_VBUS_SHIFTS (4) +#define INA228_VBUS_MASK (UINT32_C(0xffffff) << INA228_VBUS_SHIFTS) + +/* INA228 Temperature Measurement (DIETEMP) 16-bit Register (Address = 6h) [reset = 0h] */ + +#define INA228_DIETEMP_SHIFTS (0) +#define INA228_DIETEMP_MASK (0xffff << INA228_DIETEMP_SHIFTS) + +/* INA228 Current Result (CURRENT) 24-bit Register (Address = 7h) [reset = 0h] */ + +#define INA228_CURRENT_SHIFTS (4) +#define INA228_CURRENT_MASK (UINT32_C(0xffffff) << INA228_CURRENT_SHIFTS) + +/* INA228 Power Result (POWER) 24-bit Register (Address = 8h) [reset = 0h] */ + +#define INA228_POWER_SHIFTS (0) +#define INA228_POWER_MASK (UINT32_C(0xffffff) << INA228_POWER_SHIFTS) + +/* INA228 Energy Result (ENERGY) 40-bit Register (Address = 9h) [reset = 0h] */ + +#define INA228_ENERGY_SHIFTS (0) +#define INA228_ENERGY_MASK (UINT64_C(0xffffffffff) << INA228_ENERGY_SHIFTS) + +/* INA228 Energy Result (CHARGE) 40-bit Register (Address = Ah) [reset = 0h] */ + +#define INA228_CHARGE_SHIFTS (0) +#define INA228_CHARGE_MASK (UINT64_C(0xffffffffff) << INA228_CHARGE_SHIFTS) + + +/* INA228 Diagnostic Flags and Alert (DIAG_ALRT) 16-bit Register (Address = Bh) [reset = 0001h] */ + +#define INA228_MEMSTAT (1 << 0) // This bit is set to 0 if a checksum error is detected in the device trim memory space +#define INA228_CNVRF (1 << 1) // This bit is set to 1 if the conversion is completed. When ALATCH =1 this bit is cleared by reading the register or starting a new triggered conversion. +#define INA228_POL (1 << 2) // This bit is set to 1 if the power measurement exceeds the threshold limit in the power limit register. +#define INA228_BUSUL (1 << 3) // This bit is set to 1 if the bus voltage measurement falls below the threshold limit in the bus under-limit register. +#define INA228_BUSOL (1 << 4) // This bit is set to 1 if the bus voltage measurement exceeds the threshold limit in the bus over-limit register. +#define INA228_SHNTUL (1 << 5) // This bit is set to 1 if the shunt voltage measurement falls below the threshold limit in the shunt under-limit register +#define INA228_SHNTOL (1 << 6) // This bit is set to 1 if the shunt voltage measurement exceeds the threshold limit in the shunt over-limit register. +#define INA228_TMPOL (1 << 7) // This bit is set to 1 if the temperature measurement exceeds the threshold limit in the temperature over-limit register. +#define INA228_MATHOF (1 << 9) // This bit is set to 1 if an arithmetic operation resulted in an overflow error. +#define INA228_CHARGEOF (1 << 10) // This bit indicates the health of the CHARGE register. If the 40 bit CHARGE register has overflowed this bit is set to 1. +#define INA228_ENERGYOF (1 << 11) // This bit indicates the health of the ENERGY register. If the 40 bit ENERGY register has overflowed this bit is set to 1. +#define INA228_APOL (1 << 12) // Alert Polarity bit sets the Alert pin polarity. +#define INA228_SLOWALER (1 << 13) // ALERT function is asserted on the completed averaged value. This gives the flexibility to delay the ALERT after the averaged value. +#define INA228_CNVR (1 << 14) // Setting this bit high configures the Alert pin to be asserted when the Conversion Ready Flag (bit 1) is asserted, indicating that a conversion cycle has completed +#define INA228_ALATCH (1 << 15) // When the Alert Latch Enable bit is set to Transparent mode, the Alert pin and Flag bit reset to the idle state when the fault has been +// cleared. When the Alert Latch Enable bit is set to Latch mode, the Alert pin and Alert Flag bit remain active following a fault until +// the DIAG_ALRT Register has been read. + +/* Shunt Overvoltage Threshold (SOVL) 16-bit Register (Address = Ch) [reset = 7FFFh] */ + +#define INA228_SOVL_SHIFTS (0) +#define INA228_SOVL_MASK (0xffff << INA228_SOVL_SHIFTS) + +/* Shunt Undervoltage Threshold (SUVL) 16-bit Register (Address = Dh) [reset = 8000h] */ + +#define INA228_SUVL_SHIFTS (0) +#define INA228_SUVL_MASK (0xffff << INA228_SUVL_SHIFTS) + +/* Bus Overvoltage Threshold (BOVL) 16-bit Register (Address = Eh) [reset = 7FFFh] */ + +#define INA228_BOVL_SHIFTS (0) +#define INA228_BOVL_MASK (0xffff << INA228_BOVL_SHIFTS) + +/* Bus Undervoltage Threshold (BUVL) 16-bit Register (Address = Fh) [reset = 0h] */ + +#define INA228_BUVL_SHIFTS (0) +#define INA228_BUVL_MASK (0xffff << INA228_BUVL_SHIFTS) + +/* Temperature Over-Limit Threshold (TEMP_LIMIT) 16-bit Register (Address = 10h) [reset = 7FFFh */ + +#define INA228_TEMP_LIMIT_SHIFTS (0) +#define INA228_TEMP_LIMIT_MASK (0xffff << INA228_TEMP_LIMIT_SHIFTS) + +/* Power Over-Limit Threshold (PWR_LIMIT) 16-bit Register (Address = 11h) [reset = FFFFh] */ + +#define INA228_POWER_LIMIT_SHIFTS (0) +#define INA228_POWER_LIMIT_MASK (0xffff << INA228_POWER_LIMIT_SHIFTS) + +/* Manufacturer ID (MANUFACTURER_ID) 16-bit Register (Address = 3Eh) [reset = 5449h] */ + +/* Device ID (DEVICE_ID) 16-bit Register (Address = 3Fh) [reset = 2280h] */ + +#define INA228_DEVICE_REV_ID_SHIFTS (0) +#define INA228_DEVICE_REV_ID_MASK (0xf << INA228_DEVICE_REV_ID_SHIFTS) +#define INA228_DEVICEREV_ID(v) (((v) & INA228_DEVICE_REV_ID_MASK) >> INA228_DEVICE_REV_ID_SHIFTS) +#define INA228_DEVICE_ID_SHIFTS (4) +#define INA228_DEVICE_ID_MASK (0xfff << INA228_DEVICE_ID_SHIFTS) +#define INA228_DEVICEID(v) (((v) & INA228_DEVICE_ID_MASK) >> INA228_DEVICE_ID_SHIFTS) + + + +#define INA228_SAMPLE_FREQUENCY_HZ 10 +#define INA228_SAMPLE_INTERVAL_US (1_s / INA228_SAMPLE_FREQUENCY_HZ) +#define INA228_CONVERSION_INTERVAL (INA228_SAMPLE_INTERVAL_US - 7) +#define MAX_CURRENT 327.68f /* Amps */ +#define DN_MAX 524288.0f /* 2^19 */ +#define INA228_CONST 13107.2e6f /* is an internal fixed value used to ensure scaling is maintained properly */ +#define INA228_SHUNT 0.0005f /* Shunt is 500 uOhm */ +#define INA228_VSCALE 1.95e-04f /* LSB of voltage is 195.3125 uV/LSB */ + +#define swap16(w) __builtin_bswap16((w)) +#define swap32(d) __builtin_bswap32((d)) +#define swap64(q) __builtin_bswap64((q)) + +class INA228 : public device::I2C, public ModuleParams, public I2CSPIDriver +{ +public: + INA228(const I2CSPIDriverConfig &config, int battery_index); + virtual ~INA228(); + + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); + static void print_usage(); + + void RunImpl(); + + int init() override; + + /** + * Tries to call the init() function. If it fails, then it will schedule to retry again in + * INA228_INIT_RETRY_INTERVAL_US microseconds. It will keep retrying at this interval until initialization succeeds. + * + * @return PX4_OK if initialization succeeded on the first try. Negative value otherwise. + */ + int force_init(); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_status() override; + +protected: + int probe() override; + +private: + bool _sensor_ok{false}; + unsigned _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; + + int32_t _bus_voltage{0}; + int64_t _power{0}; + int32_t _current{0}; + int32_t _shunt{0}; + int16_t _cal{0}; + int16_t _range{INA228_ADCRANGE_HIGH}; + bool _mode_triggered{false}; + + float _max_current{MAX_CURRENT}; + float _rshunt{INA228_SHUNT}; + uint16_t _config{INA228_ADCCONFIG}; + 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_0)}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + int read(uint8_t address, int16_t &data); + int write(uint8_t address, int16_t data); + + int read(uint8_t address, uint16_t &data); + int write(uint8_t address, uint16_t data); + + int read(uint8_t address, int32_t &data); + int write(uint8_t address, int32_t data); + + int read(uint8_t address, int64_t &data); + int write(uint8_t address, int64_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(); + + int measure(); + int collect(); + +}; diff --git a/src/drivers/power_monitor/ina228/ina228_main.cpp b/src/drivers/power_monitor/ina228/ina228_main.cpp new file mode 100644 index 0000000000..1d1f02e5e7 --- /dev/null +++ b/src/drivers/power_monitor/ina228/ina228_main.cpp @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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. + * + ****************************************************************************/ +#include +#include + +#include "ina228.h" + +I2CSPIDriverBase *INA228::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) +{ + INA228 *instance = new INA228(config, config.custom1); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (config.keep_running) { + if (instance->force_init() != PX4_OK) { + PX4_INFO("Failed to init INA228 on bus %d, but will try again periodically.", config.bus); + } + + } else if (instance->init() != PX4_OK) { + delete instance; + return nullptr; + } + + return instance; +} + +void +INA228::print_usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Driver for the INA228 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 0x45, and one can run on Bus 2, address 0x45. + +If the INA228 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("ina228", "driver"); + + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x45); + PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG(); + PRINT_MODULE_USAGE_PARAM_INT('t', 1, 1, 2, "battery index for calibration values (1 or 2)", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int +ina228_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = INA228; + BusCLIArguments cli{true, false}; + cli.i2c_address = INA228_BASEADDR; + cli.default_i2c_frequency = 100000; + cli.support_keep_running = true; + cli.custom1 = 1; + + while ((ch = cli.getOpt(argc, argv, "t:")) != EOF) { + switch (ch) { + case 't': // battery index + cli.custom1 = (int)strtol(cli.optArg(), NULL, 0); + break; + } + } + + const char *verb = cli.optArg(); + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_POWER_DEVTYPE_INA228); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/power_monitor/ina228/ina228_params.c b/src/drivers/power_monitor/ina228/ina228_params.c new file mode 100644 index 0000000000..856834f479 --- /dev/null +++ b/src/drivers/power_monitor/ina228/ina228_params.c @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + + +/** + * INA228 Power Monitor Config + * + * @group Sensors + * @min 0 + * @max 65535 + * @decimal 1 + * @increment 1 +*/ +PARAM_DEFINE_INT32(INA228_CONFIG, 63779); + +/** + * INA228 Power Monitor Max Current + * + * @group Sensors + * @min 0.1 + * @max 327.68 + * @decimal 2 + * @increment 0.1 + */ +PARAM_DEFINE_FLOAT(INA228_CURRENT, 327.68f); + +/** + * INA228 Power Monitor Shunt + * + * @group Sensors + * @min 0.000000001 + * @max 0.1 + * @decimal 10 + * @increment .000000001 + */ +PARAM_DEFINE_FLOAT(INA228_SHUNT, 0.0005f);