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: Implement temperature sensor support for INA228 / INA238

Browse Source
This commit is contained in:
Alexander Lerach 2024-09-06 05:09:01 +02:00 committed by GitHub
parent f98eb067be
commit 3d36c8519d
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
8 changed files with 71 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
msg/BatteryStatus.msg
View File

@ -1,67 +1,67 @@
uint64 timestamp # time since system start (microseconds)
bool connected # Whether or not a battery is connected, based on a voltage threshold
float32 voltage_v # Battery voltage in volts, 0 if unknown
float32 current_a # Battery current in amperes, -1 if unknown
float32 current_average_a # Battery current average in amperes (for FW average in level flight), -1 if unknown
float32 discharged_mah # Discharged amount in mAh, -1 if unknown
float32 remaining # From 1 to 0, -1 if unknown
float32 scale # Power scaling factor, >= 1, or -1 if unknown
float32 time_remaining_s # predicted time in seconds remaining until battery is empty under previous averaged load, NAN if unknown
float32 temperature # temperature of the battery. NaN if unknown
uint8 cell_count # Number of cells, 0 if unknown
uint64 timestamp # time since system start (microseconds)
bool connected # Whether or not a battery is connected, based on a voltage threshold
float32 voltage_v # Battery voltage in volts, 0 if unknown
float32 current_a # Battery current in amperes, -1 if unknown
float32 current_average_a # Battery current average in amperes (for FW average in level flight), -1 if unknown
float32 discharged_mah # Discharged amount in mAh, -1 if unknown
float32 remaining # From 1 to 0, -1 if unknown
float32 scale # Power scaling factor, >= 1, or -1 if unknown
float32 time_remaining_s # predicted time in seconds remaining until battery is empty under previous averaged load, NAN if unknown
float32 temperature # Temperature of the battery in degrees Celcius, NaN if unknown
uint8 cell_count # Number of cells, 0 if unknown
uint8 BATTERY_SOURCE_POWER_MODULE = 0
uint8 BATTERY_SOURCE_EXTERNAL = 1
uint8 BATTERY_SOURCE_ESCS = 2
uint8 source # Battery source
uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1
uint16 capacity # actual capacity of the battery
uint16 cycle_count # number of discharge cycles the battery has experienced
uint16 average_time_to_empty # predicted remaining battery capacity based on the average rate of discharge in min
uint16 serial_number # serial number of the battery pack
uint16 manufacture_date # manufacture date, part of serial number of the battery pack. Formatted as: Day + Month×32 + (Year–1980)×512
uint16 state_of_health # state of health. FullChargeCapacity/DesignCapacity, 0-100%.
uint16 max_error # max error, expected margin of error in % in the state-of-charge calculation with a range of 1 to 100%
uint8 source # Battery source
uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1
uint16 capacity # actual capacity of the battery
uint16 cycle_count # number of discharge cycles the battery has experienced
uint16 average_time_to_empty # predicted remaining battery capacity based on the average rate of discharge in min
uint16 serial_number # serial number of the battery pack
uint16 manufacture_date # manufacture date, part of serial number of the battery pack. Formatted as: Day + Month×32 + (Year–1980)×512
uint16 state_of_health # state of health. FullChargeCapacity/DesignCapacity, 0-100%.
uint16 max_error # max error, expected margin of error in % in the state-of-charge calculation with a range of 1 to 100%
uint8 id # ID number of a battery. Should be unique and consistent for the lifetime of a vehicle. 1-indexed.
uint16 interface_error # interface error counter
uint16 interface_error # interface error counter
float32[14] voltage_cell_v # Battery individual cell voltages, 0 if unknown
float32 max_cell_voltage_delta # Max difference between individual cell voltages
float32[14] voltage_cell_v # Battery individual cell voltages, 0 if unknown
float32 max_cell_voltage_delta # Max difference between individual cell voltages
bool is_powering_off # Power off event imminent indication, false if unknown
bool is_required # Set if the battery is explicitly required before arming
bool is_powering_off # Power off event imminent indication, false if unknown
bool is_required # Set if the battery is explicitly required before arming
uint8 BATTERY_WARNING_NONE = 0 # no battery low voltage warning active
uint8 BATTERY_WARNING_LOW = 1 # warning of low voltage
uint8 BATTERY_WARNING_CRITICAL = 2 # critical voltage, return / abort immediately
uint8 BATTERY_WARNING_EMERGENCY = 3 # immediate landing required
uint8 BATTERY_WARNING_FAILED = 4 # the battery has failed completely
uint8 BATTERY_STATE_UNHEALTHY = 6 # Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in faults field.
uint8 BATTERY_STATE_CHARGING = 7 # Battery is charging
uint8 BATTERY_WARNING_NONE = 0 # no battery low voltage warning active
uint8 BATTERY_WARNING_LOW = 1 # warning of low voltage
uint8 BATTERY_WARNING_CRITICAL = 2 # critical voltage, return / abort immediately
uint8 BATTERY_WARNING_EMERGENCY = 3 # immediate landing required
uint8 BATTERY_WARNING_FAILED = 4 # the battery has failed completely
uint8 BATTERY_STATE_UNHEALTHY = 6 # Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in faults field.
uint8 BATTERY_STATE_CHARGING = 7 # Battery is charging
uint8 BATTERY_FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged
uint8 BATTERY_FAULT_SPIKES = 1 # Voltage spikes
uint8 BATTERY_FAULT_CELL_FAIL= 2 # One or more cells have failed
uint8 BATTERY_FAULT_OVER_CURRENT = 3 # Over-current
uint8 BATTERY_FAULT_OVER_TEMPERATURE = 4 # Over-temperature
uint8 BATTERY_FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault
uint8 BATTERY_FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with battery one
uint8 BATTERY_FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware
uint8 BATTERY_FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system
uint8 BATTERY_FAULT_HARDWARE_FAILURE = 9 # hardware problem
uint8 BATTERY_FAULT_FAILED_TO_ARM = 10 # Battery had a problem while arming
uint8 BATTERY_FAULT_COUNT = 11 # Counter - keep it as last element!
uint8 BATTERY_FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged
uint8 BATTERY_FAULT_SPIKES = 1 # Voltage spikes
uint8 BATTERY_FAULT_CELL_FAIL= 2 # One or more cells have failed
uint8 BATTERY_FAULT_OVER_CURRENT = 3 # Over-current
uint8 BATTERY_FAULT_OVER_TEMPERATURE = 4 # Over-temperature
uint8 BATTERY_FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault
uint8 BATTERY_FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with battery one
uint8 BATTERY_FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware
uint8 BATTERY_FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system
uint8 BATTERY_FAULT_HARDWARE_FAILURE = 9 # hardware problem
uint8 BATTERY_FAULT_FAILED_TO_ARM = 10 # Battery had a problem while arming
uint8 BATTERY_FAULT_COUNT = 11 # Counter - keep it as last element!
uint16 faults # Smart battery supply status/fault flags (bitmask) for health indication.
uint8 warning # Current battery warning
uint16 faults # Smart battery supply status/fault flags (bitmask) for health indication.
uint8 warning # Current battery warning
uint8 MAX_INSTANCES = 4
float32 full_charge_capacity_wh # The compensated battery capacity
float32 remaining_capacity_wh # The compensated battery capacity remaining
uint16 over_discharge_count # Number of battery overdischarge
float32 nominal_voltage # Nominal voltage of the battery pack
float32 full_charge_capacity_wh # The compensated battery capacity
float32 remaining_capacity_wh # The compensated battery capacity remaining
uint16 over_discharge_count # Number of battery overdischarge
float32 nominal_voltage # Nominal voltage of the battery pack
float32 internal_resistance_estimate # [Ohm] Internal resistance per cell estimate
float32 ocv_estimate # [V] Open circuit voltage estimate

