From 8720bf45b2955e414e9f48fd611b1609b8ae696f Mon Sep 17 00:00:00 2001 From: qqqlab <46283638+qqqlab@users.noreply.github.com> Date: Sun, 12 Jan 2025 10:36:22 +0100 Subject: [PATCH] add INA228 --- examples/01.Quadcopter/madflight_config.h | 2 +- .../02.QuadcopterAdvanced/madflight_config.h | 2 +- examples/03.Plane/madflight_config.h | 2 +- src/madflight.h | 5 - src/madflight/bat/INA228/CHANGELOG.md | 58 ++ src/madflight/bat/INA228/INA228.h | 879 ++++++++++++++++++ src/madflight/bat/INA228/LICENSE | 21 + src/madflight/bat/INA228/README.md | 521 +++++++++++ src/madflight/bat/bat.h | 51 +- src/madflight/rcin/rcin.h | 18 +- 10 files changed, 1538 insertions(+), 21 deletions(-) create mode 100644 src/madflight/bat/INA228/CHANGELOG.md create mode 100644 src/madflight/bat/INA228/INA228.h create mode 100644 src/madflight/bat/INA228/LICENSE create mode 100644 src/madflight/bat/INA228/README.md diff --git a/examples/01.Quadcopter/madflight_config.h b/examples/01.Quadcopter/madflight_config.h index 4e06e63..96c03a1 100644 --- a/examples/01.Quadcopter/madflight_config.h +++ b/examples/01.Quadcopter/madflight_config.h @@ -29,7 +29,7 @@ //#define MAG_I2C_ADR 0x77 // Set magnetometer I2C address, leave commented for default address. If unknown, use CLI 'i2c' //--- BATTERY MONITOR -#define BAT_USE BAT_USE_NONE // Select one: BAT_USE_INA226, BAT_USE_ADC, BAT_USE_NONE +#define BAT_USE BAT_USE_NONE // Select one: BAT_USE_INA226, BAT_USE_INA228, BAT_USE_ADC, BAT_USE_NONE //--- BLACKBOX LOGGER #define BB_USE BB_USE_NONE // Select one: BB_USE_SD, BB_USE_SDMMC, BB_USE_NONE diff --git a/examples/02.QuadcopterAdvanced/madflight_config.h b/examples/02.QuadcopterAdvanced/madflight_config.h index 4e06e63..96c03a1 100644 --- a/examples/02.QuadcopterAdvanced/madflight_config.h +++ b/examples/02.QuadcopterAdvanced/madflight_config.h @@ -29,7 +29,7 @@ //#define MAG_I2C_ADR 0x77 // Set magnetometer I2C address, leave commented for default address. If unknown, use CLI 'i2c' //--- BATTERY MONITOR -#define BAT_USE BAT_USE_NONE // Select one: BAT_USE_INA226, BAT_USE_ADC, BAT_USE_NONE +#define BAT_USE BAT_USE_NONE // Select one: BAT_USE_INA226, BAT_USE_INA228, BAT_USE_ADC, BAT_USE_NONE //--- BLACKBOX LOGGER #define BB_USE BB_USE_NONE // Select one: BB_USE_SD, BB_USE_SDMMC, BB_USE_NONE diff --git a/examples/03.Plane/madflight_config.h b/examples/03.Plane/madflight_config.h index 4e06e63..96c03a1 100644 --- a/examples/03.Plane/madflight_config.h +++ b/examples/03.Plane/madflight_config.h @@ -29,7 +29,7 @@ //#define MAG_I2C_ADR 0x77 // Set magnetometer I2C address, leave commented for default address. If unknown, use CLI 'i2c' //--- BATTERY MONITOR -#define BAT_USE BAT_USE_NONE // Select one: BAT_USE_INA226, BAT_USE_ADC, BAT_USE_NONE +#define BAT_USE BAT_USE_NONE // Select one: BAT_USE_INA226, BAT_USE_INA228, BAT_USE_ADC, BAT_USE_NONE //--- BLACKBOX LOGGER #define BB_USE BB_USE_NONE // Select one: BB_USE_SD, BB_USE_SDMMC, BB_USE_NONE diff --git a/src/madflight.h b/src/madflight.h index 882764a..45382d1 100644 --- a/src/madflight.h +++ b/src/madflight.h @@ -34,11 +34,6 @@ SOFTWARE. #define HW_PIN_OUT_LIST {} #endif -//defaults for RCIN -#ifndef RCIN_NUM_CHANNELS - #define RCIN_NUM_CHANNELS 0 -#endif - //defaults for madflight_setup(); #ifndef IMU_SAMPLE_RATE #define IMU_SAMPLE_RATE 1000 diff --git a/src/madflight/bat/INA228/CHANGELOG.md b/src/madflight/bat/INA228/CHANGELOG.md new file mode 100644 index 0000000..16885bc --- /dev/null +++ b/src/madflight/bat/INA228/CHANGELOG.md @@ -0,0 +1,58 @@ +# changes for madflight + +- replace TwoWire with HW_WIRETYPE (and move INA228.cpp into INA228.h to make it a "template") +- add calibrate() +- add isConversionReady() +- move i2c init from constructor to begin() + + +# Change Log INA228 + +All notable changes to this project will be documented in this file. + +The format is based on [Keep a Changelog](http://keepachangelog.com/) +and this project adheres to [Semantic Versioning](http://semver.org/). + + +## [0.1.4] - 2024-10-28 +- add and rename wrappers for core functions +- update all examples +- update readme.md + +## [0.1.3] - 2024-10-22 +- fix #10, remove limit for the Amperes, kudos to geoFrancis +- fix handling negative values for **getShuntVoltage()** +- fix handling negative values for **getCurrent()** +- update readme.md +- add **INA228_demo_two_devices.ino** +- minor edits. + +## [0.1.2] - 2024-09-29 +- merge fix for temperature - kudos to xkachya! +- fix for getShuntVoltage() - kudos to markliquid1 (PR #8) +- refactored many functions. +- add INA228_CFG_REGISTER defines (register 0). +- add INA228_ADC_REGISTER defines (register 1). +- fix ADC functions. +- optimize **xxxDiagnoseAlertBit()** functions +- rename **clearDiagnoseAlertBit()** +- fix **getDieID()** +- add **INA228_performance.ino** +- change return type **getEnergy()** and **getCharge()** to double. +- update readme.md + +## [0.1.1] - 2024-05-15 +- add limit functions +- add threshold functions +- add constants for DiagnoseAlert register + - check them in unit test +- move register constants to .cpp +- make readRegister() and writeRegister() private +- update readme.md, keywords.txt +- minor edits + +## [0.1.0] - 2024-05-10 +- initial version + + + diff --git a/src/madflight/bat/INA228/INA228.h b/src/madflight/bat/INA228/INA228.h new file mode 100644 index 0000000..8e08a88 --- /dev/null +++ b/src/madflight/bat/INA228/INA228.h @@ -0,0 +1,879 @@ +//modified for madflight - see CHANGELOG.md + +#pragma once +// FILE: INA228.h +// AUTHOR: Rob Tillaart +// VERSION: 0.1.4 +// DATE: 2024-05-09 +// PURPOSE: Arduino library for INA228 voltage, current and power sensor. +// URL: https://github.com/RobTillaart/INA228 +// https://www.adafruit.com/product/5832 ( 10 A version) +// https://www.mateksys.com/?portfolio=i2c-ina-bm (200 A version)) +// +// +// Read the datasheet for the details + + + + + +#define INA228_LIB_VERSION (F("0.1.4")) + + +#define INA228_ADDRESS (0x40) //default address + +// for setMode() and getMode() +enum ina228_mode_enum { + INA228_MODE_SHUTDOWN = 0x00, + INA228_MODE_TRIG_BUS = 0x01, + INA228_MODE_TRIG_SHUNT = 0x02, + INA228_MODE_TRIG_BUS_SHUNT = 0x03, + INA228_MODE_TRIG_TEMP = 0x04, + INA228_MODE_TRIG_TEMP_BUS = 0x05, + INA228_MODE_TRIG_TEMP_SHUNT = 0x06, + INA228_MODE_TRIG_TEMP_BUS_SHUNT = 0x07, + + INA228_MODE_SHUTDOWN2 = 0x08, + INA228_MODE_CONT_BUS = 0x09, + INA228_MODE_CONT_SHUNT = 0x0A, + INA228_MODE_CONT_BUS_SHUNT = 0x0B, + INA228_MODE_CONT_TEMP = 0x0C, + INA228_MODE_CONT_TEMP_BUS = 0x0D, + INA228_MODE_CONT_TEMP_SHUNT = 0x0E, + INA228_MODE_CONT_TEMP_BUS_SHUNT = 0x0F +}; + + +// for setAverage() and getAverage() +enum ina228_average_enum { + INA228_1_SAMPLE = 0, + INA228_4_SAMPLES = 1, + INA228_16_SAMPLES = 2, + INA228_64_SAMPLES = 3, + INA228_128_SAMPLES = 4, + INA228_256_SAMPLES = 5, + INA228_512_SAMPLES = 6, + INA228_1024_SAMPLES = 7 +}; + + +// for Bus, shunt and temperature conversion timing. +enum ina228_timing_enum { + INA228_50_us = 0, + INA228_84_us = 1, + INA228_150_us = 2, + INA228_280_us = 3, + INA228_540_us = 4, + INA228_1052_us = 5, + INA228_2074_us = 6, + INA228_4120_us = 7 +}; + + +// for diagnose/alert() bit fields. +// TODO bit masks? +enum ina228_diag_enum { + INA228_DIAG_MEMORY_STATUS = 0, + INA228_DIAG_CONVERT_COMPLETE = 1, + INA228_DIAG_POWER_OVER_LIMIT = 2, + INA228_DIAG_BUS_UNDER_LIMIT = 3, + INA228_DIAG_BUS_OVER_LIMIT = 4, + INA228_DIAG_SHUNT_UNDER_LIMIT = 5, + INA228_DIAG_SHUNT_OVER_LIMIT = 6, + INA228_DIAG_TEMP_OVER_LIMIT = 7, + INA228_DIAG_RESERVED = 8, + INA228_DIAG_MATH_OVERFLOW = 9, + INA228_DIAG_CHARGE_OVERFLOW = 10, + INA228_DIAG_ENERGY_OVERFLOW = 11, + INA228_DIAG_ALERT_POLARITY = 12, + INA228_DIAG_SLOW_ALERT = 13, + INA228_DIAG_CONVERT_READY = 14, + INA228_DIAG_ALERT_LATCH = 15 +}; + + +class INA228 +{ +public: + bool begin(HW_WIRETYPE *wire, uint8_t address = INA228_ADDRESS); + bool isConnected(); + uint8_t getAddress(); + + + bool isConversionReady(); + + // + // CORE FUNCTIONS + scale wrappers. + // + // BUS VOLTAGE + float getBusVoltage(); // Volt + float getBusVolt() { return getBusVoltage(); }; + float getBusMilliVolt() { return getBusVoltage() * 1e3; }; + float getBusMicroVolt() { return getBusVoltage() * 1e6; }; + + // SHUNT VOLTAGE + float getShuntVoltage(); // Volt + float getShuntVolt() { return getShuntVoltage(); }; + float getShuntMilliVolt() { return getShuntVoltage() * 1e3; }; + float getShuntMicroVolt() { return getShuntVoltage() * 1e6; }; + + // SHUNT CURRENT + float getCurrent(); // Ampere + float getAmpere() { return getCurrent(); }; + float getMilliAmpere() { return getCurrent() * 1e3; }; + float getMicroAmpere() { return getCurrent() * 1e6; }; + + // POWER + float getPower(); // Watt + float getWatt() { return getPower(); }; + float getMilliWatt() { return getPower() * 1e3; }; + float getMicroWatt() { return getPower() * 1e6; }; + float getKiloWatt() { return getPower() * 1e-3; }; + + // TEMPERATURE + float getTemperature(); // Celsius + + // the Energy and Charge functions are returning double as they have higher accuracy. + // ENERGY + double getEnergy(); // Joule or watt second + double getJoule() { return getEnergy(); }; + double getMegaJoule() { return getEnergy() * 1e-6; }; + double getKiloJoule() { return getEnergy() * 1e-3; }; + double getMilliJoule() { return getEnergy() * 1e3; }; + double getMicroJoule() { return getEnergy() * 1e6; }; + double getWattHour() { return getEnergy() * (1.0 / 3600.0); }; + double getKiloWattHour() { return getEnergy() * (1.0 / 3.6); }; + + // CHARGE + double getCharge(); // Coulombs + double getCoulomb() { return getCharge(); }; + double getMilliCoulomb() { return getCharge() * 1e3; }; + double getMicroCoulomb() { return getCharge() * 1e6; }; + + + // + // CONFIG REGISTER 0 + // read datasheet for details, section 7.6.1.1, page 22 + // + void reset(); + // value: 0 == normal operation, 1 = clear registers + bool setAccumulation(uint8_t value); + bool getAccumulation(); + // Conversion delay in 0..255 steps of 2 ms + void setConversionDelay(uint8_t steps); + uint8_t getConversionDelay(); + void setTemperatureCompensation(bool on); + bool getTemperatureCompensation(); + // flag = false => 164 mV, true => 41 mV + void setADCRange(bool flag); + bool getADCRange(); + + // + // CONFIG ADC REGISTER 1 + // read datasheet for details, section 7.6.1.2, page 22++ + // + bool setMode(uint8_t mode = INA228_MODE_CONT_TEMP_BUS_SHUNT); + uint8_t getMode(); + // default value = ~1 milliseconds for all. + bool setBusVoltageConversionTime(uint8_t bvct = INA228_1052_us); + uint8_t getBusVoltageConversionTime(); + bool setShuntVoltageConversionTime(uint8_t svct = INA228_1052_us); + uint8_t getShuntVoltageConversionTime(); + bool setTemperatureConversionTime(uint8_t tct = INA228_1052_us); + uint8_t getTemperatureConversionTime(); + bool setAverage(uint8_t avg = INA228_1_SAMPLE); + uint8_t getAverage(); + + // + // SHUNT CALIBRATION REGISTER 2 + // read datasheet for details. use with care. + // maxCurrent <= 204, (in fact no limit) + // shunt >= 0.0001. + // returns _current_LSB; + void calibrate(float shunt, float maxCurrent = 0); + bool isCalibrated() { return _current_LSB != 0.0; }; + float getMaxCurrent(); + float getShunt(); + float getCurrentLSB(); + + // + // SHUNT TEMPERATURE COEFFICIENT REGISTER 3 + // read datasheet for details, page 16. + // ppm = 0..16383. + bool setShuntTemperatureCoefficent(uint16_t ppm = 0); + uint16_t getShuntTemperatureCoefficent(); + + + // + // DIAGNOSE ALERT REGISTER 11 (0x0B) + // read datasheet for details, section 7.6.1.12, page 26++. + // + void setDiagnoseAlert(uint16_t flags); + uint16_t getDiagnoseAlert(); + // INA228.h has an enum for the bit fields. + // See ina228_diag_enum above + void setDiagnoseAlertBit(uint8_t bit); + void clearDiagnoseAlertBit(uint8_t bit); + uint16_t getDiagnoseAlertBit(uint8_t bit); + + + // + // THRESHOLD AND LIMIT REGISTERS 12-17 + // read datasheet for details, section 7.3.7, page 16++ + // + // TODO - design and implement better API? + // + void setShuntOvervoltageTH(uint16_t threshold); + uint16_t getShuntOvervoltageTH(); + void setShuntUndervoltageTH(uint16_t threshold); + uint16_t getShuntUndervoltageTH(); + void setBusOvervoltageTH(uint16_t threshold); + uint16_t getBusOvervoltageTH(); + void setBusUndervoltageTH(uint16_t threshold); + uint16_t getBusUndervoltageTH(); + void setTemperatureOverLimitTH(uint16_t threshold); + uint16_t getTemperatureOverLimitTH(); + void setPowerOverLimitTH(uint16_t threshold); + uint16_t getPowerOverLimitTH(); + + + // + // MANUFACTURER and ID REGISTER 3E and 3F + // + // typical value + uint16_t getManufacturer(); // 0x5449 + uint16_t getDieID(); // 0x0228 + uint16_t getRevision(); // 0x0001 + + +private: + // max 4 bytes + uint32_t _readRegister(uint8_t reg, uint8_t bytes); + // 5 bytes or more + double _readRegisterF(uint8_t reg, uint8_t bytes); + uint16_t _writeRegister(uint8_t reg, uint16_t value); + + float _current_LSB = 0; + float _shunt = 0; + float _maxCurrent = 0; + + uint8_t _address = 0; + HW_WIRETYPE * _wire; + + bool _ADCRange = false; +}; + + +// -- END OF FILE -- + + + + + +//modified for madflight - see CHANGELOG.md + +// FILE: INA228.cpp +// AUTHOR: Rob Tillaart +// VERSION: 0.1.4 +// DATE: 2024-05-09 +// PURPOSE: Arduino library for INA228 voltage, current and power sensor. +// URL: https://github.com/RobTillaart/INA228 +// https://www.adafruit.com/product/5832 ( 10 A version) +// https://www.mateksys.com/?portfolio=i2c-ina-bm (200 A version)) + +// +// Read the datasheet for the details + + +#include "INA228.h" + +// REGISTERS ADDRESS BITS RW +#define INA228_CONFIG 0x00 // 16 RW +#define INA228_ADC_CONFIG 0x01 // 16 RW +#define INA228_SHUNT_CAL 0x02 // 16 RW +#define INA228_SHUNT_TEMP_CO 0x03 // 16 RW +#define INA228_SHUNT_VOLTAGE 0x04 // 24 R- +#define INA228_BUS_VOLTAGE 0x05 // 24 R- +#define INA228_TEMPERATURE 0x06 // 16 R- +#define INA228_CURRENT 0x07 // 24 R- +#define INA228_POWER 0x08 // 24 R- +#define INA228_ENERGY 0x09 // 40 R- +#define INA228_CHARGE 0x0A // 40 R- +#define INA228_DIAG_ALERT 0x0B // 16 RW +#define INA228_SOVL 0x0C // 16 RW +#define INA228_SUVL 0x0D // 16 RW +#define INA228_BOVL 0x0E // 16 RW +#define INA228_BUVL 0x0F // 16 RW +#define INA228_TEMP_LIMIT 0x10 // 16 RW +#define INA228_POWER_LIMIT 0x11 // 16 RW +#define INA228_MANUFACTURER 0x3E // 16 R- +#define INA228_DEVICE_ID 0x3F // 16 R- + + +// CONFIG MASKS (register 0) +#define INA228_CFG_RST 0x8000 +#define INA228_CFG_RSTACC 0x4000 +#define INA228_CFG_CONVDLY 0x3FC0 +#define INA228_CFG_TEMPCOMP 0x0020 +#define INA228_CFG_ADCRANGE 0x0010 +#define INA228_CFG_RESERVED 0x000F + + +// ADC MASKS (register 1) +#define INA228_ADC_MODE 0xF000 +#define INA228_ADC_VBUSCT 0x0E00 +#define INA228_ADC_VSHCT 0x01C0 +#define INA228_ADC_VTCT 0x0038 +#define INA228_ADC_AVG 0x0007 + +// DIAG_ALERT register +#define INA228_ALATCH 0x8000 +#define INA228_CNVRF 0x0002 + + +//////////////////////////////////////////////////////// +// +// CONSTRUCTOR +// + +bool INA228::begin(HW_WIRETYPE *wire, uint8_t address) +{ + _address = address; + _wire = wire; + + if (! isConnected()) return false; + + //set ALATCH + uint16_t value = _readRegister(INA228_DIAG_ALERT, 2); + value |= INA228_ALATCH; + _writeRegister(INA228_DIAG_ALERT, value); + + return true; +} + + +bool INA228::isConnected() +{ + _wire->beginTransmission(_address); + return ( _wire->endTransmission() == 0); +} + + +uint8_t INA228::getAddress() +{ + return _address; +} + + +bool INA228::isConversionReady() { + uint16_t value = _readRegister(INA228_DIAG_ALERT, 2); + return (value && INA228_CNVRF != 0); //INA228_CNVRF is cleared on reading DIAG_ALERT when ALATCH=1 +} + +//////////////////////////////////////////////////////// +// +// CORE FUNCTIONS +// +// PAGE 25 +float INA228::getBusVoltage() +{ + // always positive, remove reserved bits. + int32_t value = _readRegister(INA228_BUS_VOLTAGE, 3) >> 4; + float bus_LSB = 195.3125e-6; // 195.3125 uV + float voltage = value * bus_LSB; + return voltage; +} + +// PAGE 25 +float INA228::getShuntVoltage() +{ + // shunt_LSB depends on ADCRANGE in INA228_CONFIG register. + float shunt_LSB = 312.5e-9; // 312.5 nV + if (_ADCRange) + { + shunt_LSB = 78.125e-9; // 78.125 nV + } + + // remove reserved bits. + int32_t value = _readRegister(INA228_SHUNT_VOLTAGE, 3) >> 4; + // handle negative values (20 bit) + if (value & 0x00080000) + { + value |= 0xFFF00000; + } + float voltage = value * shunt_LSB; + return voltage; +} + +// PAGE 25 + 8.1.2 +float INA228::getCurrent() +{ + // remove reserved bits. + int32_t value = _readRegister(INA228_CURRENT, 3) >> 4; + // handle negative values (20 bit) + if (value & 0x00080000) + { + value |= 0xFFF00000; + } + float current = value * _current_LSB; + return current; +} + +// PAGE 26 + 8.1.2 +float INA228::getPower() +{ + uint32_t value = _readRegister(INA228_POWER, 3) ; + // PAGE 31 (8.1.2) + return value * 3.2 * _current_LSB; +} + +// PAGE 25 +float INA228::getTemperature() +{ + uint32_t value = _readRegister(INA228_TEMPERATURE, 2); + float LSB = 7.8125e-3; // milli degree Celsius + return value * LSB; +} + +// PAGE 26 + 8.1.2 +double INA228::getEnergy() +{ + // read 40 bit unsigned as a double to prevent 64 bit ints + // double might be 8 or 4 byte, depends on platform + // 40 bit ==> O(10^12) + double value = _readRegisterF(INA228_ENERGY, 5); + // PAGE 31 (8.1.2) + return value * (16 * 3.2) * _current_LSB; +} + + +// PAGE 26 + 8.1.2 +double INA228::getCharge() +{ + // read 40 bit unsigned as a float to prevent 64 bit ints + // double might be 8 or 4 byte, depends on platform + // 40 bit ==> O(10^12) + double value = _readRegisterF(INA228_CHARGE, 5); + // PAGE 32 (8.1.2) + return value * _current_LSB; +} + + +//////////////////////////////////////////////////////// +// +// CONFIG REGISTER 0 +// +void INA228::reset() +{ + uint16_t value = _readRegister(INA228_CONFIG, 2); + value |= INA228_CFG_RST; + _writeRegister(INA228_CONFIG, value); +} + +bool INA228::setAccumulation(uint8_t value) +{ + if (value > 1) return false; + uint16_t reg = _readRegister(INA228_CONFIG, 2); + reg &= ~INA228_CFG_RSTACC; + if (value == 1) reg |= INA228_CFG_RSTACC; + _writeRegister(INA228_CONFIG, reg); + return true; +} + +bool INA228::getAccumulation() +{ + uint16_t value = _readRegister(INA228_CONFIG, 2); + return (value & INA228_CFG_RSTACC) > 0; +} + +void INA228::setConversionDelay(uint8_t steps) +{ + uint16_t value = _readRegister(INA228_CONFIG, 2); + value &= ~INA228_CFG_CONVDLY; + value |= (steps << 6); + _writeRegister(INA228_CONFIG, value); +} + +uint8_t INA228::getConversionDelay() +{ + uint16_t value = _readRegister(INA228_CONFIG, 2); + return (value >> 6) & 0xFF; +} + +void INA228::setTemperatureCompensation(bool on) +{ + uint16_t value = _readRegister(INA228_CONFIG, 2); + value &= ~INA228_CFG_TEMPCOMP; + if (on) value |= INA228_CFG_TEMPCOMP; + _writeRegister(INA228_CONFIG, value); +} + +bool INA228::getTemperatureCompensation() +{ + uint16_t value = _readRegister(INA228_CONFIG, 2); + return (value & INA228_CFG_TEMPCOMP) > 0; +} + +void INA228::setADCRange(bool flag) +{ + uint16_t value = _readRegister(INA228_CONFIG, 2); + value &= ~INA228_CFG_ADCRANGE; + if (flag) value |= INA228_CFG_ADCRANGE; + _writeRegister(INA228_CONFIG, value); + _ADCRange = flag; +} + +bool INA228::getADCRange() +{ + uint16_t value = _readRegister(INA228_CONFIG, 2); + _ADCRange = ((value & INA228_CFG_ADCRANGE) != 0); + return _ADCRange; +} + + +//////////////////////////////////////////////////////// +// +// CONFIG ADC REGISTER 1 +// +bool INA228::setMode(uint8_t mode) +{ + if (mode > 0x0F) return false; + uint16_t value = _readRegister(INA228_ADC_CONFIG, 2); + value &= ~INA228_ADC_MODE; + value |= (mode << 12); + _writeRegister(INA228_ADC_CONFIG, value); + return true; +} + +uint8_t INA228::getMode() +{ + uint16_t value = _readRegister(INA228_ADC_CONFIG, 2); + return (value & INA228_ADC_MODE) >> 12; +} + +bool INA228::setBusVoltageConversionTime(uint8_t bvct) +{ + if (bvct > 7) return false; + uint16_t value = _readRegister(INA228_ADC_CONFIG, 2); + value &= ~INA228_ADC_VBUSCT; + value |= (bvct << 9); + _writeRegister(INA228_ADC_CONFIG, value); + return true; +} + +uint8_t INA228::getBusVoltageConversionTime() +{ + uint16_t value = _readRegister(INA228_ADC_CONFIG, 2); + return (value & INA228_ADC_VBUSCT) >> 9; +} + +bool INA228::setShuntVoltageConversionTime(uint8_t svct) +{ + if (svct > 7) return false; + uint16_t value = _readRegister(INA228_ADC_CONFIG, 2); + value &= ~INA228_ADC_VSHCT; + value |= (svct << 6); + _writeRegister(INA228_ADC_CONFIG, value); + return true; +} + +uint8_t INA228::getShuntVoltageConversionTime() +{ + uint16_t value = _readRegister(INA228_ADC_CONFIG, 2); + return (value & INA228_ADC_VSHCT) >> 6; +} + +bool INA228::setTemperatureConversionTime(uint8_t tct) +{ + if (tct > 7) return false; + uint16_t value = _readRegister(INA228_ADC_CONFIG, 2); + value &= ~INA228_ADC_VTCT; + value |= (tct << 3); + _writeRegister(INA228_ADC_CONFIG, value); + return true; +} + +uint8_t INA228::getTemperatureConversionTime() +{ + uint16_t value = _readRegister(INA228_ADC_CONFIG, 2); + return (value & INA228_ADC_VTCT) >> 3; +} + +bool INA228::setAverage(uint8_t avg) +{ + if (avg > 7) return false; + uint16_t value = _readRegister(INA228_ADC_CONFIG, 2); + value &= ~INA228_ADC_AVG; + value |= avg; + _writeRegister(INA228_ADC_CONFIG, value); + return true; +} + +uint8_t INA228::getAverage() +{ + uint16_t value = _readRegister(INA228_ADC_CONFIG, 2); + return (value & INA228_ADC_AVG); +} + + +//////////////////////////////////////////////////////// +// +// SHUNT CALIBRATION REGISTER 2 +// +void INA228::calibrate(float rShuntValue, float iMaxExpected) +{ + _shunt = rShuntValue; + _maxCurrent = iMaxExpected; + _current_LSB = _maxCurrent * 1.9073486328125e-6; // pow(2, -19); + + // PAGE 31 (8.1.2) + float shunt_cal = 13107.2e6 * _current_LSB * _shunt; + // depends on ADCRANGE in INA228_CONFIG register. + if (_ADCRange) + { + shunt_cal *= 4; + } + //rounding + shunt_cal += 0.5; + // shunt_cal must be written to REGISTER. + // work in progress PR #7 + _writeRegister(INA228_SHUNT_CAL, shunt_cal); +} + + +float INA228::getMaxCurrent() +{ + return _maxCurrent; +} + +float INA228::getShunt() +{ + return _shunt; +} + +float INA228::getCurrentLSB() +{ + return _current_LSB; +} + +//////////////////////////////////////////////////////// +// +// SHUNT TEMPERATURE COEFFICIENT REGISTER 3 +// +bool INA228::setShuntTemperatureCoefficent(uint16_t ppm) +{ + if (ppm > 16383) return false; + _writeRegister(INA228_SHUNT_TEMP_CO, ppm); + return true; +} + +uint16_t INA228::getShuntTemperatureCoefficent() +{ + uint16_t value = _readRegister(INA228_SHUNT_TEMP_CO, 2); + return value; +} + + + +//////////////////////////////////////////////////////// +// +// DIAGNOSE ALERT REGISTER 11 +// +void INA228::setDiagnoseAlert(uint16_t flags) +{ + _writeRegister(INA228_DIAG_ALERT, flags); +} + +uint16_t INA228::getDiagnoseAlert() +{ + return _readRegister(INA228_DIAG_ALERT, 2); +} + +// INA228.h has an enum for the bit fields. +void INA228::setDiagnoseAlertBit(uint8_t bit) +{ + uint16_t value = _readRegister(INA228_DIAG_ALERT, 2); + uint16_t mask = (1 << bit); + // only write new value if needed. + if ((value & mask) == 0) + { + value |= mask; + _writeRegister(INA228_DIAG_ALERT, value); + } +} + +void INA228::clearDiagnoseAlertBit(uint8_t bit) +{ + uint16_t value = _readRegister(INA228_DIAG_ALERT, 2); + uint16_t mask = (1 << bit); + // only write new value if needed. + if ((value & mask ) != 0) + { + value &= ~mask; + _writeRegister(INA228_DIAG_ALERT, value); + } +} + +uint16_t INA228::getDiagnoseAlertBit(uint8_t bit) +{ + uint16_t value = _readRegister(INA228_DIAG_ALERT, 2); + return (value >> bit) & 0x01; +} + + +//////////////////////////////////////////////////////// +// +// THRESHOLD AND LIMIT REGISTERS 12-17 +// +// TODO - API ? + +void INA228::setShuntOvervoltageTH(uint16_t threshold) +{ + // TODO ADCRANGE DEPENDENT + _writeRegister(INA228_SOVL, threshold); +} + +uint16_t INA228::getShuntOvervoltageTH() +{ + // TODO ADCRANGE DEPENDENT + return _readRegister(INA228_SOVL, 2); +} + +void INA228::setShuntUndervoltageTH(uint16_t threshold) +{ + // TODO ADCRANGE DEPENDENT + _writeRegister(INA228_SUVL, threshold); +} + +uint16_t INA228::getShuntUndervoltageTH() +{ + // TODO ADCRANGE DEPENDENT + return _readRegister(INA228_SUVL, 2); +} + +void INA228::setBusOvervoltageTH(uint16_t threshold) +{ + if (threshold > 0x7FFF) return; + //float LSB = 3.125e-3; + _writeRegister(INA228_BOVL, threshold); +} + +uint16_t INA228::getBusOvervoltageTH() +{ + //float LSB = 3.125e-3; + return _readRegister(INA228_BOVL, 2); +} + +void INA228::setBusUndervoltageTH(uint16_t threshold) +{ + if (threshold > 0x7FFF) return; + //float LSB = 3.125e-3; + _writeRegister(INA228_BUVL, threshold); +} + +uint16_t INA228::getBusUndervoltageTH() +{ + //float LSB = 3.125e-3; + return _readRegister(INA228_BUVL, 2); +} + +void INA228::setTemperatureOverLimitTH(uint16_t threshold) +{ + //float LSB = 7.8125e-3; // milliCelsius + _writeRegister(INA228_TEMP_LIMIT, threshold); +} + +uint16_t INA228::getTemperatureOverLimitTH() +{ + //float LSB = 7.8125e-3; // milliCelsius + return _readRegister(INA228_TEMP_LIMIT, 2); +} + +void INA228::setPowerOverLimitTH(uint16_t threshold) +{ + // P29 + // Conversion factor: 256 × Power LSB. + _writeRegister(INA228_POWER_LIMIT, threshold); +} + +uint16_t INA228::getPowerOverLimitTH() +{ + // P29 + // Conversion factor: 256 × Power LSB. + return _readRegister(INA228_POWER_LIMIT, 2); +} + + +//////////////////////////////////////////////////////// +// +// MANUFACTURER and ID REGISTER 3E/3F +// +uint16_t INA228::getManufacturer() +{ + uint16_t value = _readRegister(INA228_MANUFACTURER, 2); + return value; +} + +uint16_t INA228::getDieID() +{ + uint16_t value = _readRegister(INA228_DEVICE_ID, 2); + return (value >> 4) & 0x0FFF; +} + +uint16_t INA228::getRevision() +{ + uint16_t value = _readRegister(INA228_DEVICE_ID, 2); + return value & 0x000F; +} + + +//////////////////////////////////////////////////////// +// +// SHOULD BE PRIVATE +// +uint32_t INA228::_readRegister(uint8_t reg, uint8_t bytes) +{ + _wire->beginTransmission(_address); + _wire->write(reg); + _wire->endTransmission(); + + _wire->requestFrom(_address, (uint8_t)bytes); + uint32_t value = 0; + for (int i = 0; i < bytes; i++) + { + value <<= 8; + value |= _wire->read(); + } + return value; +} + + +double INA228::_readRegisterF(uint8_t reg, uint8_t bytes) +{ + _wire->beginTransmission(_address); + _wire->write(reg); + _wire->endTransmission(); + + _wire->requestFrom(_address, (uint8_t)bytes); + double value = 0; + for (int i = 0; i < bytes; i++) + { + value *= 256.0; + value += _wire->read(); + } + return value; +} + + +uint16_t INA228::_writeRegister(uint8_t reg, uint16_t value) +{ + _wire->beginTransmission(_address); + _wire->write(reg); + _wire->write(value >> 8); + _wire->write(value & 0xFF); + return _wire->endTransmission(); +} + + +// -- END OF FILE -- + diff --git a/src/madflight/bat/INA228/LICENSE b/src/madflight/bat/INA228/LICENSE new file mode 100644 index 0000000..37fe70e --- /dev/null +++ b/src/madflight/bat/INA228/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2024-2024 Rob Tillaart + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/src/madflight/bat/INA228/README.md b/src/madflight/bat/INA228/README.md new file mode 100644 index 0000000..863a813 --- /dev/null +++ b/src/madflight/bat/INA228/README.md @@ -0,0 +1,521 @@ + +[![Arduino CI](https://github.com/RobTillaart/INA228/workflows/Arduino%20CI/badge.svg)](https://github.com/marketplace/actions/arduino_ci) +[![Arduino-lint](https://github.com/RobTillaart/INA228/actions/workflows/arduino-lint.yml/badge.svg)](https://github.com/RobTillaart/INA228/actions/workflows/arduino-lint.yml) +[![JSON check](https://github.com/RobTillaart/INA228/actions/workflows/jsoncheck.yml/badge.svg)](https://github.com/RobTillaart/INA228/actions/workflows/jsoncheck.yml) +[![GitHub issues](https://img.shields.io/github/issues/RobTillaart/INA228.svg)](https://github.com/RobTillaart/INA228/issues) + +[![License: MIT](https://img.shields.io/badge/license-MIT-green.svg)](https://github.com/RobTillaart/INA228/blob/master/LICENSE) +[![GitHub release](https://img.shields.io/github/release/RobTillaart/INA228.svg?maxAge=3600)](https://github.com/RobTillaart/INA228/releases) +[![PlatformIO Registry](https://badges.registry.platformio.org/packages/robtillaart/library/INA228.svg)](https://registry.platformio.org/libraries/robtillaart/INA228) + + +# INA228 + +Arduino library for the INA228 power sensor. + + +## Description + +**Experimental** + +This library controls the INA228, a device that measures voltage, +current, power, temperature and more. + +The INA228 sensor differs from the better known INA226. +Most important difference is that the INA228 has a 20 bit ADC. +This should result in higher precision however this is expected to only +be visible with stable loads and low noise. + +Another important difference is that the INA228 works up to 85 Volts, +which is more than twice the 36 volt of the INA226. +The INA228 has a build in temperature sensor (±1°C) to be used for +monitoring and temperature compensation. + +Finally the INA228 has an **energy** and **charge** register. +These are values accumulated over time, and only work in continuous mode. +(to be investigated what those mean ). + +The INA228 also provides an alert line, to generate an interrupt +in case a predefined threshold has been met. +This can be an under- or over-voltage, temperature or power limit. + +The library is limited tested and verified with hardware. + +==> **USE WITH CARE** + +Feedback as always is welcome. + + +### Details + +The INA228 is a voltage, current and power measurement device. +A few important data, Read the datasheet for the details, +Section 7, Page 12++. + + +| description | value | notes | +|:---------------|:-----------:|:--------| +| bus voltage | 85 Volt | unclear for how long. +| ADC | 20 bit | +| alert timing | 75 µs. | + + +Two breakout boards are known, with completely different maximum current. +This max current depends on the width of the traces on the PCB and the +shunt used. See the links below for more details. + + +| supplier | Shunt Ω | Current | Shunt V | URL | +|:----------:|:--------:|:---------:|:---------:|:------| +| Adafruit | 0.015 | 10 Amp | 150 mV | https://www.adafruit.com/product/5832 +| Mateksys | 0.0002 | 204 Amp | 41 mV | https://www.mateksys.com/?portfolio=i2c-ina-bm + + +Both are verified to work but not the full range. (See #10) + + +### Calibrating + +Note the power and the current are not meaningful without calibrating the sensor. +Also the values are not meaningful if there is no shunt connected. + + +### Schema LOW SIDE + + +``` + GND VCC + | | + | | + | +----[ LOAD ]----+ + | | | + | | | + /-------------------------------------\ + | VIN- VIN+ VBUS | + | | + | | + | INA 228 BREAKOUT | + | | + | | + \-------------------------------------/ + +``` + +_Tested_ + + +### Schema HIGH SIDE + + +``` + GND VCC + | | + | | + +---[ LOAD ]---+ +----+ + | | | + | | | + /-------------------------------------\ + | VIN- VIN+ VBUS | + | | + | | + | INA 228 BREAKOUT | + | | + | | + \-------------------------------------/ + +``` + +_Tested_ + + +### Special characters + +- Ω == Ohm = ALT-234 (Windows) +- µ == micro = ALT-0181 (Windows) +- ° == degree = ALT-0176 (Windows) +- ± == plus minus = ALT-0177 (Windows) + + +### Related + +- https://www.ti.com/product/INA228#tech-docs +- https://www.ti.com/product/INA228#params +- https://www.ti.com/document-viewer/INA228/datasheet +- https://github.com/RobTillaart/INA219 +- https://github.com/RobTillaart/INA226 +- https://github.com/RobTillaart/INA228 +- https://github.com/RobTillaart/INA3221_RT +- https://www.adafruit.com/product/5832 +- https://www.mateksys.com/?portfolio=i2c-ina-bm +- https://github.com/RobTillaart/printHelpers (for scientific notation) + + +## I2C + +### Address + +The sensor can have 16 different I2C addresses, +which depends on how the A0 and A1 address lines +are connected to the SCL, SDA, GND and VCC pins. + +See table - from datasheet table 7-2, page 19. + +| A1 | A0 | Addr | HEX | | A1 | A0 | Addr | HEX | +|:-----:|:-----:|:------:|:------:|:-:|:-----:|:-----:|:------:|:------:| +| GND | GND | 64 | 0x40 | | SDA | GND | 72 | 0x48 | +| GND | VCC | 65 | 0x41 | | SDA | VCC | 73 | 0x49 | +| GND | SDA | 66 | 0x42 | | SDA | SDA | 74 | 0x4A | +| GND | SCL | 67 | 0x43 | | SDA | SCL | 75 | 0x4B | +| VCC | GND | 68 | 0x44 | | SCL | GND | 76 | 0x4C | +| VCC | VCC | 69 | 0x45 | | SCL | VCC | 77 | 0x4D | +| VCC | SDA | 70 | 0x46 | | SCL | SDA | 78 | 0x4E | +| VCC | SCL | 71 | 0x47 | | SCL | SCL | 79 | 0x4F | + + +Note this might differ per breakout board. + + +### Performance + +Run **INA228_performance.ino** sketch to get a first indication. +Numbers below are based upon tests with the Adafruit board. + +Time in micros, I2C speed in kHz. + +| I2C | function | time | notes | +|:-----:|:------------------|:------:|:-------:| +| 100 | getBusVoltage | 684 | 100% +| 100 | getShuntVoltage | 1248 | +| 100 | getCurrent | 684 | +| 100 | getPower | 684 | +| 100 | getTemperature | 580 | +| 100 | getEnergy | 940 | +| 100 | getCharge | 960 | +| | | | other functions similar gain. +| 200 | getBusVoltage | 404 | 60% +| 400 | getBusVoltage | 252 | 37% +| 800 | getBusVoltage | 184 | 27% + + +Most non core functions are as fast as **getTemperature()** + + +### I2C multiplexing + +Sometimes you need to control more devices than possible with the default +address range the device provides. +This is possible with an I2C multiplexer e.g. TCA9548 which creates up +to eight channels (think of it as I2C subnets) which can use the complete +address range of the device. + +Drawback of using a multiplexer is that it takes more administration in +your code e.g. which device is on which channel. +This will slow down the access, which must be taken into account when +deciding which devices are on which channel. +Also note that switching between channels will slow down other devices +too if they are behind the multiplexer. + +- https://github.com/RobTillaart/TCA9548 + + +## Interface + +```cpp +#include "INA228.h" +``` + +### Constructor + +- **INA228(const uint8_t address, TwoWire \*wire = Wire)** Constructor to set +the address and optional Wire interface. +- **bool begin()** initializes the class. +Returns true if the INA228 address is on the I2C bus. + - Note: one needs to set **Wire.begin()** before calling **begin()**. + - Note: call **setMaxCurrentShunt(maxCurrent, shunt)** to calibrate your INA228 +- **bool isConnected()** returns true if the INA228 address is on the I2C bus. +- **uint8_t getAddress()** returns the address set in the constructor. + +### BUS VOLTAGE + +Main function + wrappers. + +- **float getBusVoltage()** idem. Returns value in volts. Max 85 Volt. +This value is always positive. +- **float getBusVolt()** +- **float getBusMilliVolt()** +- **float getBusMicroVolt()** + +### SHUNT VOLTAGE + +- **float getShuntVoltage()** idem, Returns value in volts. +Note the value can be positive or negative as the INA228 is bidirectional. +- **float getShuntVolt()** +- **float getShuntMilliVolt()** +- **float getShuntMicroVolt()** + +### SHUNT CURRENT + +- **float getCurrent()** returns the current through the shunt in Ampere. +Note this value can be positive or negative as the INA228 is bidirectional. +- **float getAmpere()** +- **float getMilliAmpere()** +- **float getMicroAmpere()** + +### TEMPERATURE + +- **float getTemperature()** returns the temperature in Celsius. + +### POWER + +- **float getPower()** returns the current x BusVoltage in Watt. +- **float getWatt()** +- **float getMilliWatt()** +- **float getMicroWatt()** +- **float getKiloWatt()** + +### ENERGY + +See page 13++, page 32, 8.1.2 + +The **getEnergy()** only has meaning in continuous mode. +This is an accumulation register and can be reset to zero by **setAccumulation(1)**. + +The accuracy of **getEnergy()** is 1.0% full scale (maximum). + +- **double getEnergy()** return Joule (elaborate). +- **double getJoule()** +- **double getMegaJoule()** +- **double getKiloJoule()** +- **double getMilliJoule()** +- **double getMicroJoule()** +- **double getWattHour()** +- **double getKiloWattHour()** + +### CHARGE + +The **getCharge()** only has meaning in continuous mode. +This is an accumulation register and can be reset to zero by **setAccumulation(1)**. + +The accuracy of **getCharge()** is 1.0% full scale (maximum). + +- **double getCharge()** return Coulomb (elaborate). +- **double getCoulomb()** +- **double getMilliCoulomb()** +- **double getMicroCoulomb()** + + +### Configuration + +Read datasheet for details, section 7.6.1.1, page 22 + +- **void reset()** Resets the device, be aware that you need to calibrate the sensor +(shunt register) again ==> call **setMaxCurrentShunt()** and more. +- **bool setAccumulation(uint8_t value)** value: 0 == normal operation, +1 = clear Energy and Charge registers. +- **bool getAccumulation()** return set value. (TODO check). +- **void setConversionDelay(uint8_t steps)** Conversion delay in 0..255 steps of 2 ms +- **uint8_t getConversionDelay()** return set value. +- **void setTemperatureCompensation(bool on)** see Shunt temperature coefficient below. +- **bool getTemperatureCompensation()** return set value. +- **void setADCRange(bool flag)** flag = false => 164 mV, true => 41 mV +- **bool getADCRange()** return set value. + +TODO: wrapper + better name for setAccumulation(). + +TODO: examples to show the effect of the ADC configuration. + + +### ADC mode + +Read datasheet for details, section 7.6.1.2, page 22++ + +- **bool setMode(uint8_t mode = INA228_MODE_CONT_TEMP_BUS_SHUNT)** default all on. +- **uint8_t getMode()** return set value. + +| MODE | value | notes | +|:--------------------------------|:-------:|:--------| +| INA228_MODE_SHUTDOWN | 0x00 | See 0x08. +| INA228_MODE_TRIG_BUS | 0x01 | +| INA228_MODE_TRIG_SHUNT | 0x02 | +| INA228_MODE_TRIG_BUS_SHUNT | 0x03 | +| INA228_MODE_TRIG_TEMP | 0x04 | +| INA228_MODE_TRIG_TEMP_BUS | 0x05 | +| INA228_MODE_TRIG_TEMP_SHUNT | 0x06 | +| INA228_MODE_TRIG_TEMP_BUS_SHUNT | 0x07 | +| INA228_MODE_SHUTDOWN2 | 0x08 | There are two shutdowns. +| INA228_MODE_CONT_BUS | 0x09 | +| INA228_MODE_CONT_SHUNT | 0x0A | +| INA228_MODE_CONT_BUS_SHUNT | 0x0B | +| INA228_MODE_CONT_TEMP | 0x0C | +| INA228_MODE_CONT_TEMP_BUS | 0x0D | +| INA228_MODE_CONT_TEMP_SHUNT | 0x0E | +| INA228_MODE_CONT_TEMP_BUS_SHUNT | 0x0F | + + +### ADC conversion time + +- **bool setBusVoltageConversionTime(uint8_t bvct = INA226_1052_us)** +- **uint8_t getBusVoltageConversionTime()** return set value. +- **bool setShuntVoltageConversionTime(uint8_t svct = INA226_1052_us)** +- **uint8_t getShuntVoltageConversionTime()** return set value. +- **bool setTemperatureConversionTime(uint8_t tct = INA226_1052_us)** +- **uint8_t getTemperatureConversionTime()** return set value. + +| TIMING | value | notes | +|:--------------------|:-------:|:--------| +| INA228_50_us | 0 | +| INA228_84_us | 1 | +| INA228_150_us | 2 | +| INA228_280_us | 3 | +| INA228_540_us | 4 | +| INA228_1052_us | 5 | +| INA228_2074_us | 6 | +| INA228_4120_us | 7 | + + +- **bool setAverage(uint8_t avg = INA228_1_SAMPLE)** +- **uint8_t getAverage()** return set value. + +| AVERAGE | value | notes | +|:--------------------|:-------:|:--------| +| INA228_1_SAMPLE | 0 | +| INA228_4_SAMPLES | 1 | +| INA228_16_SAMPLES | 2 | +| INA228_64_SAMPLES | 3 | +| INA228_128_SAMPLES | 4 | +| INA228_256_SAMPLES | 5 | +| INA228_512_SAMPLES | 6 | +| INA228_1024_SAMPLES | 7 | + + +### Shunt Calibration + +To elaborate, read datasheet for details. + +Note: **setMaxCurrentShunt()** must be called to calibrate your sensor. +Otherwise several functions will return zero or incorrect data. + +- **int setMaxCurrentShunt(float maxCurrent, float shunt)** The maxCurrent +depends on breakout used, See section above. +The shunt should be 0.0001 Ω and up. + - returns 0 if OK. + - returns -2 if shunt < 0.0001 Ohm. ( Mateksys == 0.0002 Ω ) +- **bool isCalibrated()** is valid calibration value. The currentLSB > 0. +- **float getMaxCurrent()** return set value. +- **float getShunt()** return set value. +- **float getCurrentLSB()** return actual currenLSB. 0.0 means not calibrated. + + +### Shunt temperature coefficient + +Read datasheet for details, page 16. + +The INA228 can compensate for shunt temperature variance to increase accuracy. +The reference temperature is 25°C. +- Enter the coefficient with **setShuntTemperatureCoefficent(uint16_t ppm)**. +- Enable the function with **setTemperatureCompensation(true)**. + +In formula: +``` +Radjusted = Rnominal + (Rnominal x (temperature - 25) x PPM) * 10e-6; +``` + +- **bool setShuntTemperatureCoefficent(uint16_t ppm = 0)** ppm = 0..16383 ppm/°C. +Default 0 for easy reset. +Returns false if ppm is out of range. +- **uint16_t getShuntTemperatureCoefficent()** returns the set value (default 0). + + +### Diagnose alert + +Read datasheet for details, section 7.6.1.12, page 26++. + +- **void setDiagnoseAlert(uint16_t flags)** set all flags as bit mask. +- **uint16_t getDiagnoseAlert()** return all flags as bit mask. + +INA228.h has an enum for the bit fields. + +- **void setDiagnoseAlertBit(uint8_t bit)** set individual bit. +- **void clearDiagnoseAlertBit(uint8_t bit)** clear individual bit. +- **uint16_t getDiagnoseAlertBit(uint8_t bit)** return individual bit. + + +### Threshold and Limits + +Read datasheet for details, section 7.3.7, page 16++ + +Note: the implementation of this part is rather minimalistic and +might be changed / extended in the future. + +#### Shunt + +- **void setShuntOvervoltageTH(uint16_t threshold)** +- **uint16_t getShuntOvervoltageTH()** +- **void setShuntUndervoltageTH(uint16_t threshold)** +- **uint16_t getShuntUndervoltageTH()** + +#### Bus + +- **void setBusOvervoltageTH(uint16_t threshold)** +- **uint16_t getBusOvervoltageTH()** +- **void setBusUndervoltageTH(uint16_t threshold)** +- **uint16_t getBusUndervoltageTH()** + +#### Temperature + +- **void setTemperatureOverLimitTH(uint16_t threshold)** +- **uint16_t getTemperatureOverLimitTH()** + +#### Power + +- **void setPowerOverLimitTH(uint16_t threshold)** +- **uint16_t getPowerOverLimitTH()** + + +### Manufacturer and ID + +- **bool getManufacturer()** Returns 0x5449, can be used to check right sensor. +- **uint16_t getDieID()** Returns 0x228, can be used to check right sensor. +- **uint16_t getRevision()** Returns revision, probably 0x01. + + +## Future + + +#### Must + +- update documentation. +- test and verify. +- DiagnoseAlertBit functions + - redo API (0.2.0) + +#### Should + +- TODO's in code and docs. +- add error handling. +- keep in sync with INA226 where possible. +- how to detect nothing connected? + - vshunt > maxVShunt (new variable) + - current > maxCurrent + +#### Could + +- write examples, (please share yours). +- improve unit tests +- clean up magic numbers in the code + +#### Won't + +- cache registers for performance + - first get library working / tested. + - reset should reread all cached values... + + +## Support + +If you appreciate my libraries, you can support the development and maintenance. +Improve the quality of the libraries by providing issues and Pull Requests, or +donate through PayPal or GitHub sponsors. + +Thank you, + diff --git a/src/madflight/bat/bat.h b/src/madflight/bat/bat.h index 80b4c6e..be3a639 100644 --- a/src/madflight/bat/bat.h +++ b/src/madflight/bat/bat.h @@ -7,6 +7,7 @@ Each BAT_USE_xxx section in this file defines a specific Battery class #define BAT_USE_NONE 1 #define BAT_USE_ADC 2 #define BAT_USE_INA226 3 +#define BAT_USE_INA228 4 #include "../interface.h" @@ -54,6 +55,11 @@ BatteryADC bat_instance; //================================================================================================= #elif BAT_USE == BAT_USE_INA226 +// Default INA226 address is 0x40 +#ifndef BAT_I2C_ADR + #define BAT_I2C_ADR 0x40 +#endif + #include "INA226/INA226.h" INA226 bat_ina226; @@ -62,13 +68,11 @@ class BatteryINA226: public Battery { void setup() { float Rshunt = cfg.BAT_CAL_I; //ohm - // Default INA226 address is 0x40 - bat_ina226.begin(i2c); + bat_ina226.begin(i2c, BAT_I2C_ADR); // Configure INA226 -> sample time = 2 * 128 * 140us = 36ms => 28Hz bat_ina226.configure(INA226_AVERAGES_128, INA226_BUS_CONV_TIME_140US, INA226_SHUNT_CONV_TIME_140US, INA226_MODE_SHUNT_BUS_CONT); - // Calibrate INA226. bat_ina226.calibrate(Rshunt); } bool update() { @@ -89,6 +93,47 @@ class BatteryINA226: public Battery { BatteryINA226 bat_instance; +//================================================================================================= +// INA228 Sensor +//================================================================================================= +#elif BAT_USE == BAT_USE_INA228 + +// Default INA228 address is 0x40 +#ifndef BAT_I2C_ADR + #define BAT_I2C_ADR 0x40 +#endif + +#include "INA228/INA228.h" +INA228 bat_ina228; + +class BatteryINA228: public Battery { + public: + void setup() { + float Rshunt = cfg.BAT_CAL_I; //ohm + + // Configure INA226 -> sample time = 512 * 2 * 50us = 51.2ms => 20Hz + bat_ina228.begin(i2c, BAT_I2C_ADR); + bat_ina228.setADCRange(false); // false => 164 mV, true => 41 mV + bat_ina228.setBusVoltageConversionTime(INA228_50_us); + bat_ina228.setShuntVoltageConversionTime(INA228_50_us); + bat_ina228.setTemperatureConversionTime(INA228_50_us); + bat_ina228.setAverage(INA228_512_SAMPLES); + bat_ina228.calibrate(Rshunt); + bat_ina228.setMode(INA228_MODE_CONT_BUS_SHUNT); + } + bool update() { + if(!bat_ina228.isConversionReady()) return false; + i = bat_ina228.getAmpere(); + v = bat_ina228.getBusVolt(); + w = bat_ina228.getWatt(); + mah = bat_ina228.getCoulomb() * (1000.0 / 3600); + wh = bat_ina228.getWattHour(); + return true; + } +}; + +BatteryINA228 bat_instance; + //================================================================================================= // Invalid value //================================================================================================= diff --git a/src/madflight/rcin/rcin.h b/src/madflight/rcin/rcin.h index f06b974..7c7b6c0 100644 --- a/src/madflight/rcin/rcin.h +++ b/src/madflight/rcin/rcin.h @@ -26,24 +26,18 @@ SOFTWARE. #pragma once -#include "../interface.h" //RCIN interface definition -#include "../cfg/cfg.h" -#include "rcin_calibrate.h" - +//set defaults #ifndef RCIN_NUM_CHANNELS - #define RCIN_NUM_CHANNELS 8 //number of receiver channels (minimal 6) + #define RCIN_NUM_CHANNELS 8 //number of receiver channels #endif - #ifndef RCIN_TIMEOUT #define RCIN_TIMEOUT 3000 // lost connection timeout in milliseconds #endif - #ifndef RCIN_STICK_DEADBAND - #define RCIN_STICK_DEADBAND 0 //pwm deadband around stick center + #define RCIN_STICK_DEADBAND 0 //pwm deadband around stick center in microseconds #endif - #ifndef RCIN_THROTTLE_DEADBAND - #define RCIN_THROTTLE_DEADBAND 60 //pwm deadband for zero throttle + #define RCIN_THROTTLE_DEADBAND 60 //pwm deadband for zero throttle in microseconds #endif #define RCIN_USE_NONE 0 @@ -54,6 +48,10 @@ SOFTWARE. #define RCIN_USE_PWM 5 #define RCIN_USE_DEBUG 6 +#include "../interface.h" //RCIN interface definition +#include "../cfg/cfg.h" +#include "rcin_calibrate.h" + //Rcin implements public interface, and is base for specific rcin radio classes class Rcin : public Rcin_interface { public: