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

drivers/power_monitor: add TI INA220 driver (#20504)

Browse Source
This commit is contained in:
benjinne 2022-11-23 15:11:29 -05:00 committed by GitHub
parent 7888cc8cfd
commit 85e9c17a5e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 899 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/drivers/drv_sensor.h
View File

@ -149,9 +149,6 @@
#define DRV_DIST_DEVTYPE_SRF02 0x74
#define DRV_DIST_DEVTYPE_TERARANGER 0x75
#define DRV_DIST_DEVTYPE_VL53L0X 0x76
#define DRV_POWER_DEVTYPE_INA226 0x77
#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
@ -188,7 +185,6 @@
#define DRV_DIST_DEVTYPE_GY_US42 0x9C
#define DRV_BAT_DEVTYPE_BATMON_SMBUS 0x9d
#define DRV_POWER_DEVTYPE_INA238 0x9E
#define DRV_GPIO_DEVTYPE_MCP23009 0x9F
#define DRV_GPS_DEVTYPE_ASHTECH 0xA0
@ -219,6 +215,12 @@
#define DRV_BARO_DEVTYPE_ICP101XX 0xB7
#define DRV_BARO_DEVTYPE_ICP201XX 0xB8
#define DRV_POWER_DEVTYPE_INA226 0xD0
#define DRV_POWER_DEVTYPE_INA228 0xD1
#define DRV_POWER_DEVTYPE_VOXLPM 0xD2
#define DRV_POWER_DEVTYPE_INA220 0xD3
#define DRV_POWER_DEVTYPE_INA238 0xD4
#define DRV_DEVTYPE_UNUSED 0xff
#endif /* _DRV_SENSOR_H */

46
src/drivers/power_monitor/ina220/CMakeLists.txt Normal file
View File

@ -0,0 +1,46 @@
############################################################################
#
# Copyright (c) 2022 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__ina22
MAIN ina220
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
# -DDEBUG_BUILD
# -O0
SRCS
ina220_main.cpp
ina220.cpp
DEPENDS
battery
px4_work_queue
)

5
src/drivers/power_monitor/ina220/Kconfig Normal file
View File

@ -0,0 +1,5 @@
menuconfig DRIVERS_POWER_MONITOR_INA220
bool "ina220"
default n
---help---
Enable support for ina220

376
src/drivers/power_monitor/ina220/ina220.cpp Normal file
View File

@ -0,0 +1,376 @@
/****************************************************************************
*
* Copyright (c) 2022 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 ina220.cpp
* @author Ben Linne <benjinne@gmail.com>
*
* Driver for the I2C attached INA220
* Based on the INA226 driver
*
*/
#include "ina220.h"
INA220::INA220(const I2CSPIDriverConfig &config, int battery_index) :
I2C(config),
ModuleParams(nullptr),
I2CSPIDriver(config),
_sample_perf(perf_alloc(PC_ELAPSED, "ina220_read")),
_comms_errors(perf_alloc(PC_COUNT, "ina220_com_err")),
_collection_errors(perf_alloc(PC_COUNT, "ina220_collection_err")),
_measure_errors(perf_alloc(PC_COUNT, "ina220_measurement_err")),
_ch_type((PM_CH_TYPE)config.custom2),
_battery(battery_index, this, INA220_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE)
{
float fvalue = MAX_CURRENT;
_max_current = fvalue;
param_t ph = (_ch_type == PM_CH_TYPE_VBATT) ? param_find("INA220_CURRENT_BAT") : param_find("INA220_CURRENT_REG");
if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
_max_current = fvalue;
}
fvalue = INA220_SHUNT;
_rshunt = fvalue;
ph = (_ch_type == PM_CH_TYPE_VBATT) ? param_find("INA220_SHUNT_BAT") : param_find("INA220_SHUNT_REG");
if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
_rshunt = fvalue;
}
ph = param_find("INA220_CONFIG");
int32_t value = INA220_CONFIG;
_config = (uint16_t)value;
if (ph != PARAM_INVALID && param_get(ph, &value) == PX4_OK) {
_config = (uint16_t)value;
}
_mode_triggered = ((_config & INA220_MODE_MASK) >> INA220_MODE_SHIFTS) <=
((INA220_MODE_SHUNT_BUS_TRIG & INA220_MODE_MASK) >>
INA220_MODE_SHIFTS);
_current_lsb = _max_current / DN_MAX;
_power_lsb = 25 * _current_lsb;
if (_ch_type == PM_CH_TYPE_VBATT) {
// We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0
_battery.setConnected(false);
_battery.updateVoltage(0.f);
_battery.updateCurrent(0.f);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
}
}
INA220::~INA220()
{
// Reset config register on stop so restart probes correctly
write(INA220_REG_CONFIGURATION, INA220_RST);
/* free perf counters */
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_collection_errors);
perf_free(_measure_errors);
}
int INA220::read(uint8_t address, int16_t &data)
{
// read desired little-endian value via I2C
uint16_t received_bytes;
int ret = PX4_ERROR;
for (size_t i = 0; i < 3; i++) {
ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes));
if (ret == PX4_OK) {
data = swap16(received_bytes);
break;
} else {
perf_count(_comms_errors);
PX4_DEBUG("i2c::transfer returned %d", ret);
}
}
return ret;
}
int INA220::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
INA220::init()
{
int ret = PX4_ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != PX4_OK) {
return ret;
}
write(INA220_REG_CONFIGURATION, INA220_RST);
_cal = INA220_CONST / (_current_lsb * _rshunt);
if (write(INA220_REG_CALIBRATION, _cal) < 0) {
return -3;
}
// If we run in continuous mode then start it here
if (!_mode_triggered) {
ret = write(INA220_REG_CONFIGURATION, _config);
} else {
ret = PX4_OK;
}
start();
_sensor_ok = true;
_initialized = ret == PX4_OK;
return ret;
}
int
INA220::force_init()
{
int ret = init();
start();
return ret;
}
int
INA220::probe()
{
int16_t value{0};
if (read(INA220_REG_CONFIGURATION, value) != PX4_OK || value != INA220_DEFAULT_CONFIG) {
PX4_DEBUG("probe found wrong default_config %d", value);
return -1;
}
PX4_DEBUG("probe found correct default_config %d", value);
return PX4_OK;
}
int
INA220::measure()
{
int ret = PX4_OK;
if (_mode_triggered) {
ret = write(INA220_REG_CONFIGURATION, _config);
if (ret < 0) {
perf_count(_comms_errors);
PX4_DEBUG("i2c::transfer returned %d", ret);
}
}
return ret;
}
int
INA220::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(INA220_REG_BUSVOLTAGE, _bus_voltage) == PX4_OK);
success = success && (read(INA220_REG_POWER, _bus_power) == PX4_OK);
success = success && (read(INA220_REG_CURRENT, _bus_current) == PX4_OK);
success = success && (read(INA220_REG_SHUNTVOLTAGE, _shunt) == PX4_OK);
uint16_t unsigned_bus_voltage = _bus_voltage;
_bus_voltage = unsigned_bus_voltage >> 3; // INA220 - the 3 LSB unused, so shift right
if (!success) {
PX4_DEBUG("error reading from sensor");
_bus_voltage = _bus_power = _bus_current = _shunt = 0;
}
_voltage = static_cast<float>(_bus_voltage * INA220_VSCALE);
_current = static_cast<float>(_bus_current * _current_lsb);
_vshunt = static_cast<float>(_shunt * INA220_VSHUNTSCALE);
switch (_ch_type) {
case PM_CH_TYPE_VBATT: {
_battery.setConnected(success);
_battery.updateVoltage(_voltage);
_battery.updateCurrent(_current);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
}
break;
case PM_CH_TYPE_VREG: {
memset(&_pm_status, 0x00, sizeof(_pm_status));
_pm_status.timestamp = hrt_absolute_time();
_pm_status.voltage_v = _voltage;
_pm_status.current_a = _current;
_pm_pub_topic.publish(_pm_status);
}
break;
}
perf_end(_sample_perf);
if (success) {
return PX4_OK;
} else {
return PX4_ERROR;
}
}
void
INA220::start()
{
ScheduleClear();
/* reset the report ring and state machine */
_collect_phase = false;
_measure_interval = INA220_CONVERSION_INTERVAL;
/* schedule a cycle to start things */
ScheduleDelayed(5);
}
void
INA220::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 > INA220_CONVERSION_INTERVAL) {
/* schedule a fresh cycle call when we are ready to measure again */
ScheduleDelayed(_measure_interval - INA220_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(INA220_CONVERSION_INTERVAL);
} else {
if (_ch_type == PM_CH_TYPE_VBATT) {
_battery.setConnected(false);
_battery.updateVoltage(0.f);
_battery.updateCurrent(0.f);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
}
if (init() != PX4_OK) {
ScheduleDelayed(INA220_INIT_RETRY_INTERVAL_US);
}
}
}
void
INA220::print_status()
{
I2CSPIDriverBase::print_status();
if (_initialized) {
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
switch (_ch_type) {
case PM_CH_TYPE_VBATT:
printf("- type: BATT\n");
break;
case PM_CH_TYPE_VREG:
printf("- type: VREG\n");
break;
default:
printf("- type: UNKOWN\n");
break;
}
printf(" - voltage: %9.4f VDC \n", (double) _voltage);
printf(" - current: %9.4f ADC \n", (double) _current);
printf(" - shunt: %9.4f mV \n", (double) _vshunt);
printf("poll interval: %u \n", _measure_interval);;
} else {
PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.",
INA220_INIT_RETRY_INTERVAL_US / 1000);
}
}

