From cd3793ccbc851107cc6b328c3366abe9c3808ec3 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Thu, 28 Jun 2018 01:07:39 +0200 Subject: [PATCH 1/2] Fix MSP2_INAV_ANALOG and convert amperage from int32_t to int16_t - Fix MSP2_INAV_ANALOG to return possible negative values of `power`, `mAhDrawn` and `mWhDrawn` - int16_t should be enough for amperage (+-325A) --- src/main/fc/fc_msp.c | 14 +++++++------- src/main/sensors/battery.c | 18 +++++++++--------- src/main/sensors/battery.h | 4 ++-- 3 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e2be179e851..aad30560fd0 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -541,16 +541,16 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP2_INAV_ANALOG: + // Bit 1: battery full, Bit 2: use capacity threshold, Bit 3-4: battery state, Bit 5-8: battery cell count + sbufWriteU8(dst, batteryWasFullWhenPluggedIn() | (batteryUsesCapacityThresholds() << 1) | (getBatteryState() << 2) | (getBatteryCellCount() << 4)); sbufWriteU16(dst, getBatteryVoltage()); - sbufWriteU8(dst, getBatteryCellCount()); + sbufWriteU16(dst, getAmperage()); // send amperage in 0.01 A steps + sbufWriteU32(dst, getPower()); // power draw + sbufWriteU32(dst, getMAhDrawn()); // milliamp hours drawn from battery + sbufWriteU32(dst, getMWhDrawn()); // milliWatt hours drawn from battery + sbufWriteU32(dst, getBatteryRemainingCapacity()); sbufWriteU8(dst, calculateBatteryPercentage()); - sbufWriteU16(dst, constrain(getPower(), 0, 0x7FFFFFFF)); // power draw - sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery - sbufWriteU16(dst, (uint16_t)constrain(getMWhDrawn(), 0, 0xFFFF)); // milliWatt hours drawn from battery sbufWriteU16(dst, getRSSI()); - sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A - sbufWriteU8(dst, batteryWasFullWhenPluggedIn() | (batteryUsesCapacityThresholds() << 1) | (getBatteryState() << 2)); - sbufWriteU32(dst, getBatteryRemainingCapacity()); break; case MSP_ARMING_CONFIG: diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index e35082a2027..9fd1298d415 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -84,7 +84,7 @@ static uint16_t powerSupplyImpedance = 0; // calculated impedance in milliohm static uint16_t sagCompensatedVBat = 0; // calculated no load vbat static bool powerSupplyImpedanceIsValid = false; -static int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A) +static int16_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A) static int32_t power = 0; // power draw in cW (0.01W resolution) static int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start static int32_t mWhDrawn = 0; // energy (milliWatt hours) drawn from the battery since start @@ -144,7 +144,7 @@ uint16_t batteryAdcToVoltage(uint16_t src) return((uint64_t)src * batteryMetersConfig()->voltage_scale * ADCVREF / (0xFFF * 1000)); } -int32_t currentSensorToCentiamps(uint16_t src) +int16_t currentSensorToCentiamps(uint16_t src) { int32_t microvolts = ((uint32_t)src * ADCVREF * 100) / 0xFFF * 10 - (int32_t)batteryMetersConfig()->current.offset * 100; return microvolts / batteryMetersConfig()->current.scale; // current in 0.01A steps @@ -410,12 +410,12 @@ bool isAmperageConfigured(void) return feature(FEATURE_CURRENT_METER) && batteryMetersConfig()->current.type != CURRENT_SENSOR_NONE; } -int32_t getAmperage(void) +int16_t getAmperage(void) { return amperage; } -int32_t getAmperageLatestADC(void) +int16_t getAmperageLatestADC(void) { return amperageLatestADC; } @@ -461,15 +461,15 @@ void currentMeterUpdate(timeUs_t timeDelta) break; } - mAhdrawnRaw += (amperage * timeDelta) / 1000; + mAhdrawnRaw += (int32_t)amperage * timeDelta / 1000; mAhDrawn = mAhdrawnRaw / (3600 * 100); } void powerMeterUpdate(timeUs_t timeDelta) { static int64_t mWhDrawnRaw = 0; - power = amperage * vbat / 100; // power unit is cW (0.01W resolution) - int32_t heatLossesCompensatedPower_mW = amperage * vbat / 10 + sq((int64_t)amperage) * powerSupplyImpedance / 10000; + power = (int32_t)amperage * vbat / 100; // power unit is cW (0.01W resolution) + int32_t heatLossesCompensatedPower_mW = (int32_t)amperage * vbat / 10 + sq((int64_t)amperage) * powerSupplyImpedance / 10000; mWhDrawnRaw += (int64_t)heatLossesCompensatedPower_mW * timeDelta / 10000; mWhDrawn = mWhDrawnRaw / (3600 * 100); } @@ -485,7 +485,7 @@ int32_t heatLossesCompensatedPower(int32_t power) void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta) { static timeUs_t recordTimestamp = 0; - static int32_t amperageRecord; + static int16_t amperageRecord; static uint16_t vbatRecord; static uint8_t impedanceSampleCount = 0; static pt1Filter_t impedanceFilterState; @@ -536,7 +536,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta) } - uint16_t sagCompensatedVBatSample = MIN(batteryFullVoltage, vbat + powerSupplyImpedance * amperage / 1000); + uint16_t sagCompensatedVBatSample = MIN(batteryFullVoltage, vbat + (int32_t)powerSupplyImpedance * amperage / 1000); sagCompVBatFilterState.RC = sagCompensatedVBatSample < sagCompVBatFilterState.state ? 40 : 500; sagCompensatedVBat = lrintf(pt1FilterApply3(&sagCompVBatFilterState, sagCompensatedVBatSample, timeDelta * 1e-6f)); } diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index cdf99286c1f..3fb063264f9 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -135,8 +135,8 @@ uint32_t getBatteryRemainingCapacity(void); uint16_t getPowerSupplyImpedance(void); bool isAmperageConfigured(void); -int32_t getAmperage(void); -int32_t getAmperageLatestADC(void); +int16_t getAmperage(void); +int16_t getAmperageLatestADC(void); int32_t getPower(void); int32_t getMAhDrawn(void); int32_t getMWhDrawn(void); From af360257b9a9f0717103363518f6a418aca9c7a2 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Thu, 28 Jun 2018 15:30:17 +0200 Subject: [PATCH 2/2] Compiler bug workaround for mAh and mWh calculations --- src/main/sensors/battery.c | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 9fd1298d415..b7183b97c6a 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -461,7 +461,10 @@ void currentMeterUpdate(timeUs_t timeDelta) break; } - mAhdrawnRaw += (int32_t)amperage * timeDelta / 1000; + // Work around int64 math compiler bug, don't change it unless the bug has been fixed ! + // should be: mAhdrawnRaw += (int64_t)amperage * timeDelta / 1000; + mAhdrawnRaw += (int64_t)((int32_t)amperage * timeDelta) / 1000; + mAhDrawn = mAhdrawnRaw / (3600 * 100); } @@ -470,7 +473,11 @@ void powerMeterUpdate(timeUs_t timeDelta) static int64_t mWhDrawnRaw = 0; power = (int32_t)amperage * vbat / 100; // power unit is cW (0.01W resolution) int32_t heatLossesCompensatedPower_mW = (int32_t)amperage * vbat / 10 + sq((int64_t)amperage) * powerSupplyImpedance / 10000; - mWhDrawnRaw += (int64_t)heatLossesCompensatedPower_mW * timeDelta / 10000; + + // Work around int64 math compiler bug, don't change it unless the bug has been fixed ! + // should be: mWhDrawnRaw += (int64_t)heatLossesCompensatedPower_mW * timeDelta / 10000; + mWhDrawnRaw += (int64_t)((int64_t)heatLossesCompensatedPower_mW * timeDelta) / 10000; + mWhDrawn = mWhDrawnRaw / (3600 * 100); }