diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 2d85fd84dc..9f791f004c 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -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 */ diff --git a/src/drivers/power_monitor/ina220/CMakeLists.txt b/src/drivers/power_monitor/ina220/CMakeLists.txt new file mode 100644 index 0000000000..e3a2f26012 --- /dev/null +++ b/src/drivers/power_monitor/ina220/CMakeLists.txt @@ -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 + ) diff --git a/src/drivers/power_monitor/ina220/Kconfig b/src/drivers/power_monitor/ina220/Kconfig new file mode 100644 index 0000000000..bcbf3868fb --- /dev/null +++ b/src/drivers/power_monitor/ina220/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_POWER_MONITOR_INA220 + bool "ina220" + default n + ---help--- + Enable support for ina220 diff --git a/src/drivers/power_monitor/ina220/ina220.cpp b/src/drivers/power_monitor/ina220/ina220.cpp new file mode 100644 index 0000000000..3667ee56d4 --- /dev/null +++ b/src/drivers/power_monitor/ina220/ina220.cpp @@ -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 + * + * 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(¶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(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(_bus_voltage * INA220_VSCALE); + _current = static_cast(_bus_current * _current_lsb); + _vshunt = static_cast(_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); + } +} diff --git a/src/drivers/power_monitor/ina220/ina220.h b/src/drivers/power_monitor/ina220/ina220.h new file mode 100644 index 0000000000..bbbcb27c66 --- /dev/null +++ b/src/drivers/power_monitor/ina220/ina220.h @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 +{ +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 _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(); + +}; diff --git a/src/drivers/power_monitor/ina220/ina220_main.cpp b/src/drivers/power_monitor/ina220/ina220_main.cpp new file mode 100644 index 0000000000..287c7f4686 --- /dev/null +++ b/src/drivers/power_monitor/ina220/ina220_main.cpp @@ -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 +#include + +#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; +} diff --git a/src/drivers/power_monitor/ina220/ina220_params.c b/src/drivers/power_monitor/ina220/ina220_params.c new file mode 100644 index 0000000000..d07b8570a5 --- /dev/null +++ b/src/drivers/power_monitor/ina220/ina220_params.c @@ -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);