223
src/drivers/power_monitor/ina220/ina220.h Normal file
View File

@ -0,0 +1,223 @@
/****************************************************************************
*
* Copyright (C) 2022 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 ina220.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/topics/power_monitor.h>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <px4_platform_common/i2c_spi_buses.h>
using namespace time_literals;
/* Configuration Constants */
#define INA220_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 INA220 every this many microseconds
#define INA220_INIT_RETRY_INTERVAL_US 500000
/* INA220 Registers addresses */
#define INA220_REG_CONFIGURATION (0x00)
#define INA220_REG_SHUNTVOLTAGE (0x01)
#define INA220_REG_BUSVOLTAGE (0x02)
#define INA220_REG_POWER (0x03)
#define INA220_REG_CURRENT (0x04)
#define INA220_REG_CALIBRATION (0x05)
#define INA220_DEFAULT_CONFIG (0x399F)
/* INA220 Configuration Register */
#define INA220_MODE_SHIFTS (0)
#define INA220_MODE_MASK (7 << INA220_MODE_SHIFTS)
#define INA220_MODE_SHUTDOWN (0 << INA220_MODE_SHIFTS)
#define INA220_MODE_SHUNT_TRIG (1 << INA220_MODE_SHIFTS)
#define INA220_MODE_BUS_TRIG (2 << INA220_MODE_SHIFTS)
#define INA220_MODE_SHUNT_BUS_TRIG (3 << INA220_MODE_SHIFTS)
#define INA220_MODE_ADC_OFF (4 << INA220_MODE_SHIFTS)
#define INA220_MODE_SHUNT_CONT (5 << INA220_MODE_SHIFTS)
#define INA220_MODE_BUS_CONT (6 << INA220_MODE_SHIFTS)
#define INA220_MODE_SHUNT_BUS_CONT (7 << INA220_MODE_SHIFTS)
#define INA220_SADC_SHIFTS (3)
#define INA220_SADC_MASK (15 << INA220_SADC_SHIFTS)
#define INA220_SADC_9BIT (0 << INA220_SADC_SHIFTS)
#define INA220_SADC_10BIT (1 << INA220_SADC_SHIFTS)
#define INA220_SADC_11BIT (2 << INA220_SADC_SHIFTS)
#define INA220_SADC_12BIT (3 << INA220_SADC_SHIFTS)
#define INA220_SADC_SAMPLES_2 (9 << INA220_SADC_SHIFTS)
#define INA220_SADC_SAMPLES_4 (10 << INA220_SADC_SHIFTS)
#define INA220_SADC_SAMPLES_8 (11 << INA220_SADC_SHIFTS)
#define INA220_SADC_SAMPLES_16 (12 << INA220_SADC_SHIFTS)
#define INA220_SADC_SAMPLES_32 (13 << INA220_SADC_SHIFTS)
#define INA220_SADC_SAMPLES_64 (14 << INA220_SADC_SHIFTS)
#define INA220_SADC_SAMPLES_128 (15 << INA220_SADC_SHIFTS)
#define INA220_BADC_SHIFTS (7)
#define INA220_BADC_MASK (15 << INA220_BADC_SHIFTS)
#define INA220_BADC_9BIT (0 << INA220_BADC_SHIFTS)
#define INA220_BADC_10BIT (1 << INA220_BADC_SHIFTS)
#define INA220_BADC_11BIT (2 << INA220_BADC_SHIFTS)
#define INA220_BADC_12BIT (3 << INA220_BADC_SHIFTS)
#define INA220_BADC_SAMPLES_2 (9 << INA220_BADC_SHIFTS)
#define INA220_BADC_SAMPLES_4 (10 << INA220_BADC_SHIFTS)
#define INA220_BADC_SAMPLES_8 (11 << INA220_BADC_SHIFTS)
#define INA220_BADC_SAMPLES_16 (12 << INA220_BADC_SHIFTS)
#define INA220_BADC_SAMPLES_32 (13 << INA220_BADC_SHIFTS)
#define INA220_BADC_SAMPLES_64 (14 << INA220_BADC_SHIFTS)
#define INA220_BADC_SAMPLES_128 (15 << INA220_BADC_SHIFTS)
#define INA220_PG_SHIFTS (11)
#define INA220_PG_MASK (2 << INA220_PG_SHIFTS)
#define INA220_PG_40mV (1 << INA220_PG_SHIFTS)
#define INA220_PG_80mV (2 << INA220_PG_SHIFTS)
#define INA220_PG_160mV (3 << INA220_PG_SHIFTS)
#define INA220_PG_320mV (4 << INA220_PG_SHIFTS)
#define INA220_BRNG_SHIFTS (13)
#define INA220_BRNG_MASK (1 << INA220_BRNG_SHIFTS)
#define INA220_BRNG_16V (0 << INA220_BRNG_SHIFTS)
#define INA220_BRNG_32V (1 << INA220_BRNG_SHIFTS)
#define INA220_CONFIG (INA220_BRNG_32V | INA220_PG_320mV | INA220_BADC_12BIT | INA220_SADC_12BIT | INA220_MODE_SHUNT_BUS_CONT)
#define INA220_RST (1 << 15)
#define INA220_SAMPLE_FREQUENCY_HZ 10
#define INA220_SAMPLE_INTERVAL_US (1_s / INA220_SAMPLE_FREQUENCY_HZ)
#define INA220_CONVERSION_INTERVAL (INA220_SAMPLE_INTERVAL_US - 7)
#define MAX_CURRENT 400.0f /* 400 Amps */
#define DN_MAX 32768.0f /* 2^15 */
#define INA220_CONST 0.04096f /* is an internal fixed value used to ensure scaling is maintained properly */
#define INA220_SHUNT 0.0005f /* Shunt is 500 uOhm */
#define INA220_VSCALE 0.004f /* LSB of voltage is 4 mV */
#define INA220_VSHUNTSCALE 0.01f /* LSB of shunt voltage is 10 uV */
#define swap16(w) __builtin_bswap16((w))
enum PM_CH_TYPE {
PM_CH_TYPE_VBATT = 0,
PM_CH_TYPE_VREG
};
class INA220 : public device::I2C, public ModuleParams, public I2CSPIDriver<INA220>
{
public:
INA220(const I2CSPIDriverConfig &config, int battery_index);
virtual ~INA220();
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
* INA220_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;
int16_t _bus_voltage{0};
int16_t _bus_power{0};
int16_t _bus_current{0};
int16_t _shunt{0};
uint16_t _cal{0};
bool _mode_triggered{false};
const PM_CH_TYPE _ch_type;
float _max_current{MAX_CURRENT};
float _rshunt{INA220_SHUNT};
uint16_t _config{INA220_CONFIG};
float _current_lsb{_max_current / DN_MAX};
float _power_lsb{25.0f * _current_lsb};
float _voltage{0};
float _current{0};
float _vshunt{0};
Battery _battery;
uORB::PublicationMulti<power_monitor_s> _pm_pub_topic{ORB_ID(power_monitor)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
power_monitor_s _pm_status{};
int read(uint8_t address, int16_t &data);
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();
int measure();
int collect();
};

145
src/drivers/power_monitor/ina220/ina220_main.cpp Normal file
View File

@ -0,0 +1,145 @@
/****************************************************************************
*
* Copyright (C) 2022 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 "ina220.h"
I2CSPIDriverBase *INA220::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
{
INA220 *instance = new INA220(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 INA220 on bus %d, but will try again periodically.", config.bus);
}
} else if (instance->init() != PX4_OK) {
delete instance;
return nullptr;
}
return instance;
}
void
INA220::print_usage()
{
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Driver for the INA220 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 INA220 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("ina220", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x41);
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_PARAM_STRING('T', "VBATT", "VBATT|VREG", "Type", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
extern "C" int
ina220_main(int argc, char *argv[])
{
int ch;
using ThisDriver = INA220;
BusCLIArguments cli{true, false};
cli.i2c_address = INA220_BASEADDR;
cli.default_i2c_frequency = 100000;
cli.support_keep_running = true;
cli.custom1 = 1;
cli.custom2 = PM_CH_TYPE_VBATT;
while ((ch = cli.getOpt(argc, argv, "T:")) != EOF) {
switch (ch) {
case 't': // battery index
cli.custom1 = (int)strtol(cli.optArg(), NULL, 0);
break;
case 'T':
if (strcmp(cli.optArg(), "VBATT") == 0) {
cli.custom2 = PM_CH_TYPE_VBATT;
} else if (strcmp(cli.optArg(), "VREG") == 0) {
cli.custom2 = PM_CH_TYPE_VREG;
} else {
PX4_ERR("unknown type");
return -1;
}
break;
}
}
const char *verb = cli.optArg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_POWER_DEVTYPE_INA220);
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;
}

98
src/drivers/power_monitor/ina220/ina220_params.c Normal file
View File

@ -0,0 +1,98 @@
/****************************************************************************
*
* Copyright (c) 2022 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.
*
****************************************************************************/
/**
* Enable INA220 Power Monitor
*
* For systems a INA220 Power Monitor, this should be set to true
*
* @group Sensors
* @boolean
* @reboot_required true
*/
PARAM_DEFINE_INT32(SENS_EN_INA220, 0);
/**
* INA220 Power Monitor Config
*
* @group Sensors
* @min 0
* @max 65535
* @decimal 1
* @increment 1
*/
PARAM_DEFINE_INT32(INA220_CONFIG, 8607);
/**
* INA220 Power Monitor Battery Max Current
*
* @group Sensors
* @min 0.1
* @max 500.0
* @decimal 2
* @increment 0.1
*/
PARAM_DEFINE_FLOAT(INA220_CUR_BAT, 164.0f);
/**
* INA220 Power Monitor Battery Shunt
*
* @group Sensors
* @min 0.000000001
* @max 0.1
* @decimal 10
* @increment .000000001
*/
PARAM_DEFINE_FLOAT(INA220_SHUNT_BAT, 0.0005f);
/**
* INA220 Power Monitor Regulator Max Current
*
* @group Sensors
* @min 0.1
* @max 500.0
* @decimal 2
* @increment 0.1
*/
PARAM_DEFINE_FLOAT(INA220_CUR_REG, 164.0f);
/**
* INA220 Power Monitor Regulator Shunt
*
* @group Sensors
* @min 0.000000001
* @max 0.1
* @decimal 10
* @increment .000000001
*/
PARAM_DEFINE_FLOAT(INA220_SHUNT_REG, 0.0005f);