1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 20:10:18 +03:00

Increased vbat precision

This commit is contained in:
fgiudice98 2018-12-24 16:19:31 +01:00
parent 24344405fb
commit 9d5fb85474
30 changed files with 124 additions and 120 deletions

View file

@ -42,7 +42,7 @@
voltageMeterSource_e batteryConfig_voltageMeterSource;
currentMeterSource_e batteryConfig_currentMeterSource;
uint8_t batteryConfig_vbatmaxcellvoltage;
uint16_t batteryConfig_vbatmaxcellvoltage;
uint8_t voltageSensorADCConfig_vbatscale;
@ -103,7 +103,7 @@ static OSD_Entry cmsx_menuPowerEntries[] =
{ "V METER", OME_TAB, NULL, &(OSD_TAB_t){ &batteryConfig_voltageMeterSource, VOLTAGE_METER_COUNT - 1, voltageMeterSourceNames }, 0 },
{ "I METER", OME_TAB, NULL, &(OSD_TAB_t){ &batteryConfig_currentMeterSource, CURRENT_METER_COUNT - 1, currentMeterSourceNames }, 0 },
{ "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig_vbatmaxcellvoltage, 10, 50, 1 }, 0 },
{ "VBAT CLMAX", OME_UINT16, NULL, &(OSD_UINT16_t) { &batteryConfig_vbatmaxcellvoltage, 10, 50, 1 }, 0 },
{ "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t){ &voltageSensorADCConfig_vbatscale, VBAT_SCALE_MIN, VBAT_SCALE_MAX, 1 }, 0 },

View file

@ -3682,7 +3682,7 @@ static void cliStatus(char *cmdline)
// Battery meter
cliPrintLinef("Voltage: %d * 0.1V (%dS battery - %s)", getBatteryVoltage(), getBatteryCellCount(), getBatteryStateString());
cliPrintLinef("Voltage: %d * 0.01V (%dS battery - %s)", getBatteryVoltage(), getBatteryCellCount(), getBatteryStateString());
// Other devices and status

View file

@ -523,10 +523,11 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce
break;
case MSP_ANALOG:
sbufWriteU8(dst, (uint8_t)constrain(getBatteryVoltage(), 0, 255));
sbufWriteU8(dst, (uint8_t)constrain((getBatteryVoltage() + 5) / 10, 0, 255));
sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
sbufWriteU16(dst, getRssi());
sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send current in 0.01 A steps, range is -320A to 320A
sbufWriteU16(dst, getBatteryVoltage());
break;
case MSP_DEBUG:
@ -559,12 +560,14 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce
sbufWriteU16(dst, batteryConfig()->batteryCapacity); // in mAh
// battery state
sbufWriteU8(dst, (uint8_t)constrain(getBatteryVoltage(), 0, 255)); // in 0.1V steps
sbufWriteU8(dst, (uint8_t)constrain((getBatteryVoltage() + 5) / 10, 0, 255)); // in 0.1V steps
sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send current in 0.01 A steps, range is -320A to 320A
// battery alerts
sbufWriteU8(dst, (uint8_t)getBatteryState());
sbufWriteU16(dst, getBatteryVoltage()); // in 0.01V steps
break;
}
@ -582,7 +585,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce
voltageMeterRead(id, &meter);
sbufWriteU8(dst, id);
sbufWriteU8(dst, (uint8_t)constrain(meter.filtered, 0, 255));
sbufWriteU8(dst, (uint8_t)constrain((meter.filtered + 5) / 10, 0, 255));
}
break;
}
@ -661,12 +664,15 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce
}
case MSP_BATTERY_CONFIG:
sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage);
sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage);
sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage);
sbufWriteU8(dst, (batteryConfig()->vbatmincellvoltage + 5) / 10);
sbufWriteU8(dst, (batteryConfig()->vbatmaxcellvoltage + 5) / 10);
sbufWriteU8(dst, (batteryConfig()->vbatwarningcellvoltage + 5) / 10);
sbufWriteU16(dst, batteryConfig()->batteryCapacity);
sbufWriteU8(dst, batteryConfig()->voltageMeterSource);
sbufWriteU8(dst, batteryConfig()->currentMeterSource);
sbufWriteU16(dst, batteryConfig()->vbatmincellvoltage);
sbufWriteU16(dst, batteryConfig()->vbatmaxcellvoltage);
sbufWriteU16(dst, batteryConfig()->vbatwarningcellvoltage);
break;
case MSP_TRANSPONDER_CONFIG: {
@ -2444,12 +2450,17 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src, mspPos
}
case MSP_SET_BATTERY_CONFIG:
batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src) * 10; // vbatlevel_warn1 in MWC2.3 GUI
batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src) * 10; // vbatlevel_warn2 in MWC2.3 GUI
batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src) * 10; // vbatlevel when buzzer starts to alert
batteryConfigMutable()->batteryCapacity = sbufReadU16(src);
batteryConfigMutable()->voltageMeterSource = sbufReadU8(src);
batteryConfigMutable()->currentMeterSource = sbufReadU8(src);
if (sbufBytesRemaining(src) >= 6) {
batteryConfigMutable()->vbatmincellvoltage = sbufReadU16(src);
batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU16(src);
batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU16(src);
}
break;
#if defined(USE_OSD)

View file

