Merge pull request #3469 from shellixyz/fix_msp2_inav_analog

Fix compiler bug, MSP2_INAV_ANALOG and convert amperage from int32_t to int16_t

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:

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,16 +461,23 @@ void currentMeterUpdate(timeUs_t timeDelta)
  break;
  }
 
- mAhdrawnRaw += (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);
 }
 
 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;
- mWhDrawnRaw += (int64_t)heatLossesCompensatedPower_mW * timeDelta / 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;
+
+ // 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);
 }
 
@@ -485,7 +492,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 +543,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));
  }

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);