dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Support INA228

This commit is contained in:
David Sidrane 2021-07-22 14:37:38 -07:00 committed by Daniel Agar
parent fdb92d4746
commit f819be2075
6 changed files with 1045 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/drivers/drv_sensor.h
View File

@ -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

44
src/drivers/power_monitor/ina228/CMakeLists.txt Normal file
View File

@ -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
)

419
src/drivers/power_monitor/ina228/ina228.cpp Normal file
View File

@ -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 <david.sidrane@nscdg.com>
*
* 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(&parameter_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);
}
}

384
src/drivers/power_monitor/ina228/ina228.h Normal file
View File

@ -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 <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/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <px4_platform_common/i2c_spi_buses.h>
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<INA228>
{
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();
};

130
src/drivers/power_monitor/ina228/ina228_main.cpp Normal file
View File

@ -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 <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#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;
}

66
src/drivers/power_monitor/ina228/ina228_params.c Normal file
View File

@ -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);