@ -717,14 +717,14 @@ const clivalue_t valueTable[] = {
// PG_BATTERY_CONFIG
{ "bat_capacity", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 20000 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, batteryCapacity) },
{ "vbat_max_cell_voltage", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 10, 50 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatmaxcellvoltage) },
{ "vbat_full_cell_voltage", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 10, 50 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatfullcellvoltage) },
{ "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 10, 50 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatmincellvoltage) },
{ "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 10, 50 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatwarningcellvoltage) },
{ "vbat_max_cell_voltage", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100, 500 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatmaxcellvoltage) },
{ "vbat_full_cell_voltage", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100, 500 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatfullcellvoltage) },
{ "vbat_min_cell_voltage", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100, 500 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatmincellvoltage) },
{ "vbat_warning_cell_voltage", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100, 500 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatwarningcellvoltage) },
{ "vbat_hysteresis", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 250 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbathysteresis) },
{ "current_meter", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CURRENT_METER }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, currentMeterSource) },
{ "battery_meter", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_VOLTAGE_METER }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, voltageMeterSource) },
{ "vbat_detect_cell_voltage", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatnotpresentcellvoltage) },
{ "vbat_detect_cell_voltage", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 2000 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatnotpresentcellvoltage) },
{ "use_vbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, useVBatAlerts) },
{ "use_cbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, useConsumptionAlerts) },
{ "cbat_alert_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, consumptionWarningPercentage) },

View file

@ -450,7 +450,7 @@ static void showBatteryPage(void)
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) {
tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", getBatteryVoltage() / 10, getBatteryVoltage() % 10, getBatteryCellCount());
tfp_sprintf(lineBuffer, "Volts: %d.%02d Cells: %d", getBatteryVoltage() / 100, getBatteryVoltage() % 100, getBatteryCellCount());
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);

View file

@ -137,7 +137,7 @@ static float osdGForce = 0;
typedef struct statistic_s {
timeUs_t armed_time;
int16_t max_speed;
int16_t min_voltage; // /10
int16_t min_voltage; // /100
int16_t max_current; // /10
uint8_t min_rssi;
int32_t max_altitude;
@ -268,21 +268,13 @@ static char osdGetMetersToSelectedUnitSymbol(void)
}
}
/**
* Gets average battery cell voltage in 0.01V units.
*/
static int osdGetBatteryAverageCellVoltage(void)
{
return (getBatteryVoltage() * 10) / getBatteryCellCount();
}
static char osdGetBatterySymbol(int cellVoltage)
{
if (getBatteryState() == BATTERY_CRITICAL) {
return SYM_MAIN_BATT; // FIXME: currently the BAT- symbol, ideally replace with a battery with exclamation mark
} else {
// Calculate a symbol offset using cell voltage over full cell voltage range
const int symOffset = scaleRange(cellVoltage, batteryConfig()->vbatmincellvoltage * 10, batteryConfig()->vbatmaxcellvoltage * 10, 0, 7);
const int symOffset = scaleRange(cellVoltage, batteryConfig()->vbatmincellvoltage, batteryConfig()->vbatmaxcellvoltage, 0, 8);
return SYM_BATT_EMPTY - constrain(symOffset, 0, 6);
}
}
@ -610,8 +602,8 @@ static bool osdDrawSingleElement(uint8_t item)
#endif
case OSD_MAIN_BATT_VOLTAGE:
buff[0] = osdGetBatterySymbol(osdGetBatteryAverageCellVoltage());
tfp_sprintf(buff + 1, "%2d.%1d%c", getBatteryVoltage() / 10, getBatteryVoltage() % 10, SYM_VOLT);
buff[0] = osdGetBatterySymbol(getBatteryAverageCellVoltage());
tfp_sprintf(buff + 1, "%2d.%02d%c", getBatteryVoltage() / 100, getBatteryVoltage() % 100, SYM_VOLT);
break;
case OSD_CURRENT_DRAW:
@ -892,7 +884,7 @@ static bool osdDrawSingleElement(uint8_t item)
break;
case OSD_POWER:
tfp_sprintf(buff, "%4dW", getAmperage() * getBatteryVoltage() / 1000);
tfp_sprintf(buff, "%4dW", getAmperage() * getBatteryVoltage() / 10000);
break;
case OSD_PIDRATE_PROFILE:
@ -1113,7 +1105,7 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_AVG_CELL_VOLTAGE:
{
const int cellV = osdGetBatteryAverageCellVoltage();
const int cellV = getBatteryAverageCellVoltage();
buff[0] = osdGetBatterySymbol(cellV);
tfp_sprintf(buff + 1, "%d.%02d%c", cellV / 100, cellV % 100, SYM_VOLT);
break;
@ -1556,7 +1548,7 @@ static void osdResetStats(void)
{
stats.max_current = 0;
stats.max_speed = 0;
stats.min_voltage = 500;
stats.min_voltage = 5000;
stats.min_rssi = 99; // percent
stats.max_altitude = 0;
stats.max_distance = 0;
@ -1745,17 +1737,17 @@ static void osdShowStats(uint16_t endBatteryVoltage)
#endif
if (osdStatGetState(OSD_STAT_MIN_BATTERY)) {
tfp_sprintf(buff, "%d.%1d%c", stats.min_voltage / 10, stats.min_voltage % 10, SYM_VOLT);
tfp_sprintf(buff, "%d.%02d%c", stats.min_voltage / 100, stats.min_voltage % 100, SYM_VOLT);
osdDisplayStatisticLabel(top++, "MIN BATTERY", buff);
}
if (osdStatGetState(OSD_STAT_END_BATTERY)) {
tfp_sprintf(buff, "%d.%1d%c", endBatteryVoltage / 10, endBatteryVoltage % 10, SYM_VOLT);
tfp_sprintf(buff, "%d.%02d%c", endBatteryVoltage / 100, endBatteryVoltage % 100, SYM_VOLT);
osdDisplayStatisticLabel(top++, "END BATTERY", buff);
}
if (osdStatGetState(OSD_STAT_BATTERY)) {
tfp_sprintf(buff, "%d.%1d%c", getBatteryVoltage() / 10, getBatteryVoltage() % 10, SYM_VOLT);
tfp_sprintf(buff, "%d.%02d%c", getBatteryVoltage() / 100, getBatteryVoltage() % 100, SYM_VOLT);
osdDisplayStatisticLabel(top++, "BATTERY", buff);
}

View file

@ -116,7 +116,7 @@ static void buildTelemetryFrame(uint8_t *packet)
if (rxFrSkySpiConfig()->useExternalAdc) {
a1Value = (adcGetChannel(ADC_EXTERNAL1) & 0xff0) >> 4;
} else {
a1Value = (2 * getBatteryVoltage()) & 0xff;
a1Value = (getBatteryVoltage() / 5) & 0xff;
}
const uint8_t a2Value = (adcGetChannel(ADC_RSSI)) >> 4;
telemetryId = packet[4];

View file

@ -198,7 +198,7 @@ static void buildTelemetryFrame(uint8_t *packet)
if (rxFrSkySpiConfig()->useExternalAdc) {
a1Value = (uint8_t)((adcGetChannel(ADC_EXTERNAL1) & 0xfe0) >> 5);
} else {
a1Value = getBatteryVoltage() & 0x7f;
a1Value = ((getBatteryVoltage() + 5) / 10) & 0x7f;
}
frame[4] = a1Value;
}

View file

@ -410,7 +410,7 @@ bool spektrumSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeConfi
static void dsmSendTelemetryPacket(void)
{
uint8_t packet[9];
const uint16_t voltage = getBatteryVoltage() * 10;
const uint16_t voltage = getBatteryVoltage();
packet[0] = DSM_TELEMETRY_FRAME_RPM;
packet[1] = 0xFF; //sid

View file

@ -218,7 +218,7 @@ static void buildAndWriteTelemetry (uint8_t *packet)
if (packet) {
static uint8_t bytesToWrite = FLYSKY_2A_PAYLOAD_SIZE; // first time write full packet to buffer a7105
flySky2ATelemetryPkt_t *telemertyPacket = (flySky2ATelemetryPkt_t*) packet;
uint16_t voltage = 10 * getBatteryVoltage();
uint16_t voltage = getBatteryVoltage();
telemertyPacket->type = FLYSKY_2A_PACKET_TELEMETRY;

View file

@ -58,7 +58,7 @@
static void batteryUpdateConsumptionState(void); // temporary forward reference
#define VBAT_STABLE_MAX_DELTA 2
#define VBAT_STABLE_MAX_DELTA 20
#define LVC_AFFECT_TIME 10000000 //10 secs for the LVC to slowly kick in
// Battery monitoring stuff
@ -94,10 +94,10 @@ PG_REGISTER_WITH_RESET_TEMPLATE(batteryConfig_t, batteryConfig, PG_BATTERY_CONFI
PG_RESET_TEMPLATE(batteryConfig_t, batteryConfig,
// voltage
.vbatmaxcellvoltage = 43,
.vbatmincellvoltage = 33,
.vbatwarningcellvoltage = 35,
.vbatnotpresentcellvoltage = 30, //A cell below 3 will be ignored
.vbatmaxcellvoltage = 430,
.vbatmincellvoltage = 330,
.vbatwarningcellvoltage = 350,
.vbatnotpresentcellvoltage = 300, //A cell below 3 will be ignored
.voltageMeterSource = DEFAULT_VOLTAGE_METER_SOURCE,
.lvcPercentage = 100, //Off by default at 100%
@ -114,7 +114,7 @@ PG_RESET_TEMPLATE(batteryConfig_t, batteryConfig,
.consumptionWarningPercentage = 10,
.vbathysteresis = 1,
.vbatfullcellvoltage = 41
.vbatfullcellvoltage = 410
);
void batteryUpdateVoltage(timeUs_t currentTimeUs)

View file

@ -29,10 +29,10 @@
typedef struct batteryConfig_s {
// voltage
uint8_t vbatmaxcellvoltage; // maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, default is 43 (4.3V)
uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery critical alarm, in 0.1V units, default is 33 (3.3V)
uint8_t vbatwarningcellvoltage; // warning voltage per cell, this triggers battery warning alarm, in 0.1V units, default is 35 (3.5V)
uint8_t vbatnotpresentcellvoltage; // Between vbatmaxcellvoltage and 2*this is considered to be USB powered. Below this it is notpresent
uint16_t vbatmaxcellvoltage; // maximum voltage per cell, used for auto-detecting battery voltage in 0.01V units, default is 430 (4.30V)
uint16_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery critical alarm, in 0.01V units, default is 330 (3.30V)
uint16_t vbatwarningcellvoltage; // warning voltage per cell, this triggers battery warning alarm, in 0.01V units, default is 350 (3.50V)
uint16_t vbatnotpresentcellvoltage; // Between vbatmaxcellvoltage and 2*this is considered to be USB powered. Below this it is notpresent
uint8_t lvcPercentage; // Percentage of throttle when lvc is triggered
voltageMeterSource_e voltageMeterSource; // source of battery voltage meter used, either ADC or ESC
@ -46,7 +46,7 @@ typedef struct batteryConfig_s {
uint8_t consumptionWarningPercentage; // Percentage of remaining capacity that should trigger a battery warning
uint8_t vbathysteresis; // hysteresis for alarm, default 1 = 0.1V
uint8_t vbatfullcellvoltage; // Cell voltage at which the battery is deemed to be "full" 0.1V units, default is 41 (4.1V)
uint16_t vbatfullcellvoltage; // Cell voltage at which the battery is deemed to be "full" 0.01V units, default is 410 (4.1V)
uint8_t forceBatteryCellCount; // number of cells in battery, used for overwriting auto-detected cell count if someone has issues with it.

View file

@ -101,8 +101,8 @@ void voltageMeterReset(voltageMeter_t *meter)
#endif
typedef struct voltageMeterADCState_s {
uint16_t voltageFiltered; // battery voltage in 0.1V steps (filtered)
uint16_t voltageUnfiltered; // battery voltage in 0.1V steps (unfiltered)
uint16_t voltageFiltered; // battery voltage in 0.01V steps (filtered)
uint16_t voltageUnfiltered; // battery voltage in 0.01V steps (unfiltered)
biquadFilter_t filter;
} voltageMeterADCState_t;
@ -145,8 +145,8 @@ static const uint8_t voltageMeterAdcChannelMap[] = {
STATIC_UNIT_TESTED uint16_t voltageAdcToVoltage(const uint16_t src, const voltageSensorADCConfig_t *config)
{
// calculate battery voltage based on ADC reading
// result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 10:1 voltage divider (10k:1k) * 10 for 0.1V
return ((((uint32_t)src * config->vbatscale * getVrefMv() / 100 + (0xFFF * 5)) / (0xFFF * config->vbatresdivval)) / config->vbatresdivmultiplier);
// result is Vbatt in 0.01V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 10:1 voltage divider (10k:1k) * 100 for 0.01V
return ((((uint32_t)src * config->vbatscale * getVrefMv() / 10 + (0xFFF * 5)) / (0xFFF * config->vbatresdivval)) / config->vbatresdivmultiplier);
}
void voltageMeterADCRefresh(void)
@ -201,8 +201,8 @@ void voltageMeterADCInit(void)
#ifdef USE_ESC_SENSOR
typedef struct voltageMeterESCState_s {
uint16_t voltageFiltered; // battery voltage in 0.1V steps (filtered)
uint16_t voltageUnfiltered; // battery voltage in 0.1V steps (unfiltered)
uint16_t voltageFiltered; // battery voltage in 0.01V steps (filtered)
uint16_t voltageUnfiltered; // battery voltage in 0.01V steps (unfiltered)
biquadFilter_t filter;
} voltageMeterESCState_t;
@ -224,7 +224,7 @@ void voltageMeterESCRefresh(void)
#ifdef USE_ESC_SENSOR
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
if (escData) {
voltageMeterESCState.voltageUnfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage / 10 : 0;
voltageMeterESCState.voltageUnfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage : 0;
voltageMeterESCState.voltageFiltered = biquadFilterApply(&voltageMeterESCState.filter, voltageMeterESCState.voltageUnfiltered);
}
#endif
@ -238,7 +238,7 @@ void voltageMeterESCReadMotor(uint8_t motorNumber, voltageMeter_t *voltageMeter)
#else
escSensorData_t *escData = getEscSensorData(motorNumber);
if (escData) {
voltageMeter->unfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage / 10 : 0;
voltageMeter->unfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage : 0;
voltageMeter->filtered = voltageMeter->unfiltered; // no filtering for ESC motors currently.
} else {
voltageMeterReset(voltageMeter);

View file

@ -38,8 +38,8 @@ extern const char * const voltageMeterSourceNames[VOLTAGE_METER_COUNT];
// WARNING - do not mix usage of VOLTAGE_METER_* and VOLTAGE_SENSOR_*, they are separate concerns.
typedef struct voltageMeter_s {
uint16_t filtered; // voltage in 0.1V steps
uint16_t unfiltered; // voltage in 0.1V steps
uint16_t filtered; // voltage in 0.01V steps
uint16_t unfiltered; // voltage in 0.01V steps
bool lowVoltageCutoff;
} voltageMeter_t;

View file

@ -98,8 +98,8 @@ void targetConfiguration(void)
}
batteryConfigMutable()->batteryCapacity = 250;
batteryConfigMutable()->vbatmincellvoltage = 28;
batteryConfigMutable()->vbatwarningcellvoltage = 33;
batteryConfigMutable()->vbatmincellvoltage = 280;
batteryConfigMutable()->vbatwarningcellvoltage = 330;
*customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R
*customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R

View file

@ -372,9 +372,9 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
bstWrite16(compassConfig()->mag_declination / 10);
bstWrite8(voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale);
bstWrite8(batteryConfig()->vbatmincellvoltage);
bstWrite8(batteryConfig()->vbatmaxcellvoltage);
bstWrite8(batteryConfig()->vbatwarningcellvoltage);
bstWrite8((batteryConfig()->vbatmincellvoltage + 5) / 10);
bstWrite8((batteryConfig()->vbatmaxcellvoltage + 5) / 10);
bstWrite8((batteryConfig()->vbatwarningcellvoltage + 5) / 10);
break;
case BST_FEATURE:
@ -529,9 +529,9 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
compassConfigMutable()->mag_declination = bstRead16() * 10;
voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale = bstRead8(); // actual vbatscale as intended
batteryConfigMutable()->vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI
batteryConfigMutable()->vbatmaxcellvoltage = bstRead8(); // vbatlevel_warn2 in MWC2.3 GUI
batteryConfigMutable()->vbatwarningcellvoltage = bstRead8(); // vbatlevel when buzzer starts to alert
batteryConfigMutable()->vbatmincellvoltage = bstRead8() * 10; // vbatlevel_warn1 in MWC2.3 GUI
batteryConfigMutable()->vbatmaxcellvoltage = bstRead8() * 10; // vbatlevel_warn2 in MWC2.3 GUI
batteryConfigMutable()->vbatwarningcellvoltage = bstRead8() * 10; // vbatlevel when buzzer starts to alert
break;
case BST_ACC_CALIBRATION:

View file

@ -55,9 +55,9 @@ void targetConfiguration(void)
voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale = VBAT_SCALE;
voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatresdivval = 15;
voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatresdivmultiplier = 4;
batteryConfigMutable()->vbatmaxcellvoltage = 44;
batteryConfigMutable()->vbatmincellvoltage = 32;
batteryConfigMutable()->vbatwarningcellvoltage = 33;
batteryConfigMutable()->vbatmaxcellvoltage = 440;
batteryConfigMutable()->vbatmincellvoltage = 320;
batteryConfigMutable()->vbatwarningcellvoltage = 330;
rxConfigMutable()->spektrum_sat_bind = 5;
rxConfigMutable()->spektrum_sat_bind_autoreset = 1;

View file

@ -30,6 +30,6 @@
void targetConfiguration(void)
{
batteryConfigMutable()->vbatmaxcellvoltage = 45;
batteryConfigMutable()->vbatmaxcellvoltage = 450;
}
#endif

View file

@ -201,9 +201,9 @@ void crsfFrameBatterySensor(sbuf_t *dst)
sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
sbufWriteU8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR);
if (telemetryConfig()->report_cell_voltage) {
sbufWriteU16BigEndian(dst, getBatteryAverageCellVoltage()); // vbat is in units of 0.1V
sbufWriteU16BigEndian(dst, (getBatteryAverageCellVoltage() + 5) / 10); // vbat is in units of 0.01V
} else {
sbufWriteU16BigEndian(dst, getBatteryVoltage());
sbufWriteU16BigEndian(dst, (getBatteryVoltage() + 5) / 10);
}
sbufWriteU16BigEndian(dst, getAmperage() / 10);
const uint32_t mAhDrawn = getMAhDrawn();

View file

@ -389,7 +389,7 @@ static void sendVoltageCells(void)
*/
static void sendVoltageAmp(void)
{
uint16_t voltage = getBatteryVoltage();
uint16_t voltage = (getBatteryVoltage() + 5) / 10;
const uint8_t cellCount = getBatteryCellCount();
if (telemetryConfig()->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) {

View file

@ -279,10 +279,11 @@ static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage)
static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage)
{
hottEAMMessage->main_voltage_L = getBatteryVoltage() & 0xFF;
hottEAMMessage->main_voltage_H = getBatteryVoltage() >> 8;
hottEAMMessage->batt1_voltage_L = getBatteryVoltage() & 0xFF;
hottEAMMessage->batt1_voltage_H = getBatteryVoltage() >> 8;
const uint16_t volt = (getBatteryVoltage() + 5) / 10;
hottEAMMessage->main_voltage_L = volt & 0xFF;
hottEAMMessage->main_voltage_H = volt >> 8;
hottEAMMessage->batt1_voltage_L = volt & 0xFF;
hottEAMMessage->batt1_voltage_H = volt >> 8;
updateAlarmBatteryStatus(hottEAMMessage);
}

View file

@ -191,7 +191,7 @@ static void setIbusSensorType(ibusAddress_t address)
static uint16_t getVoltage()
{
uint16_t voltage = getBatteryVoltage() *10;
uint16_t voltage = getBatteryVoltage();
if (telemetryConfig()->report_cell_voltage) {
voltage /= getBatteryCellCount();
}
@ -383,7 +383,7 @@ static void setValue(uint8_t* bufferPtr, uint8_t sensorType, uint8_t length)
value.uint16 = getMode();
break;
case IBUS_SENSOR_TYPE_CELL:
value.uint16 = (uint16_t)(getBatteryAverageCellVoltage() *10);
value.uint16 = (uint16_t)(getBatteryAverageCellVoltage());
break;
case IBUS_SENSOR_TYPE_BAT_CURR:
value.uint16 = (uint16_t)getAmperage();

View file

@ -306,7 +306,7 @@ int32_t getSensorValue(uint8_t sensor)
{
switch (sensor) {
case EX_VOLTAGE:
return getBatteryVoltage();
return ((getBatteryVoltage() + 5) / 10);
break;
case EX_CURRENT:
@ -322,7 +322,7 @@ int32_t getSensorValue(uint8_t sensor)
break;
case EX_POWER:
return (getBatteryVoltage() * getAmperage() / 100);
return (getBatteryVoltage() * getAmperage() / 1000);
break;
case EX_ROLL_ANGLE:

View file

@ -188,7 +188,7 @@ static void ltm_sframe(void)
if (failsafeIsActive())
lt_statemode |= 2;
ltm_initialise_packet('S');
ltm_serialise_16(getBatteryVoltage() * 100); //vbat converted to mv
ltm_serialise_16(getBatteryVoltage() * 10); //vbat converted to mV
ltm_serialise_16(0); // current, not implemented
ltm_serialise_8(constrain(scaleRange(getRssi(), 0, RSSI_MAX_VALUE, 0, 255), 0, 255)); // scaled RSSI (uchar)
ltm_serialise_8(0); // no airspeed

View file

@ -227,7 +227,7 @@ void mavlinkSendSystemStatus(void)
int8_t batteryRemaining = 100;
if (getBatteryState() < BATTERY_NOT_PRESENT) {
batteryVoltage = isBatteryVoltageConfigured() ? getBatteryVoltage() * 100 : batteryVoltage;
batteryVoltage = isBatteryVoltageConfigured() ? getBatteryVoltage() * 10 : batteryVoltage;
batteryAmperage = isAmperageConfigured() ? getAmperage() : batteryAmperage;
batteryRemaining = isBatteryVoltageConfigured() ? calculateBatteryPercentageRemaining() : batteryRemaining;
}

View file

@ -592,7 +592,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
cellCount = getBatteryCellCount();
vfasVoltage = cellCount ? getBatteryVoltage() / cellCount : 0;
}
smartPortSendPackage(id, vfasVoltage * 10); // given in 0.1V, convert to volts
smartPortSendPackage(id, vfasVoltage); // given in 0.01V, convert to volts
*clearToSend = false;
break;
#ifdef USE_ESC_SENSOR_TELEMETRY
@ -831,7 +831,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
#endif
case FSSP_DATAID_A4 :
cellCount = getBatteryCellCount();
vfasVoltage = cellCount ? (getBatteryVoltage() * 10 / cellCount) : 0; // given in 0.1V, convert to volts
vfasVoltage = cellCount ? (getBatteryVoltage() / cellCount) : 0; // given in 0.01V, convert to volts
smartPortSendPackage(id, vfasVoltage);
*clearToSend = false;
break;

View file

@ -153,7 +153,7 @@ bool srxlFrameRpm(sbuf_t *dst, timeUs_t currentTimeUs)
sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_RPM);
sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
sbufWriteU16BigEndian(dst, 0xFFFF); // pulse leading edges
sbufWriteU16BigEndian(dst, getBatteryVoltage() * 10); // vbat is in units of 0.1V
sbufWriteU16BigEndian(dst, getBatteryVoltage()); // vbat is in units of 0.01V
sbufWriteU16BigEndian(dst, 0x7FFF); // temperature
sbufFill(dst, 0xFF, STRU_TELE_RPM_EMPTY_FIELDS_COUNT);

View file

@ -116,7 +116,7 @@ void setDefaultSimulationState()
simulationBatteryState = BATTERY_OK;
simulationBatteryCellCount = 4;
simulationBatteryVoltage = 168;
simulationBatteryVoltage = 1680;
simulationBatteryAmperage = 0;
simulationMahDrawn = 0;
simulationAltitude = 0;
@ -203,8 +203,8 @@ TEST(OsdTest, TestInit)
// and
// this battery configuration (used for battery voltage elements)
batteryConfigMutable()->vbatmincellvoltage = 33;
batteryConfigMutable()->vbatmaxcellvoltage = 43;
batteryConfigMutable()->vbatmincellvoltage = 330;
batteryConfigMutable()->vbatmaxcellvoltage = 430;
// when
// OSD is initialised
@ -361,7 +361,7 @@ TEST(OsdTest, TestStatsImperial)
gpsSol.groundSpeed = 500;
GPS_distanceToHome = 20;
GPS_distanceFlownInCm = 2000;
simulationBatteryVoltage = 158;
simulationBatteryVoltage = 1580;
simulationAltitude = 100;
simulationTime += 1e6;
osdRefresh(simulationTime);
@ -370,7 +370,7 @@ TEST(OsdTest, TestStatsImperial)
gpsSol.groundSpeed = 800;
GPS_distanceToHome = 50;
GPS_distanceFlownInCm = 10000;
simulationBatteryVoltage = 147;
simulationBatteryVoltage = 1470;
simulationAltitude = 150;
simulationTime += 1e6;
osdRefresh(simulationTime);
@ -379,7 +379,7 @@ TEST(OsdTest, TestStatsImperial)
gpsSol.groundSpeed = 200;
GPS_distanceToHome = 100;
GPS_distanceFlownInCm = 20000;
simulationBatteryVoltage = 152;
simulationBatteryVoltage = 1520;
simulationAltitude = 200;
simulationTime += 1e6;
osdRefresh(simulationTime);
@ -396,8 +396,8 @@ TEST(OsdTest, TestStatsImperial)
displayPortTestBufferSubstring(2, row++, "LAST ARM : 00:03");
displayPortTestBufferSubstring(2, row++, "MAX SPEED : 17");
displayPortTestBufferSubstring(2, row++, "MAX DISTANCE : 328%c", SYM_FT);
displayPortTestBufferSubstring(2, row++, "MIN BATTERY : 14.7%c", SYM_VOLT);
displayPortTestBufferSubstring(2, row++, "END BATTERY : 15.2%c", SYM_VOLT);
displayPortTestBufferSubstring(2, row++, "MIN BATTERY : 14.70%c", SYM_VOLT);
displayPortTestBufferSubstring(2, row++, "END BATTERY : 15.20%c", SYM_VOLT);
displayPortTestBufferSubstring(2, row++, "MIN RSSI : 25%%");
displayPortTestBufferSubstring(2, row++, "MAX ALTITUDE : 6.5%c", SYM_FT);
displayPortTestBufferSubstring(2, row++, "FLIGHT DISTANCE : 656%c", SYM_FT);
@ -427,13 +427,13 @@ TEST(OsdTest, TestStatsMetric)
gpsSol.groundSpeed = 800;
GPS_distanceToHome = 100;
GPS_distanceFlownInCm = 10000;
simulationBatteryVoltage = 147;
simulationBatteryVoltage = 1470;
simulationAltitude = 200;
simulationTime += 1e6;
osdRefresh(simulationTime);
osdRefresh(simulationTime);
simulationBatteryVoltage = 152;
simulationBatteryVoltage = 1520;
simulationTime += 1e6;
osdRefresh(simulationTime);
@ -449,8 +449,8 @@ TEST(OsdTest, TestStatsMetric)
displayPortTestBufferSubstring(2, row++, "LAST ARM : 00:02");
displayPortTestBufferSubstring(2, row++, "MAX SPEED : 28");
displayPortTestBufferSubstring(2, row++, "MAX DISTANCE : 100%c", SYM_M);
displayPortTestBufferSubstring(2, row++, "MIN BATTERY : 14.7%c", SYM_VOLT);
displayPortTestBufferSubstring(2, row++, "END BATTERY : 15.2%c", SYM_VOLT);
displayPortTestBufferSubstring(2, row++, "MIN BATTERY : 14.70%c", SYM_VOLT);
displayPortTestBufferSubstring(2, row++, "END BATTERY : 15.20%c", SYM_VOLT);
displayPortTestBufferSubstring(2, row++, "MIN RSSI : 25%%");
displayPortTestBufferSubstring(2, row++, "MAX ALTITUDE : 2.0%c", SYM_M);
displayPortTestBufferSubstring(2, row++, "FLIGHT DISTANCE : 100%c", SYM_M);
@ -513,7 +513,7 @@ TEST(OsdTest, TestAlarms)
printf("%d\n", i);
#endif
displayPortTestBufferSubstring(8, 1, "%c99", SYM_RSSI);
displayPortTestBufferSubstring(12, 1, "%c16.8%c", SYM_BATT_FULL, SYM_VOLT);
displayPortTestBufferSubstring(12, 1, "%c16.80%c", SYM_BATT_FULL, SYM_VOLT);
displayPortTestBufferSubstring(1, 1, "%c00:", SYM_FLY_M); // only test the minute part of the timer
displayPortTestBufferSubstring(20, 1, "%c01:", SYM_ON_M); // only test the minute part of the timer
displayPortTestBufferSubstring(23, 7, " .0%c", SYM_M);
@ -523,7 +523,7 @@ TEST(OsdTest, TestAlarms)
// all values are out of range
rssi = 128;
simulationBatteryState = BATTERY_CRITICAL;
simulationBatteryVoltage = 135;
simulationBatteryVoltage = 1350;
simulationAltitude = 12000;
simulationTime += 60e6;
osdRefresh(simulationTime);
@ -542,7 +542,7 @@ TEST(OsdTest, TestAlarms)
#endif
if (i % 2 == 0) {
displayPortTestBufferSubstring(8, 1, "%c12", SYM_RSSI);
displayPortTestBufferSubstring(12, 1, "%c13.5%c", SYM_MAIN_BATT, SYM_VOLT);
displayPortTestBufferSubstring(12, 1, "%c13.50%c", SYM_MAIN_BATT, SYM_VOLT);
displayPortTestBufferSubstring(1, 1, "%c01:", SYM_FLY_M); // only test the minute part of the timer
displayPortTestBufferSubstring(20, 1, "%c02:", SYM_ON_M); // only test the minute part of the timer
displayPortTestBufferSubstring(23, 7, " 120.0%c", SYM_M);
@ -677,7 +677,7 @@ TEST(OsdTest, TestElementPower)
osdConfigMutable()->item_pos[OSD_POWER] = OSD_POS(1, 10) | OSD_PROFILE_1_FLAG;
// and
simulationBatteryVoltage = 100; // 10V
simulationBatteryVoltage = 1000; // 10V
// and
simulationBatteryAmperage = 0; // 0A
@ -837,7 +837,7 @@ TEST(OsdTest, TestElementWarningsBattery)
osdWarnSetState(OSD_WARNING_BATTERY_NOT_FULL, true);
// and
batteryConfigMutable()->vbatfullcellvoltage = 41;
batteryConfigMutable()->vbatfullcellvoltage = 410;
// and
// 4S battery
@ -845,7 +845,7 @@ TEST(OsdTest, TestElementWarningsBattery)
// and
// full battery
simulationBatteryVoltage = 168;
simulationBatteryVoltage = 1680;
simulationBatteryState = BATTERY_OK;
// when
@ -857,7 +857,7 @@ TEST(OsdTest, TestElementWarningsBattery)
// given
// low battery
simulationBatteryVoltage = 140;
simulationBatteryVoltage = 1400;
simulationBatteryState = BATTERY_WARNING;
// when
@ -868,8 +868,8 @@ TEST(OsdTest, TestElementWarningsBattery)
displayPortTestBufferSubstring(9, 10, "LOW BATTERY ");
// given
// crtical battery
simulationBatteryVoltage = 132;
// critical battery
simulationBatteryVoltage = 1320;
simulationBatteryState = BATTERY_CRITICAL;
// when
@ -881,7 +881,7 @@ TEST(OsdTest, TestElementWarningsBattery)
// given
// used battery
simulationBatteryVoltage = ((batteryConfig()->vbatmaxcellvoltage - 2) * simulationBatteryCellCount) - 1;
simulationBatteryVoltage = ((batteryConfig()->vbatmaxcellvoltage - 20) * simulationBatteryCellCount) - 1;
simulationBatteryState = BATTERY_OK;
// when
@ -893,7 +893,7 @@ TEST(OsdTest, TestElementWarningsBattery)
// given
// full battery
simulationBatteryVoltage = ((batteryConfig()->vbatmaxcellvoltage - 2) * simulationBatteryCellCount);
simulationBatteryVoltage = ((batteryConfig()->vbatmaxcellvoltage - 20) * simulationBatteryCellCount);
simulationBatteryState = BATTERY_OK;
// when

View file

@ -164,7 +164,7 @@ TEST(TelemetryCrsfTest, TestBattery)
EXPECT_EQ(67, remaining);
EXPECT_EQ(crfsCrc(frame, frameLen), frame[11]);
testBatteryVoltage = 33; // 3.3V = 3300 mv
testBatteryVoltage = 330; // 3.3V = 3300 mv
testAmperage = 2960; // = 29.60A = 29600mA - amperage is in 0.01A steps
testmAhDrawn = 1234;
frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_BATTERY_SENSOR);

View file

@ -62,7 +62,7 @@ int16_t gyroGetTemperature(void) {
return gyroTemperature;
}
static uint16_t vbat = 100;
static uint16_t vbat = 1000;
uint16_t getVbat(void)
{
return vbat;
@ -128,7 +128,7 @@ typedef struct serialPortStub_s {
} serialPortStub_t;
static uint16_t testBatteryVoltage = 100;
static uint16_t testBatteryVoltage = 1000;
uint16_t getBatteryVoltage(void)
{
return testBatteryVoltage;
@ -469,13 +469,13 @@ TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToGetMeasurementVbattCellV
//Given ibus command: Sensor at address 1, please send your measurement
//then we respond with: I'm reading 0.1 volts
testBatteryCellCount =3;
testBatteryVoltage = 30;
testBatteryVoltage = 300;
checkResponseToCommand("\x04\xA1\x5a\xff", 4, "\x06\xA1\x64\x00\xf4\xFe", 6);
//Given ibus command: Sensor at address 1, please send your measurement
//then we respond with: I'm reading 0.1 volts
testBatteryCellCount =1;
testBatteryVoltage = 10;
testBatteryVoltage = 100;
checkResponseToCommand("\x04\xA1\x5a\xff", 4, "\x06\xA1\x64\x00\xf4\xFe", 6);
}
@ -486,13 +486,13 @@ TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToGetMeasurementVbattPackV
//Given ibus command: Sensor at address 1, please send your measurement
//then we respond with: I'm reading 0.1 volts
testBatteryCellCount =3;
testBatteryVoltage = 10;
testBatteryVoltage = 100;
checkResponseToCommand("\x04\xA1\x5a\xff", 4, "\x06\xA1\x64\x00\xf4\xFe", 6);
//Given ibus command: Sensor at address 1, please send your measurement
//then we respond with: I'm reading 0.1 volts
testBatteryCellCount =1;
testBatteryVoltage = 10;
testBatteryVoltage = 100;
checkResponseToCommand("\x04\xA1\x5a\xff", 4, "\x06\xA1\x64\x00\xf4\xFe", 6);
}
@ -580,7 +580,7 @@ TEST_F(IbusTelemteryProtocolUnitTestDaisyChained, Test_IbusRespondToGetMeasureme
//Given ibus command: Sensor at address 3, please send your measurement
//then we respond with: I'm reading 0.1 volts
testBatteryCellCount = 1;
testBatteryVoltage = 10;
testBatteryVoltage = 100;
checkResponseToCommand("\x04\xA3\x58\xff", 4, "\x06\xA3\x64\x00\xf2\xfe", 6);
//Given ibus command: Sensor at address 4, please send your measurement