diff --git a/cmake/pg_struct_sizes.reference.db b/cmake/pg_struct_sizes.reference.db index 49fa79ce34d..7e544f477a4 100644 --- a/cmake/pg_struct_sizes.reference.db +++ b/cmake/pg_struct_sizes.reference.db @@ -1,7 +1,7 @@ adcChannelConfig_t 4 0 armingConfig_t 6 3 barometerConfig_t 8 5 -batteryMetersConfig_t 24 2 +batteryMetersConfig_t 24 3 beeperConfig_t 12 2 blackboxConfig_t 16 4 compassConfig_t 24 6 diff --git a/docs/Settings.md b/docs/Settings.md index cbc526adb33..d191bf4e2ed 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -696,18 +696,20 @@ This sets the output voltage to current scaling for the current sensor in 0.1 mV ### current_meter_type -ADC, VIRTUAL, FAKE, ESC, SMARTPORT, CAN, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position. +ADC, VIRTUAL, FAKE, ESC, SMARTPORT, CRSF, CAN, INA226, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position. | Allowed Values | | | --- | --- | | NONE | | -| ADC | Default | +| ADC | | | VIRTUAL | | | FAKE | | | ESC | | | SMARTPORT | | | CRSF | | | CAN | | +| INA226 | | +| _target default_ | Default | --- @@ -884,7 +886,7 @@ Re-purpose the craft name field for messages. ### dronecan_bitrate_kbps -The speed of the CANbus network in kbps. Set all devices to the same speed. +The speed of the CANbus network in kbps. Set all devices to the same speed. | Allowed Values | | | --- | --- | @@ -2255,6 +2257,16 @@ Power draw at zero throttle used for remaining flight time/distance estimation i --- +### ina_shunt_res_uohm + +INA226 shunt resistor value in micro-ohms. + +| Default | Min | Max | +| --- | --- | --- | +| _target default_ | 1 | 4294967295 | + +--- + ### inav_allow_dead_reckoning Defines if INAV will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation @@ -6952,17 +6964,19 @@ Maximum voltage per cell in 0.01V units, default is 4.20V ### vbat_meter_type -Vbat voltage source. Possible values: `NONE`, `ADC`, `SMARTPORT`, `ESC`, 'CAN'. `ESC` requires ESC telemetry enabled and running. `SMARTPORT` requires SmartPort Master enabled and running. 'CAN' requires requires dronecan running and a sensor on the bus. +Vbat voltage source. Possible values: `NONE`, `ADC`, `SMARTPORT`, `ESC`, `CRSF`, `CAN`, `INA226`. `ESC` requires ESC telemetry enabled and running. `SMARTPORT` requires SmartPort Master enabled and running. `CRSF` requires CRSF battery telemetry. `CAN` requires DroneCAN running and a sensor on the bus. `INA226` requires an INA226 I2C sensor. | Allowed Values | | | --- | --- | | NONE | | -| ADC | Default | +| ADC | | | ESC | | | FAKE | | | SMARTPORT | | | CRSF | | | CAN | | +| INA226 | | +| _target default_ | Default | --- @@ -7149,4 +7163,3 @@ Defines rotation rate on YAW axis that UAV will try to archive on max. stick def | 20 | 1 | 180 | --- - diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index e060e558f89..4e4136fa286 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -103,6 +103,9 @@ main_sources(COMMON_SRC drivers/adc.c drivers/adc.h + drivers/ina226.c + drivers/ina226.h + drivers/barometer/barometer.h drivers/barometer/barometer_bmp085.c drivers/barometer/barometer_bmp085.h diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f5225ca3e1e..fbb69649a6d 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1911,10 +1911,14 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", getThrottleIdleValue(),getMaxThrottle()); BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G); -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR BLACKBOX_PRINT_HEADER_LINE_CUSTOM( if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { +#ifdef USE_ADC blackboxPrintfHeaderLine("vbat_scale", "%u", batteryMetersConfig()->voltage.scale / 10); +#else + blackboxPrintfHeaderLine("vbat_scale", "%u", 0); +#endif } else { xmitState.headerIndex += 2; // Skip the next two vbat fields too } diff --git a/src/main/common/log.c b/src/main/common/log.c index c2cc30a3a48..a66d3ab0186 100644 --- a/src/main/common/log.c +++ b/src/main/common/log.c @@ -205,9 +205,12 @@ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t { // Print lines of up to maxBytes bytes. We need 5 characters per byte // 0xAB[space|\n] - const size_t charsPerByte = 5; - const size_t maxBytes = 8; - char buf[LOG_PREFIX_FORMATTED_SIZE + charsPerByte * maxBytes + 1]; // +1 for the null terminator + enum { + LOG_HEX_CHARS_PER_BYTE = 5, + LOG_HEX_MAX_BYTES = 8, + LOG_HEX_BUFFER_SIZE = LOG_PREFIX_FORMATTED_SIZE + LOG_HEX_CHARS_PER_BYTE * LOG_HEX_MAX_BYTES + 1, + }; + char buf[LOG_HEX_BUFFER_SIZE]; // +1 for the null terminator size_t bufPos = LOG_PREFIX_FORMATTED_SIZE; const uint8_t *inputPtr = buffer; @@ -219,7 +222,7 @@ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t for (size_t ii = 0; ii < size; ii++) { tfp_sprintf(buf + bufPos, "0x%02x ", inputPtr[ii]); - bufPos += charsPerByte; + bufPos += LOG_HEX_CHARS_PER_BYTE; if (bufPos == sizeof(buf)-1) { buf[bufPos-1] = '\n'; buf[bufPos] = '\0'; diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 385fbd36c0e..e9aa596079b 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -149,6 +149,7 @@ typedef enum { DEVHW_SDCARD, // Generic SD-Card DEVHW_IRLOCK, // IR-Lock visual positioning hardware DEVHW_PCF8574, // 8-bit I/O expander + DEVHW_INA226, // I2C current and voltage monitor } devHardwareType_e; typedef enum { diff --git a/src/main/drivers/ina226.c b/src/main/drivers/ina226.c new file mode 100644 index 00000000000..3b55bc22d94 --- /dev/null +++ b/src/main/drivers/ina226.c @@ -0,0 +1,128 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify this software + * under the terms of the GNU General Public License as published by the Free + * Software Foundation, either version 3 of the License, or (at your option) any + * later version. + * + * INAV is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR + * A PARTICULAR PURPOSE. See the GNU General Public License for more details. + */ + +#include +#include + +#include "platform.h" + +#if defined(USE_INA226) + +#include "drivers/ina226.h" + +#define INA226_REG_SHUNT_VOLTAGE 0x01 +#define INA226_REG_BUS_VOLTAGE 0x02 +#define INA226_REG_MANUFACTURER_ID 0xFE +#define INA226_REG_DIE_ID 0xFF + +#define INA226_MANUFACTURER_ID 0x5449 +#define INA226_DIE_ID 0x2260 +#define INA226_DIE_ID_MASK 0xFFF0 + +static bool ina226ReadRegister(ina226Dev_t *dev, uint8_t reg, uint16_t *value) +{ + if (!dev || !dev->busDev || !value) { + return false; + } + + uint8_t buffer[2]; + if (!busReadBuf(dev->busDev, reg, buffer, sizeof(buffer))) { + return false; + } + + *value = ((uint16_t)buffer[0] << 8) | buffer[1]; + return true; +} + +uint16_t ina226BusVoltageToCentivolts(uint16_t rawBusVoltage) +{ + // INA226 bus voltage LSB is 1.25mV: raw * 125 / 1000 = centivolts. + return ((uint32_t)rawBusVoltage * 125 + 500) / 1000; +} + +int16_t ina226ShuntVoltageToCentiamps(int16_t rawShuntVoltage, uint32_t shuntMicroOhm) +{ + if (shuntMicroOhm == 0) { + return 0; + } + + const int32_t centiAmps = (int32_t)rawShuntVoltage * 250 / (int32_t)shuntMicroOhm; + + if (centiAmps > INT16_MAX) { + return INT16_MAX; + } + + if (centiAmps < INT16_MIN) { + return INT16_MIN; + } + + return centiAmps; +} + +bool ina226Detect(ina226Dev_t *dev) +{ + uint16_t manufacturerId; + uint16_t dieId; + + return ina226ReadRegister(dev, INA226_REG_MANUFACTURER_ID, &manufacturerId) + && ina226ReadRegister(dev, INA226_REG_DIE_ID, &dieId) + && manufacturerId == INA226_MANUFACTURER_ID + && (dieId & INA226_DIE_ID_MASK) == INA226_DIE_ID; +} + +bool ina226Init(ina226Dev_t *dev) +{ + if (!dev) { + return false; + } + + dev->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_INA226, 0, OWNER_CURRENT_METER); + + if (!dev->busDev) { + return false; + } + + if (!ina226Detect(dev)) { + busDeviceDeInit(dev->busDev); + dev->busDev = NULL; + return false; + } + + return true; +} + +bool ina226ReadBusVoltage(ina226Dev_t *dev, uint16_t *centiVolts) +{ + uint16_t rawBusVoltage; + + if (!centiVolts || !ina226ReadRegister(dev, INA226_REG_BUS_VOLTAGE, &rawBusVoltage)) { + return false; + } + + *centiVolts = ina226BusVoltageToCentivolts(rawBusVoltage); + return true; +} + +bool ina226ReadShuntCurrent(ina226Dev_t *dev, uint32_t shuntMicroOhm, int16_t *centiAmps) +{ + uint16_t rawShuntVoltage; + + if (!centiAmps || !ina226ReadRegister(dev, INA226_REG_SHUNT_VOLTAGE, &rawShuntVoltage)) { + return false; + } + + *centiAmps = ina226ShuntVoltageToCentiamps((int16_t)rawShuntVoltage, shuntMicroOhm); + return true; +} + +#endif diff --git a/src/main/drivers/ina226.h b/src/main/drivers/ina226.h new file mode 100644 index 00000000000..199a83a536a --- /dev/null +++ b/src/main/drivers/ina226.h @@ -0,0 +1,33 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify this software + * under the terms of the GNU General Public License as published by the Free + * Software Foundation, either version 3 of the License, or (at your option) any + * later version. + * + * INAV is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR + * A PARTICULAR PURPOSE. See the GNU General Public License for more details. + */ + +#pragma once + +#include +#include + +#include "drivers/bus.h" + +#define INA226_DEFAULT_I2C_ADDRESS 0x40 + +typedef struct ina226Dev_s { + busDevice_t *busDev; +} ina226Dev_t; + +bool ina226Init(ina226Dev_t *dev); +bool ina226Detect(ina226Dev_t *dev); +bool ina226ReadBusVoltage(ina226Dev_t *dev, uint16_t *centiVolts); +bool ina226ReadShuntCurrent(ina226Dev_t *dev, uint32_t shuntMicroOhm, int16_t *centiAmps); + +uint16_t ina226BusVoltageToCentivolts(uint16_t rawBusVoltage); +int16_t ina226ShuntVoltageToCentiamps(int16_t rawShuntVoltage, uint32_t shuntMicroOhm); diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index a66bebb8c02..d3d3aea8773 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -22,7 +22,7 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "RANGEFINDER", "SYSTEM", "SPI", "QUADSPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD", "BARO", "MPU", "INVERTER", "LED STRIP", "LED", "RECEIVER", "TRANSMITTER", "VTX", "SPI_PREINIT", "COMPASS", "TEMPERATURE", "1-WIRE", "AIRSPEED", "OLED DISPLAY", - "PINIO", "IRLOCK", "DRONECAN" + "PINIO", "IRLOCK", "DRONECAN", "CURRENT METER" }; const char * const resourceNames[RESOURCE_TOTAL_COUNT] = { diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index e9241304ca6..56276a3c9f2 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -55,6 +55,7 @@ typedef enum { OWNER_PINIO, OWNER_IRLOCK, OWNER_DRONECAN, + OWNER_CURRENT_METER, OWNER_TOTAL_COUNT } resourceOwner_e; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e5cfe7e81e8..b3734efa987 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -910,8 +910,12 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, 0); #endif +#ifdef USE_BATTERY_VOLTAGE_SENSOR #ifdef USE_ADC sbufWriteU8(dst, batteryMetersConfig()->voltage.scale / 10); +#else + sbufWriteU8(dst, 0); +#endif sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10); @@ -949,8 +953,12 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, 0); #endif +#ifdef USE_BATTERY_VOLTAGE_SENSOR #ifdef USE_ADC sbufWriteU16(dst, batteryMetersConfig()->voltage.scale); +#else + sbufWriteU16(dst, 0); +#endif sbufWriteU8(dst, batteryMetersConfig()->voltageSource); sbufWriteU8(dst, currentBatteryProfile->cells); sbufWriteU16(dst, currentBatteryProfile->voltage.cellDetect); @@ -985,8 +993,12 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP2_INAV_BATTERY_CONFIG: +#ifdef USE_BATTERY_VOLTAGE_SENSOR #ifdef USE_ADC sbufWriteU16(dst, batteryMetersConfig()->voltage.scale); +#else + sbufWriteU16(dst, 0); +#endif sbufWriteU8(dst, batteryMetersConfig()->voltageSource); sbufWriteU8(dst, currentBatteryProfile->cells); sbufWriteU16(dst, currentBatteryProfile->voltage.cellDetect); @@ -1168,8 +1180,12 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP_VOLTAGE_METER_CONFIG: +#ifdef USE_BATTERY_VOLTAGE_SENSOR #ifdef USE_ADC sbufWriteU8(dst, batteryMetersConfig()->voltage.scale / 10); +#else + sbufWriteU8(dst, 0); +#endif sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10); @@ -2286,8 +2302,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) sbufReadU16(src); #endif +#ifdef USE_BATTERY_VOLTAGE_SENSOR #ifdef USE_ADC batteryMetersConfigMutable()->voltage.scale = sbufReadU8(src) * 10; +#else + sbufReadU8(src); +#endif currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10; // vbatlevel_warn1 in MWC2.3 GUI currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10; // vbatlevel_warn2 in MWC2.3 GUI currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10; // vbatlevel when buzzer starts to alert @@ -2331,8 +2351,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) sbufReadU16(src); #endif +#ifdef USE_BATTERY_VOLTAGE_SENSOR #ifdef USE_ADC batteryMetersConfigMutable()->voltage.scale = sbufReadU16(src); +#else + sbufReadU16(src); +#endif batteryMetersConfigMutable()->voltageSource = sbufReadU8(src); currentBatteryProfileMutable->cells = sbufReadU8(src); currentBatteryProfileMutable->voltage.cellDetect = sbufReadU16(src); @@ -2374,8 +2398,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP2_INAV_SET_BATTERY_CONFIG: if (dataSize == 29) { +#ifdef USE_BATTERY_VOLTAGE_SENSOR #ifdef USE_ADC batteryMetersConfigMutable()->voltage.scale = sbufReadU16(src); +#else + sbufReadU16(src); +#endif batteryMetersConfigMutable()->voltageSource = sbufReadU8(src); currentBatteryProfileMutable->cells = sbufReadU8(src); currentBatteryProfileMutable->voltage.cellDetect = sbufReadU16(src); @@ -3281,8 +3309,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_VOLTAGE_METER_CONFIG: if (dataSize == 4) { +#ifdef USE_BATTERY_VOLTAGE_SENSOR #ifdef USE_ADC batteryMetersConfigMutable()->voltage.scale = sbufReadU8(src) * 10; +#else + sbufReadU8(src); +#endif currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10; currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10; currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10; diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 17dfecbf71a..411e01a1ad3 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -137,7 +137,7 @@ void taskUpdateBattery(timeUs_t currentTimeUs) #endif } -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR if (feature(FEATURE_VBAT)) { batteryUpdate(BatMonitoringTimeSinceLastServiced); } @@ -145,7 +145,7 @@ void taskUpdateBattery(timeUs_t currentTimeUs) if (feature(FEATURE_VBAT) && isAmperageConfigured()) { powerMeterUpdate(BatMonitoringTimeSinceLastServiced); sagCompensatedVBatUpdate(currentTimeUs, BatMonitoringTimeSinceLastServiced); -#if defined(USE_POWER_LIMITS) && defined(USE_ADC) +#if defined(USE_POWER_LIMITS) && defined(USE_BATTERY_VOLTAGE_SENSOR) powerLimiterUpdate(BatMonitoringTimeSinceLastServiced); #endif } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 15a0c0fff4f..13f643bfa99 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -33,10 +33,10 @@ tables: - name: failsafe_procedure values: ["LAND", "DROP", "RTH", "NONE"] - name: current_sensor - values: ["NONE", "ADC", "VIRTUAL", "FAKE", "ESC", "SMARTPORT", "CRSF", "CAN"] + values: ["NONE", "ADC", "VIRTUAL", "FAKE", "ESC", "SMARTPORT", "CRSF", "CAN", "INA226"] enum: currentSensor_e - name: voltage_sensor - values: ["NONE", "ADC", "ESC", "FAKE", "SMARTPORT", "CRSF", "CAN"] + values: ["NONE", "ADC", "ESC", "FAKE", "SMARTPORT", "CRSF", "CAN", "INA226"] enum: voltageSensor_e - name: imu_inertia_comp_method values: ["VELNED", "TURNRATE","ADAPTIVE"] @@ -979,9 +979,9 @@ groups: headers: ["sensors/battery_config_structs.h"] members: - name: vbat_meter_type - description: "Vbat voltage source. Possible values: `NONE`, `ADC`, `SMARTPORT`, `ESC`, 'CAN'. `ESC` requires ESC telemetry enabled and running. `SMARTPORT` requires SmartPort Master enabled and running. 'CAN' requires requires dronecan running and a sensor on the bus." - condition: USE_ADC - default_value: ADC + description: "Vbat voltage source. Possible values: `NONE`, `ADC`, `SMARTPORT`, `ESC`, `CRSF`, `CAN`, `INA226`. `ESC` requires ESC telemetry enabled and running. `SMARTPORT` requires SmartPort Master enabled and running. `CRSF` requires CRSF battery telemetry. `CAN` requires DroneCAN running and a sensor on the bus. `INA226` requires an INA226 I2C sensor." + condition: USE_BATTERY_VOLTAGE_SENSOR + default_value: :target field: voltage.type table: voltage_sensor type: uint8_t @@ -1011,11 +1011,18 @@ groups: min: -32768 max: 32767 - name: current_meter_type - description: "ADC, VIRTUAL, FAKE, ESC, SMARTPORT, CAN, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position." - default_value: "ADC" + description: "ADC, VIRTUAL, FAKE, ESC, SMARTPORT, CRSF, CAN, INA226, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position." + default_value: :target field: current.type table: current_sensor type: uint8_t + - name: ina_shunt_res_uohm + description: "INA226 shunt resistor value in micro-ohms." + default_value: :target + field: ina226.shuntResistanceMicroOhm + condition: USE_INA226 + min: 1 + max: 4294967295 - name: bat_voltage_src description: "Chose between raw and sag compensated battery voltage to use for battery alarms and telemetry. Possible values are `RAW` and `SAG_COMP`" default_value: "RAW" @@ -1055,35 +1062,35 @@ groups: description: "Number of cells of the battery (0 = auto-detect), see battery documentation. 7S, 9S and 11S batteries cannot be auto-detected." default_value: 0 field: cells - condition: USE_ADC + condition: USE_BATTERY_VOLTAGE_SENSOR min: 0 max: 12 - name: vbat_cell_detect_voltage description: "Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units." default_value: 425 field: voltage.cellDetect - condition: USE_ADC + condition: USE_BATTERY_VOLTAGE_SENSOR min: 100 max: 500 - name: vbat_max_cell_voltage description: "Maximum voltage per cell in 0.01V units, default is 4.20V" default_value: 420 field: voltage.cellMax - condition: USE_ADC + condition: USE_BATTERY_VOLTAGE_SENSOR min: 100 max: 500 - name: vbat_min_cell_voltage description: "Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V)" default_value: 330 field: voltage.cellMin - condition: USE_ADC + condition: USE_BATTERY_VOLTAGE_SENSOR min: 100 max: 500 - name: vbat_warning_cell_voltage description: "Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V)" default_value: 350 field: voltage.cellWarning - condition: USE_ADC + condition: USE_BATTERY_VOLTAGE_SENSOR min: 100 max: 500 - name: battery_capacity @@ -1203,25 +1210,25 @@ groups: max: 3000 - name: limit_cont_power description: "Continous power limit (dW), set to 0 to disable" - condition: USE_POWER_LIMITS && USE_ADC + condition: USE_POWER_LIMITS && USE_BATTERY_VOLTAGE_SENSOR default_value: 0 field: powerLimits.continuousPower max: 40000 - name: limit_burst_power description: "Burst power limit (dW): the current which is allowed during `limit_burst_power_time` after which `limit_cont_power` will be enforced, set to 0 to disable" - condition: USE_POWER_LIMITS && USE_ADC + condition: USE_POWER_LIMITS && USE_BATTERY_VOLTAGE_SENSOR default_value: 0 field: powerLimits.burstPower max: 40000 - name: limit_burst_power_time description: "Allowed power burst time (ds) during which `limit_burst_power` is allowed and after which `limit_cont_power` will be enforced" - condition: USE_POWER_LIMITS && USE_ADC + condition: USE_POWER_LIMITS && USE_BATTERY_VOLTAGE_SENSOR default_value: 0 field: powerLimits.burstPowerTime max: 3000 - name: limit_burst_power_falldown_time description: "Time slice at the end of the burst time during which the power limit will be ramped down from `limit_burst_power` back down to `limit_cont_power`" - condition: USE_POWER_LIMITS && USE_ADC + condition: USE_POWER_LIMITS && USE_BATTERY_VOLTAGE_SENSOR default_value: 0 field: powerLimits.burstPowerFalldownTime max: 3000 @@ -4010,7 +4017,7 @@ groups: - name: stats_total_energy description: "Total energy consumption [in mWh]. The value is updated on every disarm when \"stats\" are enabled." max: INT32_MAX - condition: USE_ADC + condition: USE_BATTERY_VOLTAGE_SENSOR default_value: 0 - name: stats_flight_count description: "Total number of flights. The value is updated on every disarm when \"stats\" are enabled." @@ -4528,7 +4535,7 @@ groups: min: 1 max: 127 - name: dronecan_bitrate_kbps - description: "The speed of the CANbus network in kbps. Set all devices to the same speed. " + description: "The speed of the CANbus network in kbps. Set all devices to the same speed." default_value: "1000" field: bitRateKbps table: dronecan_bitrate_table diff --git a/src/main/flight/power_limits.c b/src/main/flight/power_limits.c index a75d0a81206..a1c05655884 100644 --- a/src/main/flight/power_limits.c +++ b/src/main/flight/power_limits.c @@ -61,7 +61,7 @@ static pt1Filter_t currentThrAttnFilter; static pt1Filter_t currentThrLimitingBaseFilter; static bool wasLimitingCurrent = false; -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR static float burstPowerReserve; // cW.µs static float burstPowerReserveMax; // cW.µs static float burstPowerReserveFalldown; // cW.µs @@ -91,7 +91,7 @@ void powerLimiterInit(void) { pt1FilterSetCutoff(¤tThrAttnFilter, attnFilterCutoff); pt1FilterSetTimeConstant(¤tThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST); -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR // Only enforce burst >= continuous if burst is enabled (non-zero) if (currentBatteryProfile->powerLimits.burstPower > 0 && currentBatteryProfile->powerLimits.burstPower < currentBatteryProfile->powerLimits.continuousPower) { @@ -128,7 +128,7 @@ void currentLimiterUpdate(timeDelta_t timeDelta) { timeDelta); } -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR void powerLimiterUpdate(timeDelta_t timeDelta) { activePowerLimit = calculateActiveLimit(getPower(), currentBatteryProfile->powerLimits.continuousPower, currentBatteryProfile->powerLimits.burstPower, @@ -139,7 +139,7 @@ void powerLimiterUpdate(timeDelta_t timeDelta) { void powerLimiterApply(int16_t *throttleCommand) { -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR if (!activeCurrentLimit && !activePowerLimit) { return; } @@ -155,13 +155,13 @@ void powerLimiterApply(int16_t *throttleCommand) { int16_t throttleBase; int16_t currentThrottleCommand; -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR int16_t powerThrottleCommand; #endif int16_t current = getAmperageSample(); -#ifdef USE_ADC - uint16_t voltage = getVBatSample(); +#ifdef USE_BATTERY_VOLTAGE_SENSOR + uint16_t voltage = getBatteryVoltageSample(); int32_t power = (int32_t)voltage * current / 100; #endif @@ -192,7 +192,7 @@ void powerLimiterApply(int16_t *throttleCommand) { currentThrottleCommand = *throttleCommand; } -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR // Power limiting int32_t overPower = power - activePowerLimit; @@ -229,7 +229,7 @@ void powerLimiterApply(int16_t *throttleCommand) { } bool powerLimiterIsLimiting(void) { -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR return wasLimitingPower || wasLimitingCurrent; #else return wasLimitingCurrent; @@ -240,7 +240,7 @@ bool powerLimiterIsLimitingCurrent(void) { return wasLimitingCurrent; } -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR bool powerLimiterIsLimitingPower(void) { return wasLimitingPower; } @@ -251,7 +251,7 @@ float powerLimiterGetRemainingBurstTime(void) { uint16_t currentBurstOverContinuous = currentBatteryProfile->powerLimits.burstCurrent - currentBatteryProfile->powerLimits.continuousCurrent; float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7f; -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR uint16_t powerBurstOverContinuous = currentBatteryProfile->powerLimits.burstPower - currentBatteryProfile->powerLimits.continuousPower; float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7f; @@ -274,7 +274,7 @@ uint16_t powerLimiterGetActiveCurrentLimit(void) { return activeCurrentLimit; } -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR // returns cW uint16_t powerLimiterGetActivePowerLimit(void) { return activePowerLimit; diff --git a/src/main/flight/power_limits.h b/src/main/flight/power_limits.h index d5b877273be..cba5d233841 100644 --- a/src/main/flight/power_limits.h +++ b/src/main/flight/power_limits.h @@ -29,6 +29,8 @@ #include "config/parameter_group.h" +#include "sensors/battery_config_structs.h" + #if defined(USE_POWER_LIMITS) typedef struct { @@ -48,7 +50,7 @@ bool powerLimiterIsLimiting(void); bool powerLimiterIsLimitingCurrent(void); float powerLimiterGetRemainingBurstTime(void); // returns seconds uint16_t powerLimiterGetActiveCurrentLimit(void); // returns cA -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR uint16_t powerLimiterGetActivePowerLimit(void); // returns cW bool powerLimiterIsLimitingPower(void); #endif diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 67416a16dff..7ba97df3eb8 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -770,7 +770,7 @@ static void osdFormatCoordinate(char *buff, char sym, int32_t val) int integerDigits = tfp_sprintf(buff + 1, (integerPart == 0 && val < 0) ? "-%d" : "%d", (int)integerPart); // We can show up to 7 digits in decimalPart. int32_t decimalPart = abs(val % (int)GPS_DEGREES_DIVIDER); - STATIC_ASSERT(GPS_DEGREES_DIVIDER == 1e7, adjust_max_decimal_digits); + STATIC_ASSERT(GPS_DEGREES_DIVIDER == 10000000L, adjust_max_decimal_digits); int decimalDigits; bool djiCompat = false; // Assume DJICOMPAT mode is no enabled @@ -2503,7 +2503,7 @@ static bool osdDrawSingleElement(uint8_t item) { /*static int32_t updatedTimeSeconds = 0;*/ static int32_t timeSeconds = -1; -#if defined(USE_ADC) && defined(USE_GPS) +#if defined(USE_BATTERY_VOLTAGE_SENSOR) && defined(USE_GPS) static timeUs_t updatedTimestamp = 0; timeUs_t currentTimeUs = micros(); if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { @@ -2518,7 +2518,7 @@ static bool osdDrawSingleElement(uint8_t item) if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) { buff[0] = SYM_FLIGHT_MINS_REMAINING; strcpy(buff + 1, "--:--"); -#if defined(USE_ADC) && defined(USE_GPS) +#if defined(USE_BATTERY_VOLTAGE_SENSOR) && defined(USE_GPS) updatedTimestamp = 0; #endif } else if (timeSeconds == -2) { @@ -2538,7 +2538,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_REMAINING_DISTANCE_BEFORE_RTH:; static int32_t distanceMeters = -1; -#if defined(USE_ADC) && defined(USE_GPS) +#if defined(USE_BATTERY_VOLTAGE_SENSOR) && defined(USE_GPS) static timeUs_t updatedTimestamp = 0; timeUs_t currentTimeUs = micros(); if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { @@ -3981,7 +3981,7 @@ static bool osdDrawSingleElement(uint8_t item) } break; -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR case OSD_PLIMIT_ACTIVE_POWER_LIMIT: { if (currentBatteryProfile->powerLimits.continuousPower) { @@ -3995,7 +3995,7 @@ static bool osdDrawSingleElement(uint8_t item) } break; } -#endif // USE_ADC +#endif // USE_BATTERY_VOLTAGE_SENSOR #endif // USE_POWER_LIMITS case OSD_MULTI_FUNCTION: { @@ -4669,7 +4669,7 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool displayWrite(osdDisplayPort, statValueX-(isBootStats ? 7 : 0), row, string_buffer); -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && statsConfig()->stats_total_energy) { uint8_t buffOffset = 0; if (isBootStats) { @@ -4731,7 +4731,7 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool displayWrite(osdDisplayPort, statValueX-(isBootStats ? 4 : 0), row++, avgEffBuff); } -#endif // USE_ADC +#endif // USE_BATTERY_VOLTAGE_SENSOR } return row; } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index da8a6237bd0..8413794063d 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -32,7 +32,12 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#if defined(USE_ADC) #include "drivers/adc.h" +#endif +#if defined(USE_INA226) +#include "drivers/ina226.h" +#endif #include "drivers/time.h" #include "fc/config.h" @@ -72,6 +77,11 @@ #include "sensors/battery_sensor_dronecan.h" #endif +#if defined(USE_INA226) +static ina226Dev_t ina226Dev; +static bool ina226Detected = false; +#endif + #define ADCVREF 3300 // in mV (3300 = 3.3V) #define VBATT_CELL_FULL_MAX_DIFF 10 // Max difference with cell max voltage for the battery to be considered full (10mV steps) @@ -114,7 +124,7 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance) { for (int i = 0; i < MAX_BATTERY_PROFILE_COUNT; i++) { RESET_CONFIG(batteryProfile_t, &instance[i], -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR .cells = SETTING_BAT_CELLS_DEFAULT, .voltage = { @@ -164,12 +174,12 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance) .burstCurrent = SETTING_LIMIT_BURST_CURRENT_DEFAULT, // dA .burstCurrentTime = SETTING_LIMIT_BURST_CURRENT_TIME_DEFAULT, // dS .burstCurrentFalldownTime = SETTING_LIMIT_BURST_CURRENT_FALLDOWN_TIME_DEFAULT, // dS -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR .continuousPower = SETTING_LIMIT_CONT_POWER_DEFAULT, // dW .burstPower = SETTING_LIMIT_BURST_POWER_DEFAULT, // dW .burstPowerTime = SETTING_LIMIT_BURST_POWER_TIME_DEFAULT, // dS .burstPowerFalldownTime = SETTING_LIMIT_BURST_POWER_FALLDOWN_TIME_DEFAULT, // dS -#endif // USE_ADC +#endif // USE_BATTERY_VOLTAGE_SENSOR } #endif // USE_POWER_LIMITS @@ -181,19 +191,27 @@ PG_REGISTER_WITH_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, PG_B PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR .voltage = { - .type = SETTING_VBAT_METER_TYPE_DEFAULT, + .type = VBAT_METER_TYPE_DEFAULT, +#ifdef USE_ADC .scale = VBAT_SCALE_DEFAULT, +#endif }, #endif .current = { - .type = SETTING_CURRENT_METER_TYPE_DEFAULT, + .type = CURRENT_METER_TYPE_DEFAULT, .scale = CURRENT_METER_SCALE, .offset = CURRENT_METER_OFFSET }, +#ifdef USE_INA226 + .ina226 = { + .shuntResistanceMicroOhm = INA226_SHUNT_RES_UOHM_DEFAULT, + }, +#endif + .voltageSource = SETTING_BAT_VOLTAGE_SRC_DEFAULT, .capacity_unit = SETTING_BATTERY_CAPACITY_UNIT_DEFAULT, @@ -215,9 +233,17 @@ void batteryInit(void) batteryCriticalVoltage = 0; pt1FilterSetCutoff(&erageFilterState, AMPERAGE_LPF_FREQ); + +#if defined(USE_INA226) + ina226Detected = false; + if (batteryMetersConfig()->current.type == CURRENT_SENSOR_INA226 + || batteryMetersConfig()->voltage.type == VOLTAGE_SENSOR_INA226) { + ina226Detected = ina226Init(&ina226Dev); + } +#endif } -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR // profileDetect() profile sorting compare function static int profile_compare(profile_comp_t *a, profile_comp_t *b) { if (a->max_voltage < b->max_voltage) @@ -274,17 +300,19 @@ void activateBatteryProfile(void) } } -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected) { static pt1Filter_t vbatFilterState; switch (batteryMetersConfig()->voltage.type) { +#if defined(USE_ADC) case VOLTAGE_SENSOR_ADC: { vbat = getVBatSample(); break; } +#endif #if defined(USE_ESC_SENSOR) case VOLTAGE_SENSOR_ESC: { @@ -334,6 +362,14 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected) break; #endif +#if defined(USE_INA226) + case VOLTAGE_SENSOR_INA226: + if (ina226Detected && ina226ReadBusVoltage(&ina226Dev, &vbat)) { + break; + } + vbat = 0; + break; +#endif case VOLTAGE_SENSOR_NONE: default: vbat = 0; @@ -514,6 +550,53 @@ uint16_t getVBatSample(void) { } #endif +#ifdef USE_BATTERY_VOLTAGE_SENSOR +uint16_t getBatteryVoltageSample(void) +{ + switch (batteryMetersConfig()->voltage.type) { +#if defined(USE_ADC) + case VOLTAGE_SENSOR_ADC: + return getVBatSample(); +#endif +#if defined(USE_ESC_SENSOR) + case VOLTAGE_SENSOR_ESC: { + escSensorData_t *escSensor = escSensorGetData(); + return (escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE) ? escSensor->voltage : 0; + } +#endif +#if defined(USE_FAKE_BATT_SENSOR) + case VOLTAGE_SENSOR_FAKE: + return fakeBattSensorGetVBat(); +#endif +#if defined(USE_SMARTPORT_MASTER) + case VOLTAGE_SENSOR_SMARTPORT: { + int16_t *smartportVoltageData = smartportMasterGetVoltageData(); + return smartportVoltageData ? *smartportVoltageData : 0; + } +#endif +#if defined(USE_BATTERY_SENSOR_CRSF) + case VOLTAGE_SENSOR_CRSF: { + int16_t *crsfVoltageData = crsfBatterySensorGetVoltageData(); + return crsfVoltageData ? *crsfVoltageData : 0; + } +#endif +#if defined(USE_DRONECAN) + case VOLTAGE_SENSOR_CAN: + return dronecanBattSensorGetVBat(); +#endif +#if defined(USE_INA226) + case VOLTAGE_SENSOR_INA226: { + uint16_t ina226Voltage; + return (ina226Detected && ina226ReadBusVoltage(&ina226Dev, &ina226Voltage)) ? ina226Voltage : 0; + } +#endif + case VOLTAGE_SENSOR_NONE: + default: + return 0; + } +} +#endif + uint16_t getBatteryVoltage(void) { if (batteryMetersConfig()->voltageSource == BAT_VOLTAGE_SAG_COMP) { @@ -584,8 +667,53 @@ int16_t getAmperage(void) int16_t getAmperageSample(void) { - int32_t microvolts = ((uint32_t)adcGetChannel(ADC_CURRENT) * ADCVREF * 100) / 0xFFF * 10 - (int32_t)batteryMetersConfig()->current.offset * 100; - return microvolts / batteryMetersConfig()->current.scale; // current in 0.01A steps + switch (batteryMetersConfig()->current.type) { +#ifdef USE_ADC + case CURRENT_SENSOR_ADC: { + int32_t microvolts = ((uint32_t)adcGetChannel(ADC_CURRENT) * ADCVREF * 100) / 0xFFF * 10 - (int32_t)batteryMetersConfig()->current.offset * 100; + return microvolts / batteryMetersConfig()->current.scale; // current in 0.01A steps + } +#endif +#if defined(USE_ESC_SENSOR) + case CURRENT_SENSOR_ESC: { + escSensorData_t *escSensor = escSensorGetData(); + return (escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE) ? escSensor->current : 0; + } +#endif +#if defined(USE_SMARTPORT_MASTER) + case CURRENT_SENSOR_SMARTPORT: { + int16_t *smartportCurrentData = smartportMasterGetCurrentData(); + return smartportCurrentData ? *smartportCurrentData : 0; + } +#endif +#if defined(USE_BATTERY_SENSOR_CRSF) + case CURRENT_SENSOR_CRSF: { + int16_t *crsfCurrentData = crsfBatterySensorGetCurrentData(); + return crsfCurrentData ? *crsfCurrentData : 0; + } +#endif +#if defined(USE_DRONECAN) + case CURRENT_SENSOR_CAN: + return dronecanBattSensorGetAmperage(); +#endif +#if defined(USE_INA226) + case CURRENT_SENSOR_INA226: { + int16_t ina226Current; + return (ina226Detected && ina226ReadShuntCurrent(&ina226Dev, batteryMetersConfig()->ina226.shuntResistanceMicroOhm, &ina226Current)) + ? ina226Current + batteryMetersConfig()->current.offset + : 0; + } +#endif +#if defined(USE_FAKE_BATT_SENSOR) + case CURRENT_SENSOR_FAKE: + return fakeBattSensorGetAmerperage(); +#endif + case CURRENT_SENSOR_VIRTUAL: + return getAmperage(); + case CURRENT_SENSOR_NONE: + default: + return 0; + } } int32_t getPower(void) @@ -608,11 +736,13 @@ void currentMeterUpdate(timeUs_t timeDelta) static int64_t mAhdrawnRaw = 0; switch (batteryMetersConfig()->current.type) { +#if defined(USE_ADC) case CURRENT_SENSOR_ADC: { amperage = pt1FilterApply3(&erageFilterState, getAmperageSample(), US2S(timeDelta)); break; } +#endif case CURRENT_SENSOR_VIRTUAL: amperage = batteryMetersConfig()->current.offset; if (ARMING_FLAG(ARMED)) { @@ -670,6 +800,18 @@ void currentMeterUpdate(timeUs_t timeDelta) amperage = dronecanBattSensorGetAmperage(); break; #endif +#if defined(USE_INA226) + case CURRENT_SENSOR_INA226: + { + int16_t ina226Current; + if (ina226Detected && ina226ReadShuntCurrent(&ina226Dev, batteryMetersConfig()->ina226.shuntResistanceMicroOhm, &ina226Current)) { + amperage = pt1FilterApply3(&erageFilterState, ina226Current + batteryMetersConfig()->current.offset, US2S(timeDelta)); + } else { + amperage = 0; + } + } + break; +#endif #if defined(USE_FAKE_BATT_SENSOR) case CURRENT_SENSOR_FAKE: amperage = fakeBattSensorGetAmerperage(); diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 0ecde181b37..7809061f264 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -29,6 +29,10 @@ #define VBAT_SCALE_DEFAULT 1100 #endif +#ifndef VBAT_METER_TYPE_DEFAULT +#define VBAT_METER_TYPE_DEFAULT VOLTAGE_SENSOR_ADC +#endif + #ifndef CURRENT_METER_SCALE #define CURRENT_METER_SCALE 400 // for Allegro ACS758LCB-100U (40mV/A) #endif @@ -37,6 +41,22 @@ #define CURRENT_METER_OFFSET 0 #endif +#ifndef CURRENT_METER_TYPE_DEFAULT +#define CURRENT_METER_TYPE_DEFAULT CURRENT_SENSOR_ADC +#endif + +#ifndef INA226_SHUNT_RESISTANCE_UOHM +#define INA226_SHUNT_RESISTANCE_UOHM 2000 +#endif + +#ifndef INA226_SHUNT_RES_UOHM +#define INA226_SHUNT_RES_UOHM INA226_SHUNT_RESISTANCE_UOHM +#endif + +#ifndef INA226_SHUNT_RES_UOHM_DEFAULT +#define INA226_SHUNT_RES_UOHM_DEFAULT INA226_SHUNT_RES_UOHM +#endif + #ifndef MAX_BATTERY_PROFILE_COUNT #define MAX_BATTERY_PROFILE_COUNT SETTING_CONSTANT_MAX_BATTERY_PROFILE_COUNT #endif @@ -90,10 +110,14 @@ int32_t getPower(void); int32_t getMAhDrawn(void); int32_t getMWhDrawn(void); -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR void batteryUpdate(timeUs_t timeDelta); void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta); void powerMeterUpdate(timeUs_t timeDelta); +uint16_t getBatteryVoltageSample(void); +#endif + +#ifdef USE_ADC uint16_t getVBatSample(void); #endif diff --git a/src/main/sensors/battery_config_structs.h b/src/main/sensors/battery_config_structs.h index 8fe49295f8e..72e1316c0cf 100644 --- a/src/main/sensors/battery_config_structs.h +++ b/src/main/sensors/battery_config_structs.h @@ -34,7 +34,8 @@ typedef enum { CURRENT_SENSOR_SMARTPORT, CURRENT_SENSOR_CRSF, CURRENT_SENSOR_CAN, - CURRENT_SENSOR_MAX = CURRENT_SENSOR_CAN + CURRENT_SENSOR_INA226, + CURRENT_SENSOR_MAX = CURRENT_SENSOR_INA226 } currentSensor_e; typedef enum { @@ -45,9 +46,14 @@ typedef enum { VOLTAGE_SENSOR_SMARTPORT, VOLTAGE_SENSOR_CRSF, VOLTAGE_SENSOR_CAN, - VOLTAGE_SENSOR_MAX = VOLTAGE_SENSOR_CAN + VOLTAGE_SENSOR_INA226, + VOLTAGE_SENSOR_MAX = VOLTAGE_SENSOR_INA226 } voltageSensor_e; +#if defined(USE_ADC) || defined(USE_INA226) || defined(USE_ESC_SENSOR) || defined(USE_FAKE_BATT_SENSOR) || defined(USE_SMARTPORT_MASTER) || defined(USE_BATTERY_SENSOR_CRSF) || defined(USE_DRONECAN) +#define USE_BATTERY_VOLTAGE_SENSOR +#endif + typedef enum { BAT_CAPACITY_UNIT_MAH, BAT_CAPACITY_UNIT_MWH, @@ -60,9 +66,11 @@ typedef enum { typedef struct batteryMetersConfig_s { -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR struct { +#ifdef USE_ADC uint16_t scale; +#endif voltageSensor_e type; } voltage; #endif @@ -73,6 +81,12 @@ typedef struct batteryMetersConfig_s { currentSensor_e type; // type of current meter used, either ADC or virtual } current; +#ifdef USE_INA226 + struct { + uint32_t shuntResistanceMicroOhm; + } ina226; +#endif + batVoltageSource_e voltageSource; batCapacityUnit_e capacity_unit; // Describes unit of capacity.value, capacity.warning and capacity.critical @@ -87,7 +101,7 @@ typedef struct batteryMetersConfig_s { typedef struct batteryProfile_s { -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR uint8_t cells; struct { @@ -140,12 +154,12 @@ typedef struct batteryProfile_s { uint16_t burstCurrentTime; // ds uint16_t burstCurrentFalldownTime; // ds -#ifdef USE_ADC +#ifdef USE_BATTERY_VOLTAGE_SENSOR uint16_t continuousPower; // dW uint16_t burstPower; // dW uint16_t burstPowerTime; // ds uint16_t burstPowerFalldownTime; // ds -#endif // USE_ADC +#endif // USE_BATTERY_VOLTAGE_SENSOR } powerLimits; #endif // USE_POWER_LIMITS diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 8314926184f..0621bce3484 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -18,6 +18,7 @@ #include #include "drivers/io.h" #include "drivers/bus.h" +#include "drivers/ina226.h" #include "drivers/sensor.h" #if !defined(USE_TARGET_HARDWARE_DESCRIPTORS) @@ -468,6 +469,14 @@ BUSDEV_REGISTER_I2C(busdev_irlock, DEVHW_IRLOCK, IRLOCK_I2C_BUS, 0x54, NONE, DEVFLAGS_USE_RAW_REGISTERS); #endif +#if defined(USE_INA226) && defined(INA226_I2C_BUS) + #if !defined(INA226_I2C_ADDRESS) + #define INA226_I2C_ADDRESS INA226_DEFAULT_I2C_ADDRESS + #endif + + BUSDEV_REGISTER_I2C(busdev_ina226, DEVHW_INA226, INA226_I2C_BUS, INA226_I2C_ADDRESS, NONE, DEVFLAGS_NONE, 0); +#endif + #if defined(USE_I2C) && defined(USE_I2C_IO_EXPANDER) #if !defined(PCF8574_I2C_BUS) && defined(EXTERNAL_I2C_BUS) diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 5b43b636a77..3fcf99867d6 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -11,6 +11,10 @@ set_property(SOURCE adsb_unittest.cc PROPERTY extra_includes "../../lib/main/MAV set_property(SOURCE alignsensor_unittest.cc PROPERTY depends "common/maths.c" "sensors/boardalignment.c") +set_property(SOURCE battery_ina226_unittest.cc PROPERTY depends + "sensors/battery.c" "drivers/ina226.c" "common/filter.c" "common/lulu.c" "common/maths.c") +set_property(SOURCE battery_ina226_unittest.cc PROPERTY definitions USE_INA226 USE_I2C USE_HEADTRACKER USE_STATS USE_ADC) + set_property(SOURCE bitarray_unittest.cc PROPERTY depends "common/bitarray.c") set_property(SOURCE flight_imu_unittest.cc PROPERTY depends "build/debug.c" @@ -70,6 +74,9 @@ set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TE set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c" "common/maths.c" "drivers/headtracker_common.c") set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST USE_HEADTRACKER) +set_property(SOURCE ina226_unittest.cc PROPERTY depends "drivers/ina226.c") +set_property(SOURCE ina226_unittest.cc PROPERTY definitions USE_INA226 USE_I2C USE_HEADTRACKER) + function(unit_test src) get_filename_component(basename ${src} NAME) string(REPLACE ".cc" "" name ${basename} ) diff --git a/src/test/unit/battery_ina226_unittest.cc b/src/test/unit/battery_ina226_unittest.cc new file mode 100644 index 00000000000..16fe0cdbe43 --- /dev/null +++ b/src/test/unit/battery_ina226_unittest.cc @@ -0,0 +1,292 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify this software + * under the terms of the GNU General Public License as published by the Free + * Software Foundation, either version 3 of the License, or (at your option) any + * later version. + * + * INAV is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of FITNESS FOR A PARTICULAR + * PURPOSE. See the GNU General Public License for more details. + */ + +#include "gtest/gtest.h" + +extern "C" { + #include + #include + #include + + #include "build/debug.h" + #include "common/time.h" + #include "config/feature.h" + #include "drivers/ina226.h" + #include "fc/config.h" + #include "fc/runtime_config.h" + #include "flight/mixer.h" + #include "io/beeper.h" + #include "navigation/navigation.h" + #include "rx/rx.h" + #include "sensors/battery.h" +} + +typedef int navigationFSMStateFlags_t; + +typedef struct navigationPosControl_s { + int navState; +} navigationPosControl_t; + +#define INA226_REG_SHUNT_VOLTAGE 0x01 +#define INA226_REG_BUS_VOLTAGE 0x02 +#define INA226_REG_MANUFACTURER_ID 0xFE +#define INA226_REG_DIE_ID 0xFF + +static busDevice_t fakeBusDevice; +static uint16_t fakeRegisters[256]; +static bool fakeRegisterReadable[256]; +static uint32_t fakeFeatures; +static timeUs_t fakeTimeUs; + +extern "C" { + void pgResetFn_batteryProfiles(batteryProfile_t *instance); + extern const batteryMetersConfig_t pgResetTemplate_batteryMetersConfig; +} + +static void setFakeRegister(uint8_t reg, uint16_t value) +{ + fakeRegisters[reg] = value; + fakeRegisterReadable[reg] = true; +} + +static void resetFakeBus(void) +{ + memset(&fakeBusDevice, 0, sizeof(fakeBusDevice)); + memset(fakeRegisters, 0, sizeof(fakeRegisters)); + memset(fakeRegisterReadable, 0, sizeof(fakeRegisterReadable)); + + fakeBusDevice.busType = BUSTYPE_I2C; + setFakeRegister(INA226_REG_MANUFACTURER_ID, 0x5449); + setFakeRegister(INA226_REG_DIE_ID, 0x2260); +} + +static void clearFakeRegister(uint8_t reg) +{ + fakeRegisterReadable[reg] = false; +} + +static void settleVoltage(void) +{ + for (int i = 0; i < 5; i++) { + batteryUpdate(10000000); + } +} + +static void settleCurrent(void) +{ + for (int i = 0; i < 5; i++) { + currentMeterUpdate(10000000); + } +} + +static void resetBatteryTestState(void) +{ + fakeFeatures = FEATURE_VBAT | FEATURE_CURRENT_METER; + fakeTimeUs = 0; + memset(&systemConfig_System, 0, sizeof(systemConfig_System)); + memset(&navConfig_System, 0, sizeof(navConfig_System)); + + pgResetFn_batteryProfiles(*batteryProfiles_array()); + *batteryMetersConfigMutable() = pgResetTemplate_batteryMetersConfig; + currentBatteryProfile = batteryProfiles(0); + + batteryMetersConfigMutable()->voltage.type = VOLTAGE_SENSOR_INA226; + batteryMetersConfigMutable()->current.type = CURRENT_SENSOR_INA226; + batteryMetersConfigMutable()->ina226.shuntResistanceMicroOhm = 2000; + batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW; + + currentBatteryProfileMutable->cells = 0; + currentBatteryProfileMutable->voltage.cellDetect = 425; + currentBatteryProfileMutable->voltage.cellMax = 420; + currentBatteryProfileMutable->voltage.cellWarning = 350; + currentBatteryProfileMutable->voltage.cellMin = 330; + + resetFakeBus(); + batteryInit(); + + clearFakeRegister(INA226_REG_BUS_VOLTAGE); + clearFakeRegister(INA226_REG_SHUNT_VOLTAGE); + settleVoltage(); + settleCurrent(); +} + +extern "C" { + uint32_t armingFlags = 0; + int16_t rcCommand[4] = {}; + navigationPosControl_t posControl = {}; + int32_t debug[DEBUG32_VALUE_COUNT] = {}; + uint8_t debugMode = 0; + systemConfig_t systemConfig_System = {}; + systemConfig_t systemConfig_Copy = {}; + navConfig_t navConfig_System = {}; + navConfig_t navConfig_Copy = {}; + + busDevice_t *busDeviceInit(busType_e bus, devHardwareType_e hw, uint8_t tag, resourceOwner_e owner) + { + EXPECT_EQ(BUSTYPE_I2C, bus); + EXPECT_EQ(DEVHW_INA226, hw); + EXPECT_EQ(0, tag); + EXPECT_EQ(OWNER_CURRENT_METER, owner); + return &fakeBusDevice; + } + + void busDeviceDeInit(busDevice_t *dev) + { + EXPECT_EQ(&fakeBusDevice, dev); + } + + bool busReadBuf(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length) + { + EXPECT_EQ(&fakeBusDevice, busdev); + EXPECT_EQ(2, length); + + if (length != 2 || !fakeRegisterReadable[reg]) { + return false; + } + + data[0] = fakeRegisters[reg] >> 8; + data[1] = fakeRegisters[reg] & 0xFF; + return true; + } + + uint16_t adcGetChannel(uint8_t) + { + return 0; + } + + bool feature(uint32_t mask) + { + return fakeFeatures & mask; + } + + timeUs_t micros(void) + { + fakeTimeUs += 1000; + return fakeTimeUs; + } + + bool failsafeIsActive(void) + { + return false; + } + + void beeper(beeperMode_e) + { + } + + bool setConfigProfile(uint8_t) + { + return true; + } + + navigationFSMStateFlags_t navGetCurrentStateFlags(void) + { + return (navigationFSMStateFlags_t)0; + } + + bool throttleStickIsLow(void) + { + return true; + } + + bool ifMotorstopFeatureEnabled(void) + { + return false; + } + + float getFlightTime(void) + { + return 1.0f; + } + + uint32_t getFlyingEnergy(void) + { + return 0; + } + + uint32_t getTotalTravelDistance(void) + { + return 1; + } +} + +TEST(BatteryINA226, UpdatesBatteryVoltageFromIna226BusVoltage) +{ + resetBatteryTestState(); + setFakeRegister(INA226_REG_BUS_VOLTAGE, 9600); + + settleVoltage(); + + EXPECT_EQ(1200, getBatteryRawVoltage()); +} + +TEST(BatteryINA226, SamplesBatteryVoltageDirectlyFromIna226BusVoltage) +{ + resetBatteryTestState(); + setFakeRegister(INA226_REG_BUS_VOLTAGE, 9600); + + EXPECT_EQ(1200, getBatteryVoltageSample()); + EXPECT_EQ(0, getBatteryRawVoltage()); +} + +TEST(BatteryINA226, UpdatesCurrentFromIna226ShuntVoltage) +{ + resetBatteryTestState(); + setFakeRegister(INA226_REG_SHUNT_VOLTAGE, 8000); + + settleCurrent(); + + EXPECT_NEAR(1000, getAmperage(), 2); +} + +TEST(BatteryINA226, SamplesCurrentDirectlyFromConfiguredIna226Shunt) +{ + resetBatteryTestState(); + batteryMetersConfigMutable()->ina226.shuntResistanceMicroOhm = 300; + setFakeRegister(INA226_REG_SHUNT_VOLTAGE, 1200); + + EXPECT_EQ(1000, getAmperageSample()); + EXPECT_EQ(0, getAmperage()); +} + +TEST(BatteryINA226, UsesConfiguredMicroOhmShuntForCurrent) +{ + resetBatteryTestState(); + batteryMetersConfigMutable()->ina226.shuntResistanceMicroOhm = 300; + setFakeRegister(INA226_REG_SHUNT_VOLTAGE, 1200); + + settleCurrent(); + + EXPECT_NEAR(1000, getAmperage(), 2); +} + +TEST(BatteryINA226, ClearsVoltageAndCurrentOnReadFailure) +{ + resetBatteryTestState(); + setFakeRegister(INA226_REG_BUS_VOLTAGE, 9600); + setFakeRegister(INA226_REG_SHUNT_VOLTAGE, 8000); + + settleVoltage(); + settleCurrent(); + EXPECT_EQ(1200, getBatteryRawVoltage()); + EXPECT_NEAR(1000, getAmperage(), 2); + + clearFakeRegister(INA226_REG_BUS_VOLTAGE); + clearFakeRegister(INA226_REG_SHUNT_VOLTAGE); + + settleVoltage(); + settleCurrent(); + + EXPECT_EQ(0, getBatteryRawVoltage()); + EXPECT_EQ(0, getAmperage()); +} diff --git a/src/test/unit/ina226_unittest.cc b/src/test/unit/ina226_unittest.cc new file mode 100644 index 00000000000..fbb79af9a18 --- /dev/null +++ b/src/test/unit/ina226_unittest.cc @@ -0,0 +1,170 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify this software + * under the terms of the GNU General Public License as published by the Free + * Software Foundation, either version 3 of the License, or (at your option) any + * later version. + * + * INAV is distributed in the hope that it will be useful, but WITHOUT ANY + * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR + * A PARTICULAR PURPOSE. See the GNU General Public License for more details. + */ + +#include "gtest/gtest.h" + +extern "C" { + #include + #include + #include + + #include "drivers/ina226.h" +} + +#define INA226_REG_SHUNT_VOLTAGE 0x01 +#define INA226_REG_BUS_VOLTAGE 0x02 +#define INA226_REG_MANUFACTURER_ID 0xFE +#define INA226_REG_DIE_ID 0xFF + +static busDevice_t fakeBusDevice; +static uint16_t fakeRegisters[256]; +static bool fakeRegisterReadable[256]; +static bool fakeInitSucceeds; +static bool fakeDeInitCalled; + +static void resetFakeBus(void) +{ + memset(&fakeBusDevice, 0, sizeof(fakeBusDevice)); + memset(fakeRegisters, 0, sizeof(fakeRegisters)); + memset(fakeRegisterReadable, 0, sizeof(fakeRegisterReadable)); + + fakeBusDevice.busType = BUSTYPE_I2C; + fakeInitSucceeds = true; + fakeDeInitCalled = false; +} + +static void setFakeRegister(uint8_t reg, uint16_t value) +{ + fakeRegisters[reg] = value; + fakeRegisterReadable[reg] = true; +} + +extern "C" { + busDevice_t *busDeviceInit(busType_e bus, devHardwareType_e hw, uint8_t tag, resourceOwner_e owner) + { + EXPECT_EQ(BUSTYPE_I2C, bus); + EXPECT_EQ(DEVHW_INA226, hw); + EXPECT_EQ(0, tag); + EXPECT_EQ(OWNER_CURRENT_METER, owner); + + return fakeInitSucceeds ? &fakeBusDevice : nullptr; + } + + void busDeviceDeInit(busDevice_t *dev) + { + EXPECT_EQ(&fakeBusDevice, dev); + fakeDeInitCalled = true; + } + + bool busReadBuf(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length) + { + EXPECT_EQ(&fakeBusDevice, busdev); + EXPECT_EQ(2, length); + + if (length != 2 || !fakeRegisterReadable[reg]) { + return false; + } + + data[0] = fakeRegisters[reg] >> 8; + data[1] = fakeRegisters[reg] & 0xFF; + return true; + } +} + +TEST(INA226, DetectsExpectedIds) +{ + resetFakeBus(); + setFakeRegister(INA226_REG_MANUFACTURER_ID, 0x5449); + setFakeRegister(INA226_REG_DIE_ID, 0x2260); + + ina226Dev_t dev = {}; + EXPECT_TRUE(ina226Init(&dev)); + EXPECT_EQ(&fakeBusDevice, dev.busDev); + EXPECT_FALSE(fakeDeInitCalled); +} + +TEST(INA226, RejectsUnexpectedIds) +{ + resetFakeBus(); + setFakeRegister(INA226_REG_MANUFACTURER_ID, 0x1234); + setFakeRegister(INA226_REG_DIE_ID, 0x2260); + + ina226Dev_t dev = {}; + EXPECT_FALSE(ina226Init(&dev)); + EXPECT_EQ(nullptr, dev.busDev); + EXPECT_TRUE(fakeDeInitCalled); +} + +TEST(INA226, RejectsReadFailureDuringDetection) +{ + resetFakeBus(); + setFakeRegister(INA226_REG_MANUFACTURER_ID, 0x5449); + + ina226Dev_t dev = {}; + EXPECT_FALSE(ina226Init(&dev)); + EXPECT_EQ(nullptr, dev.busDev); + EXPECT_TRUE(fakeDeInitCalled); +} + +TEST(INA226, ConvertsBusVoltage) +{ + EXPECT_EQ(0, ina226BusVoltageToCentivolts(0)); + EXPECT_EQ(1200, ina226BusVoltageToCentivolts(9600)); + EXPECT_EQ(583, ina226BusVoltageToCentivolts(0x1234)); +} + +TEST(INA226, ReadsBusVoltageBigEndian) +{ + resetFakeBus(); + setFakeRegister(INA226_REG_BUS_VOLTAGE, 9600); + + ina226Dev_t dev = {}; + dev.busDev = &fakeBusDevice; + uint16_t centiVolts = 0; + + EXPECT_TRUE(ina226ReadBusVoltage(&dev, ¢iVolts)); + EXPECT_EQ(1200, centiVolts); +} + +TEST(INA226, ConvertsShuntVoltageToCurrent) +{ + EXPECT_EQ(1000, ina226ShuntVoltageToCentiamps(8000, 2000)); + EXPECT_EQ(-1000, ina226ShuntVoltageToCentiamps(-8000, 2000)); + EXPECT_EQ(0, ina226ShuntVoltageToCentiamps(8000, 0)); +} + +TEST(INA226, ReadsNegativeShuntCurrentBigEndian) +{ + resetFakeBus(); + setFakeRegister(INA226_REG_SHUNT_VOLTAGE, 0xE0C0); + + ina226Dev_t dev = {}; + dev.busDev = &fakeBusDevice; + int16_t centiAmps = 0; + + EXPECT_TRUE(ina226ReadShuntCurrent(&dev, 2000, ¢iAmps)); + EXPECT_EQ(-1000, centiAmps); +} + +TEST(INA226, ReportsReadFailures) +{ + resetFakeBus(); + + ina226Dev_t dev = {}; + dev.busDev = &fakeBusDevice; + uint16_t centiVolts = 1; + int16_t centiAmps = 1; + + EXPECT_FALSE(ina226ReadBusVoltage(&dev, ¢iVolts)); + EXPECT_FALSE(ina226ReadShuntCurrent(&dev, 2000, ¢iAmps)); +} diff --git a/src/utils/update_cli_docs.py b/src/utils/update_cli_docs.py index c549fb21bf5..28e6f7d06d2 100755 --- a/src/utils/update_cli_docs.py +++ b/src/utils/update_cli_docs.py @@ -132,7 +132,7 @@ def write_settings_md(lines): """Write the contents of the CLI settings docs""" with open(SETTINGS_MD_PATH, "w") as settings_md: - settings_md.writelines(lines) + settings_md.write("".join(lines).rstrip() + "\n") # Return all matches of a compiled regex in a list of files def regex_search(regex, files):