Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

[ESC] Add battery voltage ESC sensor

Browse source
This commit is contained in:
Konstantin (DigitalEntity) Sharlaimov 2019-09-29 10:29:52 +02:00 • committed by Pawel Spychalski (DzikuVx)
parent 15c1412ff7
commit 4063eb6e07
6 changed files with 62 additions and 21 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/blackbox/blackbox.c
View file

@ -1625,7 +1625,7 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
blackboxPrintfHeaderLine("vbat_scale", "%u", batteryMetersConfig()->voltage_scale / 10);
blackboxPrintfHeaderLine("vbat_scale", "%u", batteryMetersConfig()->voltage.scale / 10);
} else {
xmitState.headerIndex += 2; // Skip the next two vbat fields too
}

16
src/main/fc/fc_msp.c
View file

@ -723,7 +723,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, compassConfig()->mag_declination / 10);
sbufWriteU8(dst, batteryMetersConfig()->voltage_scale / 10);
sbufWriteU8(dst, batteryMetersConfig()->voltage.scale / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10);
@ -751,7 +751,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, compassConfig()->mag_declination / 10);
sbufWriteU16(dst, batteryMetersConfig()->voltage_scale);
sbufWriteU16(dst, batteryMetersConfig()->voltage.scale);
sbufWriteU8(dst, batteryMetersConfig()->voltageSource);
sbufWriteU8(dst, currentBatteryProfile->cells);
sbufWriteU16(dst, currentBatteryProfile->voltage.cellDetect);
@ -766,7 +766,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break;
case MSP2_INAV_BATTERY_CONFIG:
sbufWriteU16(dst, batteryMetersConfig()->voltage_scale);
sbufWriteU16(dst, batteryMetersConfig()->voltage.scale);
sbufWriteU8(dst, batteryMetersConfig()->voltageSource);
sbufWriteU8(dst, currentBatteryProfile->cells);
sbufWriteU16(dst, currentBatteryProfile->voltage.cellDetect);
@ -872,7 +872,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break;
case MSP_VOLTAGE_METER_CONFIG:
sbufWriteU8(dst, batteryMetersConfig()->voltage_scale / 10);
sbufWriteU8(dst, batteryMetersConfig()->voltage.scale / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10);
@ -1765,7 +1765,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
sbufReadU16(src);
#endif
batteryMetersConfigMutable()->voltage_scale = sbufReadU8(src) * 10;
batteryMetersConfigMutable()->voltage.scale = sbufReadU8(src) * 10;
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
@ -1803,7 +1803,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
sbufReadU16(src);
#endif
batteryMetersConfigMutable()->voltage_scale = sbufReadU16(src);
batteryMetersConfigMutable()->voltage.scale = sbufReadU16(src);
batteryMetersConfigMutable()->voltageSource = sbufReadU8(src);
currentBatteryProfileMutable->cells = sbufReadU8(src);
currentBatteryProfileMutable->voltage.cellDetect = sbufReadU16(src);
@ -1829,7 +1829,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP2_INAV_SET_BATTERY_CONFIG:
if (dataSize == 29) {
batteryMetersConfigMutable()->voltage_scale = sbufReadU16(src);
batteryMetersConfigMutable()->voltage.scale = sbufReadU16(src);
batteryMetersConfigMutable()->voltageSource = sbufReadU8(src);
currentBatteryProfileMutable->cells = sbufReadU8(src);
currentBatteryProfileMutable->voltage.cellDetect = sbufReadU16(src);
@ -2475,7 +2475,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP_SET_VOLTAGE_METER_CONFIG:
if (dataSize >= 4) {
batteryMetersConfigMutable()->voltage_scale = sbufReadU8(src) * 10;
batteryMetersConfigMutable()->voltage.scale = sbufReadU8(src) * 10;
currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10;
currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10;
currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10;

9
src/main/fc/settings.yaml
View file

@ -38,6 +38,9 @@ tables:
- name: current_sensor
values: ["NONE", "ADC", "VIRTUAL", "ESC"]
enum: currentSensor_e
- name: voltage_sensor
values: ["NONE", "ADC", "ESC"]
enum: voltageSensor_e
- name: gps_provider
values: ["NMEA", "UBLOX", "UNUSED", "NAZA", "UBLOX7", "MTK"]
enum: gpsProvider_e
@ -588,8 +591,12 @@ groups:
type: batteryMetersConfig_t
headers: ["sensors/battery.h"]
members:
- name: vbat_meter_type
field: voltage.type
table: voltage_sensor
type: uint8_t
- name: vbat_scale
field: voltage_scale
field: voltage.scale
condition: USE_ADC
min: VBAT_SCALE_MIN
max: VBAT_SCALE_MAX

35
src/main/sensors/battery.c
View file

@ -116,11 +116,14 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance)
}
}
PG_REGISTER_WITH_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, PG_BATTERY_METERS_CONFIG, 0);
PG_REGISTER_WITH_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, PG_BATTERY_METERS_CONFIG, 1);
PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig,
.voltage_scale = VBAT_SCALE_DEFAULT,
.voltage = {
.type = VOLTAGE_SENSOR_ADC,
.scale = VBAT_SCALE_DEFAULT,
},
.current = {
.type = CURRENT_SENSOR_ADC,
@ -203,9 +206,32 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected)
{
static pt1Filter_t vbatFilterState;
switch (batteryMetersConfig()->voltage.type) {
case VOLTAGE_SENSOR_ADC:
{
// calculate battery voltage based on ADC reading
// result is Vbatt in 0.01V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 1100 = 11:1 voltage divider (10k:1k)
vbat = (uint64_t)adcGetChannel(ADC_BATTERY) * batteryMetersConfig()->voltage_scale * ADCVREF / (0xFFF * 1000);
vbat = (uint64_t)adcGetChannel(ADC_BATTERY) * batteryMetersConfig()->voltage.scale * ADCVREF / (0xFFF * 1000);
break;
}
#if defined(USE_ESC_SENSOR)
case VOLTAGE_SENSOR_ESC:
{
escSensorData_t * escSensor = escSensorGetData();
if (escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE) {
vbat = escSensor->voltage;
}
else {
vbat = 0;
}
}
break;
#endif
case VOLTAGE_SENSOR_NONE:
default:
vbat = 0;
break;
}
if (justConnected) {
pt1FilterReset(&vbatFilterState, vbat);
@ -451,10 +477,9 @@ void currentMeterUpdate(timeUs_t timeDelta)
{
escSensorData_t * escSensor = escSensorGetData();
if (escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE) {
amperage = escSensor->current;
amperage = pt1FilterApply4(&amperageFilterState, escSensor->current, AMPERAGE_LPF_FREQ, timeDelta * 1e-6f);
}
else {
amperage = pt1FilterApply4(&amperageFilterState, amperage, AMPERAGE_LPF_FREQ, timeDelta * 1e-6f);
amperage = 0;
}
}

11
src/main/sensors/battery.h
View file

@ -46,6 +46,12 @@ typedef enum {
CURRENT_SENSOR_MAX = CURRENT_SENSOR_VIRTUAL
} currentSensor_e;
typedef enum {
VOLTAGE_SENSOR_NONE = 0,
VOLTAGE_SENSOR_ADC,
VOLTAGE_SENSOR_ESC
} voltageSensor_e;
typedef enum {
BAT_CAPACITY_UNIT_MAH,
BAT_CAPACITY_UNIT_MWH,
@ -63,7 +69,10 @@ typedef enum {
typedef struct batteryMetersConfig_s {
uint16_t voltage_scale;
struct {
uint16_t scale;
voltageSensor_e type;
} voltage;
struct {
int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A

6
src/main/sensors/esc_sensor.c
View file

@ -108,9 +108,9 @@ static bool escSensorDecodeFrame(void)
if (checksum == telemetryBuffer[TELEMETRY_FRAME_SIZE - 1]) {
escSensorData[escSensorMotor].dataAge = 0;
escSensorData[escSensorMotor].temperature = telemetryBuffer[0];
escSensorData[escSensorMotor].voltage = telemetryBuffer[1] << 8 | telemetryBuffer[2];
escSensorData[escSensorMotor].current = telemetryBuffer[3] << 8 | telemetryBuffer[4];
escSensorData[escSensorMotor].erpm = telemetryBuffer[7] << 8 | telemetryBuffer[8];
escSensorData[escSensorMotor].voltage = ((uint16_t)telemetryBuffer[1]) << 8 | telemetryBuffer[2];
escSensorData[escSensorMotor].current = ((uint16_t)telemetryBuffer[3]) << 8 | telemetryBuffer[4];
escSensorData[escSensorMotor].erpm = ((uint16_t)telemetryBuffer[7]) << 8 | telemetryBuffer[8];
escSensorDataNeedsUpdate = true;
return ESC_SENSOR_FRAME_COMPLETE;
}