4
src/drivers/power_monitor/ina228/ina228.cpp
View File

@ -88,6 +88,7 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) :
_battery.setConnected(false);
_battery.updateVoltage(0.f);
_battery.updateCurrent(0.f);
_battery.updateTemperature(0.f);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
}
@ -306,6 +307,7 @@ INA228::collect()
// success = success && (read(INA228_REG_POWER, _power) == PX4_OK);
success = success && (read(INA228_REG_CURRENT, _current) == PX4_OK);
//success = success && (read(INA228_REG_VSHUNT, _shunt) == PX4_OK);
success = success && (read(INA228_REG_DIETEMP, _temperature) == PX4_OK);
if (!success) {
PX4_DEBUG("error reading from sensor");
@ -315,6 +317,7 @@ INA228::collect()
_battery.setConnected(success);
_battery.updateVoltage(static_cast<float>(_bus_voltage * INA228_VSCALE));
_battery.updateCurrent(static_cast<float>(_current * _current_lsb));
_battery.updateTemperature(static_cast<float>(_temperature * INA228_TSCALE));
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
perf_end(_sample_perf);
@ -381,6 +384,7 @@ INA228::RunImpl()
_battery.setConnected(false);
_battery.updateVoltage(0.f);
_battery.updateCurrent(0.f);
_battery.updateTemperature(0.f);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
if (init() != PX4_OK) {

2
src/drivers/power_monitor/ina228/ina228.h
View File

@ -291,6 +291,7 @@ using namespace time_literals;
#define INA228_CONST 13107.2e6f /* is an internal fixed value used to ensure scaling is maintained properly */
#define INA228_SHUNT 0.0005f /* Shunt is 500 uOhm */
#define INA228_VSCALE 1.95e-04f /* LSB of voltage is 195.3125 uV/LSB */
#define INA228_TSCALE 7.8125e-03f /* LSB of temperature is 7.8125 mDegC/LSB */
#define swap16(w) __builtin_bswap16((w))
#define swap32(d) __builtin_bswap32((d))
@ -339,6 +340,7 @@ private:
int32_t _bus_voltage{0};
int64_t _power{0};
int32_t _current{0};
int16_t _temperature{0};
int32_t _shunt{0};
int16_t _cal{0};
int16_t _range{INA228_ADCRANGE_HIGH};

5
src/drivers/power_monitor/ina238/ina238.cpp
View File

@ -80,6 +80,7 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) :
_battery.setConnected(false);
_battery.updateVoltage(0.f);
_battery.updateCurrent(0.f);
_battery.updateTemperature(0.f);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
}
@ -224,9 +225,11 @@ int INA238::collect()
bool success{true};
int16_t bus_voltage{0};
int16_t current{0};
int16_t temperature{0};
success = (RegisterRead(Register::VS_BUS, (uint16_t &)bus_voltage) == PX4_OK);
success = success && (RegisterRead(Register::CURRENT, (uint16_t &)current) == PX4_OK);
success = success && (RegisterRead(Register::DIETEMP, (uint16_t &)temperature) == PX4_OK);
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
// check configuration registers periodically or immediately following any failure
@ -250,6 +253,7 @@ int INA238::collect()
_battery.setConnected(success);
_battery.updateVoltage(static_cast<float>(bus_voltage * INA238_VSCALE));
_battery.updateCurrent(static_cast<float>(current * _current_lsb));
_battery.updateTemperature(static_cast<float>(temperature * INA238_TSCALE));
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
perf_end(_sample_perf);
@ -309,6 +313,7 @@ void INA238::RunImpl()
_battery.setConnected(false);
_battery.updateVoltage(0.f);
_battery.updateCurrent(0.f);
_battery.updateTemperature(0.f);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
if (init() != PX4_OK) {

1
src/drivers/power_monitor/ina238/ina238.h
View File

@ -73,6 +73,7 @@ using namespace ina238;
#define INA238_DN_MAX 32768.0f /* 2^15 */
#define INA238_CONST 819.2e6f /* is an internal fixed value used to ensure scaling is maintained properly */
#define INA238_VSCALE 3.125e-03f /* LSB of voltage is 3.1255 mV/LSB */
#define INA238_TSCALE 7.8125e-03f /* LSB of temperature is 7.8125 mDegC/LSB */
#define DEFAULT_MAX_CURRENT 327.68f /* Amps */
#define DEFAULT_SHUNT 0.0003f /* Shunt is 300 uOhm */

1
src/drivers/power_monitor/ina238/ina238_registers.hpp
View File

@ -14,6 +14,7 @@ enum class Register : uint8_t {
ADCCONFIG = 0x01, // ADC Configuration Register
SHUNT_CAL = 0x02, // Shunt Calibration Register
VS_BUS = 0x05,
DIETEMP = 0x06,
CURRENT = 0x07,
MANUFACTURER_ID = 0x3e,
DEVICE_ID = 0x3f

7
src/lib/battery/battery.cpp
View File

@ -106,6 +106,11 @@ void Battery::updateCurrent(const float current_a)
_current_a = current_a;
}
void Battery::updateTemperature(const float temperature_c)
{
_temperature_c = temperature_c;
}
void Battery::updateBatteryStatus(const hrt_abstime &timestamp)
{
// Require minimum voltage otherwise override connected status
@ -149,7 +154,7 @@ battery_status_s Battery::getBatteryStatus()
battery_status.remaining = _state_of_charge;
battery_status.scale = _scale;
battery_status.time_remaining_s = computeRemainingTime(_current_a);
battery_status.temperature = NAN;
battery_status.temperature = _temperature_c;
battery_status.cell_count = _params.n_cells;
battery_status.connected = _connected;
battery_status.source = _source;

2
src/lib/battery/battery.h
View File

@ -91,6 +91,7 @@ public:
void setStateOfCharge(const float soc) { _state_of_charge = soc; _external_state_of_charge = true; }
void updateVoltage(const float voltage_v);
void updateCurrent(const float current_a);
void updateTemperature(const float temperature_c);
/**
* Update state of charge calculations
@ -168,6 +169,7 @@ private:
float _current_a{-1};
AlphaFilter<float>
_current_average_filter_a; ///< averaging filter for current. For FW, it is the current in level flight.
float _temperature_c{NAN};
float _discharged_mah{0.f};
float _discharged_mah_loop{0.f};
float _state_of_charge_volt_based{-1.f}; // [0,1]