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

CF/BF - First cut of Current/Voltage/Battery cleanup.

Browse source 
many refactorings, bugs squished, concerns separated,
single-responsibility violations fixed and performance optimizations.
This commit is contained in:
Hydra 2017-03-12 11:26:30 +00:00 • committed by Dominic Clifton
parent b46e0fe46d
commit 067d3c0ac2
69 changed files with 1178 additions and 514 deletions
Download patch file Download diff file Expand all files Collapse all files

2
Makefile
View file

@ -641,6 +641,8 @@ COMMON_SRC = \
scheduler/scheduler.c \
sensors/acceleration.c \
sensors/battery.c \
sensors/current.c \
sensors/voltage.c \
sensors/boardalignment.c \
sensors/compass.c \
sensors/gyro.c \

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

@ -425,10 +425,10 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
#endif
case FLIGHT_LOG_FIELD_CONDITION_VBAT:
return feature(FEATURE_VBAT);
return batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE;
case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC:
return feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC;
return batteryConfig()->currentMeterSource == CURRENT_METER_ADC;
case FLIGHT_LOG_FIELD_CONDITION_SONAR:
#ifdef SONAR
@ -841,7 +841,7 @@ void startBlackbox(void)
blackboxHistory[1] = &blackboxHistoryRing[1];
blackboxHistory[2] = &blackboxHistoryRing[2];
vbatReference = vbatLatest;
vbatReference = getBatteryVoltageLatest();
//No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
@ -1037,8 +1037,8 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->motor[i] = motor[i];
}
blackboxCurrent->vbatLatest = vbatLatest;
blackboxCurrent->amperageLatest = amperageLatest;
blackboxCurrent->vbatLatest = getBatteryVoltageLatest();
blackboxCurrent->amperageLatest = getAmperageLatest();
#ifdef MAG
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
@ -1214,7 +1214,7 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
blackboxPrintfHeaderLine("vbatscale:%u", batteryConfig()->vbatscale);
blackboxPrintfHeaderLine("vbatscale:%u", voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale);
} else {
xmitState.headerIndex += 2; // Skip the next two vbat fields too
}
@ -1226,9 +1226,8 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("vbatref:%u", vbatReference);
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
//Note: Log even if this is a virtual current meter, since the virtual meter uses these parameters too:
if (feature(FEATURE_CURRENT_METER)) {
blackboxPrintfHeaderLine("currentMeter:%d,%d", batteryConfig()->currentMeterOffset, batteryConfig()->currentMeterScale);
if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) {
blackboxPrintfHeaderLine("currentSensor:%d,%d",currentMeterADCOrVirtualConfig(CURRENT_SENSOR_ADC)->offset, currentMeterADCOrVirtualConfig(CURRENT_SENSOR_ADC)->scale);
}
);

14
src/main/cms/cms_menu_misc.c
View file

@ -91,14 +91,14 @@ CMS_Menu cmsx_menuRcPreview = {
static uint16_t motorConfig_minthrottle;
static uint8_t motorConfig_digitalIdleOffsetPercent;
static uint8_t batteryConfig_vbatscale;
static uint8_t voltageSensorADCConfig_vbatscale;
static uint8_t batteryConfig_vbatmaxcellvoltage;
static long cmsx_menuMiscOnEnter(void)
{
motorConfig_minthrottle = motorConfig()->minthrottle;
motorConfig_digitalIdleOffsetPercent = 10 * motorConfig()->digitalIdleOffsetPercent;
batteryConfig_vbatscale = batteryConfig()->vbatscale;
voltageSensorADCConfig_vbatscale = voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale;
batteryConfig_vbatmaxcellvoltage = batteryConfig()->vbatmaxcellvoltage;
return 0;
}
@ -109,7 +109,7 @@ static long cmsx_menuMiscOnExit(const OSD_Entry *self)
motorConfigMutable()->minthrottle = motorConfig_minthrottle;
motorConfigMutable()->digitalIdleOffsetPercent = motorConfig_digitalIdleOffsetPercent / 10.0f;
batteryConfigMutable()->vbatscale = batteryConfig_vbatscale;
voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale = voltageSensorADCConfig_vbatscale;
batteryConfigMutable()->vbatmaxcellvoltage = batteryConfig_vbatmaxcellvoltage;
return 0;
}
@ -118,10 +118,10 @@ static OSD_Entry menuMiscEntries[]=
{
{ "-- MISC --", OME_Label, NULL, NULL, 0 },
{ "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &motorConfig_minthrottle, 1000, 2000, 1 }, 0 },
{ "DIGITAL IDLE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &motorConfig_digitalIdleOffsetPercent, 0, 200, 1, 100 }, 0 },
{ "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig_vbatscale, 1, 250, 1 }, 0 },
{ "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig_vbatmaxcellvoltage, 10, 50, 1 }, 0 },
{ "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &motorConfig_minthrottle, 1000, 2000, 1 }, 0 },
{ "DIGITAL IDLE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &motorConfig_digitalIdleOffsetPercent, 0, 200, 1, 100 }, 0 },
{ "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &voltageSensorADCConfig_vbatscale, 1, 250, 1 }, 0 },
{ "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig_vbatmaxcellvoltage, 10, 50, 1 }, 0 },
{ "RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0},
{ "BACK", OME_Back, NULL, NULL, 0},

7
src/main/config/parameter_group_ids.h
View file

@ -67,9 +67,12 @@
#define PG_SPECIAL_COLOR_CONFIG 46 // part of led strip, structs OK
#define PG_PILOT_CONFIG 47 // does not exist in betaflight
#define PG_MSP_SERVER_CONFIG 48 // does not exist in betaflight
#define PG_VOLTAGE_METER_CONFIG 49 // Cleanflight has voltageMeterConfig_t, betaflight has batteryConfig_t
#define PG_AMPERAGE_METER_CONFIG 50 // Cleanflight has amperageMeterConfig_t, betaflight has batteryConfig_t
#define PG_VOLTAGE_SENSOR_ADC_CONFIG 49 // renamed from PG_VOLTAGE_METER_CONFIG
#define PG_CURRENT_SENSOR_ADC_OR_VIRTUAL_CONFIG 50 // renamed from PG_AMPERAGE_METER_CONFIG
#define PG_DEBUG_CONFIG 51 // does not exist in betaflight
#define PG_SERVO_CONFIG 52
#define PG_IBUS_TELEMETRY_CONFIG 53 // CF 1.x
//#define PG_VTX_CONFIG 54 // CF 1.x
// Driver configuration
#define PG_DRIVER_PWM_RX_CONFIG 100 // does not exist in betaflight

2
src/main/drivers/adc.h
View file

@ -43,7 +43,7 @@ typedef struct adcChannelConfig_t {
typedef struct adcConfig_s {
adcChannelConfig_t vbat;
adcChannelConfig_t rssi;
adcChannelConfig_t currentMeter;
adcChannelConfig_t currentMeter; // FIXME rename to current/amperage
adcChannelConfig_t external1;
} adcConfig_t;

37
src/main/fc/cli.c
View file

@ -249,7 +249,7 @@ static const char * const lookupTableCurrentSensor[] = {
};
static const char * const lookupTableBatterySensor[] = {
"ADC", "ESC"
"NONE", "ADC", "ESC"
};
#ifdef USE_SERVOS
@ -339,7 +339,7 @@ static const char * const lookupTableSuperExpoYaw[] = {
static const char * const lookupTablePwmProtocol[] = {
"OFF", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED",
#ifdef USE_DSHOT
"DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200",
"DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200"
#endif
};
@ -375,8 +375,8 @@ typedef enum {
#ifdef BLACKBOX
TABLE_BLACKBOX_DEVICE,
#endif
TABLE_CURRENT_SENSOR,
TABLE_BATTERY_SENSOR,
TABLE_CURRENT_METER,
TABLE_VOLTAGE_METER,
#ifdef USE_SERVOS
TABLE_GIMBAL_MODE,
#endif
@ -622,21 +622,26 @@ static 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_scale", VAR_UINT8 | MASTER_VALUE, .config.minmax = { VBAT_SCALE_MIN, VBAT_SCALE_MAX }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatscale) },
{ "vbat_max_cell_voltage", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 10, 50 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatmaxcellvoltage) },
{ "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_hysteresis", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 250 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbathysteresis) },
{ "ibat_scale", VAR_INT16 | MASTER_VALUE, .config.minmax = { -16000, 16000 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, currentMeterScale) },
{ "ibat_offset", VAR_INT16 | MASTER_VALUE, .config.minmax = { -16000, 16000 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, currentMeterOffset) },
{ "mwii_ibat_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, multiwiiCurrentMeterOutput) },
{ "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CURRENT_SENSOR }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, currentMeterType) },
{ "battery_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BATTERY_SENSOR }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, batteryMeterType) },
{ "bat_detect_thresh", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, batterynotpresentlevel) },
{ "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) },
{ "bat_detect_thresh", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, batteryNotPresentLevel) },
{ "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) },
// PG_VOLTAGE_SENSOR_ADC_CONFIG
{ "vbat_scale", VAR_UINT8 | MASTER_VALUE, .config.minmax = { VBAT_SCALE_MIN, VBAT_SCALE_MAX }, PG_VOLTAGE_SENSOR_ADC_CONFIG, offsetof(voltageSensorADCConfig_t, vbatscale) },
// PG_CURRENT_SENSOR_ADC_OR_VIRTUAL_CONFIG
// FIXME this will only allow configuration of the FIRST current meter. there are currently two
{ "ibat_scale", VAR_INT16 | MASTER_VALUE, .config.minmax = { -16000, 16000 }, PG_CURRENT_SENSOR_ADC_OR_VIRTUAL_CONFIG, offsetof(currentMeterADCOrVirtualConfig_t, scale) },
{ "ibat_offset", VAR_INT16 | MASTER_VALUE, .config.minmax = { -16000, 16000 }, PG_CURRENT_SENSOR_ADC_OR_VIRTUAL_CONFIG, offsetof(currentMeterADCOrVirtualConfig_t, offset) },
// PG_BEEPER_DEV_CONFIG
#ifdef BEEPER
{ "beeper_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BEEPER_DEV_CONFIG, offsetof(beeperDevConfig_t, isInverted) },
@ -3739,7 +3744,7 @@ static void cliStatus(char *cmdline)
UNUSED(cmdline);
cliPrintf("System Uptime: %d seconds\r\n", millis() / 1000);
cliPrintf("Voltage: %d * 0.1V (%dS battery - %s)\r\n", getVbat(), batteryCellCount, getBatteryStateString());
cliPrintf("Voltage: %d * 0.1V (%dS battery - %s)\r\n", getBatteryVoltage(), getBatteryCellCount(), getBatteryStateString());
cliPrintf("CPU Clock=%dMHz", (SystemCoreClock / 1000000));
@ -3800,7 +3805,7 @@ static void cliTasks(char *cmdline)
#ifndef MINIMAL_CLI
if (systemConfig()->task_statistics) {
cliPrintf("Task list rate/hz max/us avg/us maxload avgload total/ms\r\n");
cliPrintf("Task list rate/hz max/us avg/us maxload avgload total/ms\r\n");
} else {
cliPrintf("Task list\r\n");
}
@ -3815,14 +3820,14 @@ static void cliTasks(char *cmdline)
subTaskFrequency = taskInfo.latestDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)taskInfo.latestDeltaTime));
taskFrequency = subTaskFrequency / pidConfig()->pid_process_denom;
if (pidConfig()->pid_process_denom > 1) {
cliPrintf("%02d - (%13s) ", taskId, taskInfo.taskName);
cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName);
} else {
taskFrequency = subTaskFrequency;
cliPrintf("%02d - (%9s/%3s) ", taskId, taskInfo.subTaskName, taskInfo.taskName);
cliPrintf("%02d - (%11s/%3s) ", taskId, taskInfo.subTaskName, taskInfo.taskName);
}
} else {
taskFrequency = taskInfo.latestDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)taskInfo.latestDeltaTime));
cliPrintf("%02d - (%13s) ", taskId, taskInfo.taskName);
cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName);
}
const int maxLoad = taskInfo.maxExecutionTime == 0 ? 0 :(taskInfo.maxExecutionTime * taskFrequency + 5000) / 1000;
const int averageLoad = taskInfo.averageExecutionTime == 0 ? 0 : (taskInfo.averageExecutionTime * taskFrequency + 5000) / 1000;
@ -3838,7 +3843,7 @@ static void cliTasks(char *cmdline)
cliPrintf("%6d\r\n", taskFrequency);
}
if (taskId == TASK_GYROPID && pidConfig()->pid_process_denom > 1) {
cliPrintf(" - (%13s) %6d\r\n", taskInfo.subTaskName, subTaskFrequency);
cliPrintf(" - (%15s) %6d\r\n", taskInfo.subTaskName, subTaskFrequency);
}
}
}

19
src/main/fc/config.c
View file

@ -113,7 +113,7 @@ pidProfile_t *currentPidProfile;
PG_REGISTER_WITH_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_FEATURE_CONFIG, 0);
PG_RESET_TEMPLATE(featureConfig_t, featureConfig,
.enabledFeatures = DEFAULT_FEATURES | DEFAULT_RX_FEATURE | FEATURE_FAILSAFE
.enabledFeatures = DEFAULT_FEATURES | DEFAULT_RX_FEATURE | FEATURE_FAILSAFE
);
PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
@ -503,12 +503,6 @@ void createDefaultConfig(master_t *config)
#endif
#endif // USE_PARAMETER_GROUPS
#ifdef BOARD_HAS_VOLTAGE_DIVIDER
// only enable the VBAT feature by default if the board has a voltage divider otherwise
// the user may see incorrect readings and unexpected issues with pin mappings may occur.
intFeatureSet(FEATURE_VBAT, featuresPtr);
#endif
config->version = EEPROM_CONF_VERSION;
// global settings
@ -873,8 +867,8 @@ void validateAndFixConfig(void)
// rssi adc needs the same ports
featureClear(FEATURE_RSSI_ADC);
// current meter needs the same ports
if (batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
featureClear(FEATURE_CURRENT_METER);
if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) {
batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE;
}
#endif
// software serial needs free PWM ports
@ -883,14 +877,15 @@ void validateAndFixConfig(void)
#ifdef USE_SOFTSPI
if (featureConfigured(FEATURE_SOFTSPI)) {
featureClear(FEATURE_RX_PPM | FEATURE_RX_PARALLEL_PWM | FEATURE_SOFTSERIAL | FEATURE_VBAT);
featureClear(FEATURE_RX_PPM | FEATURE_RX_PARALLEL_PWM | FEATURE_SOFTSERIAL);
batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_NONE;
#if defined(STM32F10X)
featureClear(FEATURE_LED_STRIP);
// rssi adc needs the same ports
featureClear(FEATURE_RSSI_ADC);
// current meter needs the same ports
if (batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
featureClear(FEATURE_CURRENT_METER);
if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) {
batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE;
}
#endif
}

4
src/main/fc/config.h
View file

@ -34,7 +34,7 @@
typedef enum {
FEATURE_RX_PPM = 1 << 0,
FEATURE_VBAT = 1 << 1,
//FEATURE_VBAT = 1 << 1,
FEATURE_INFLIGHT_ACC_CAL = 1 << 2,
FEATURE_RX_SERIAL = 1 << 3,
FEATURE_MOTOR_STOP = 1 << 4,
@ -44,7 +44,7 @@ typedef enum {
FEATURE_FAILSAFE = 1 << 8,
FEATURE_SONAR = 1 << 9,
FEATURE_TELEMETRY = 1 << 10,
FEATURE_CURRENT_METER = 1 << 11,
//FEATURE_CURRENT_METER = 1 << 11,
FEATURE_3D = 1 << 12,
FEATURE_RX_PARALLEL_PWM = 1 << 13,
FEATURE_RX_MSP = 1 << 14,

11
src/main/fc/fc_init.c
View file

@ -377,9 +377,9 @@ void init(void)
#endif
#ifdef USE_ADC
/* these can be removed from features! */
adcConfigMutable()->vbat.enabled = feature(FEATURE_VBAT);
adcConfigMutable()->currentMeter.enabled = feature(FEATURE_CURRENT_METER);
adcConfigMutable()->vbat.enabled = (batteryConfig()->voltageMeterSource == VOLTAGE_METER_ADC);
adcConfigMutable()->currentMeter.enabled = (batteryConfig()->currentMeterSource == CURRENT_METER_ADC);
adcConfigMutable()->rssi.enabled = feature(FEATURE_RSSI_ADC);
adcInit(adcConfig());
#endif
@ -556,10 +556,7 @@ void init(void)
serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif
// Now that everything has powered up the voltage and cell count be determined.
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
batteryInit();
batteryInit(); // always needs doing, regardless of features.
#ifdef USE_DASHBOARD
if (feature(FEATURE_DASHBOARD)) {

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

@ -727,16 +727,18 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
#endif
break;
case MSP_ANALOG:
sbufWriteU8(dst, (uint8_t)constrain(getVbat(), 0, 255));
sbufWriteU16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
case MSP_ANALOG: {
sbufWriteU8(dst, (uint8_t)constrain(getBatteryVoltage(), 0, 255));
sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
sbufWriteU16(dst, rssi);
if(batteryConfig()->multiwiiCurrentMeterOutput) {
sbufWriteU16(dst, (uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
} else
sbufWriteU16(dst, (int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
break;
if(batteryConfig()->multiwiiCurrentMeterOutput) {
sbufWriteU16(dst, (uint16_t)constrain(getAmperage() * 10, 0, 0xFFFF)); // send current in 0.001 A steps. Negative range is truncated to zero
} else
sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send current in 0.01 A steps, range is -320A to 320A
break;
}
case MSP_ARMING_CONFIG:
sbufWriteU8(dst, armingConfig()->auto_disarm_delay);
sbufWriteU8(dst, armingConfig()->disarm_kill_switch);
@ -839,7 +841,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU16(dst, compassConfig()->mag_declination / 10);
sbufWriteU8(dst, batteryConfig()->vbatscale);
sbufWriteU8(dst, 0); // batteryConfig()->vbatscale
sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage);
sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage);
sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage);
@ -912,18 +914,40 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_VOLTAGE_METER_CONFIG:
sbufWriteU8(dst, batteryConfig()->vbatscale);
sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage);
sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage);
sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage);
sbufWriteU8(dst, batteryConfig()->batteryMeterType);
BUILD_BUG_ON(VOLTAGE_SENSOR_ADC_VBAT != 0);
sbufWriteU8(dst, MAX_VOLTAGE_SENSOR_ADC); // voltage meters in payload
for (int i = VOLTAGE_SENSOR_ADC_VBAT; i < MAX_VOLTAGE_SENSOR_ADC; i++) {
// note, by indicating a sensor type and a sub-frame length it's possible to configure any type of voltage meter, i.e. all sensors not built directly into the FC such as ESC/RX/SPort/SBus
sbufWriteU8(dst, VOLTAGE_SENSOR_TYPE_ADC_RESISTOR_DIVIDER); // indicate the type of sensor that the next part of the payload is for
sbufWriteU8(dst, 3); // ADC sensor sub-frame length
sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatscale);
sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivval);
sbufWriteU8(dst, voltageSensorADCConfig(i)->vbatresdivmultiplier);
}
// if we had any other voltage sensors, this is where we would output any needed configuration
break;
case MSP_CURRENT_METER_CONFIG:
sbufWriteU16(dst, batteryConfig()->currentMeterScale);
sbufWriteU16(dst, batteryConfig()->currentMeterOffset);
sbufWriteU8(dst, batteryConfig()->currentMeterType);
BUILD_BUG_ON(CURRENT_SENSOR_VIRTUAL != 0);
BUILD_BUG_ON(CURRENT_SENSOR_ADC != 1);
sbufWriteU8(dst, MAX_ADC_OR_VIRTUAL_CURRENT_METERS); // current meters in payload
for (int i = CURRENT_SENSOR_VIRTUAL; i < MAX_ADC_OR_VIRTUAL_CURRENT_METERS; i++) {
sbufWriteU8(dst, i); // indicate the type of sensor that the next part of the payload is for
sbufWriteU8(dst, 4); // ADC or Virtual sensor sub-frame length
sbufWriteU16(dst, currentMeterADCOrVirtualConfig(i)->scale);
sbufWriteU16(dst, currentMeterADCOrVirtualConfig(i)->offset);
}
// if we had any other current sensors, this is where we would output any needed configuration
break;
case MSP_BATTERY_CONFIG:
sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage);
sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage);
sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage);
sbufWriteU16(dst, batteryConfig()->batteryCapacity);
sbufWriteU8(dst, batteryConfig()->voltageMeterSource);
sbufWriteU8(dst, batteryConfig()->currentMeterSource);
break;
case MSP_MIXER:
@ -982,8 +1006,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU16(dst, boardAlignment()->pitchDegrees);
sbufWriteU16(dst, boardAlignment()->yawDegrees);
sbufWriteU16(dst, batteryConfig()->currentMeterScale);
sbufWriteU16(dst, batteryConfig()->currentMeterOffset);
sbufWriteU16(dst, 0); // was currentMeterScale, see MSP_CURRENT_METER_CONFIG
sbufWriteU16(dst, 0); //was currentMeterOffset, see MSP_CURRENT_METER_CONFIG
break;
case MSP_CF_SERIAL_CONFIG:
@ -1417,7 +1441,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
compassConfigMutable()->mag_declination = sbufReadU16(src) * 10;
batteryConfigMutable()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
sbufReadU8(src); // actual vbatscale as intended // was batteryConfigMutable()->vbatscale
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
@ -1756,21 +1780,48 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
boardAlignmentMutable()->yawDegrees = sbufReadU16(src);
break;
case MSP_SET_VOLTAGE_METER_CONFIG:
batteryConfigMutable()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
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
if (dataSize > 4) {
batteryConfigMutable()->batteryMeterType = sbufReadU8(src);
case MSP_SET_VOLTAGE_METER_CONFIG: {
int sensor = sbufReadU8(src);
if (sensor != VOLTAGE_METER_ADC) {
return -1;
}
break;
case MSP_SET_CURRENT_METER_CONFIG:
batteryConfigMutable()->currentMeterScale = sbufReadU16(src);
batteryConfigMutable()->currentMeterOffset = sbufReadU16(src);
batteryConfigMutable()->currentMeterType = sbufReadU8(src);
int index = sbufReadU8(src);
if (index >= MAX_VOLTAGE_SENSOR_ADC) {
return -1;
}
voltageSensorADCConfigMutable(index)->vbatscale = sbufReadU8(src);
voltageSensorADCConfigMutable(index)->vbatresdivval = sbufReadU8(src);
voltageSensorADCConfigMutable(index)->vbatresdivmultiplier = sbufReadU8(src);
break;
}
case MSP_SET_CURRENT_METER_CONFIG: {
int sensor = sbufReadU8(src);
if (sensor != CURRENT_SENSOR_VIRTUAL || sensor != CURRENT_SENSOR_ADC) {
return -1;
}
int index = sbufReadU8(src);
if (index >= MAX_ADC_OR_VIRTUAL_CURRENT_METERS) {
return -1;
}
currentMeterADCOrVirtualConfigMutable(index)->scale = sbufReadU16(src);
currentMeterADCOrVirtualConfigMutable(index)->offset = sbufReadU16(src);
break;
}
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()->batteryCapacity = sbufReadU16(src);
batteryConfigMutable()->voltageMeterSource = sbufReadU8(src);
batteryConfigMutable()->currentMeterSource = sbufReadU8(src);
break;
#ifndef USE_QUAD_MIXER_ONLY
@ -1849,8 +1900,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
boardAlignmentMutable()->pitchDegrees = sbufReadU16(src); // board_align_pitch
boardAlignmentMutable()->yawDegrees = sbufReadU16(src); // board_align_yaw
batteryConfigMutable()->currentMeterScale = sbufReadU16(src);
batteryConfigMutable()->currentMeterOffset = sbufReadU16(src);
sbufReadU16(src); // was currentMeterScale, see MSP_SET_CURRENT_METER_CONFIG
sbufReadU16(src); // was currentMeterOffset see MSP_SET_CURRENT_METER_CONFIG
break;
case MSP_SET_CF_SERIAL_CONFIG:

54
src/main/fc/fc_tasks.c
View file

@ -26,6 +26,7 @@
#include "common/axis.h"
#include "common/color.h"
#include "common/utils.h"
#include "common/filter.h"
#include "config/feature.h"
#include "config/config_profile.h"
@ -114,29 +115,44 @@ static void taskHandleSerial(timeUs_t currentTimeUs)
mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand);
}
static void taskUpdateBattery(timeUs_t currentTimeUs)
void taskBatterySensors(timeUs_t currentTimeUs)
{
#if defined(USE_ADC) || defined(USE_ESC_SENSOR)
if (feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) {
static uint32_t vbatLastServiced = 0;
static uint32_t vbatLastServiced = 0;
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) {
if (cmp32(currentTimeUs, vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTimeUs;
updateBattery();
batteryUpdateVoltage();
}
}
#endif
if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_SENSOR)) {
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
static uint32_t ibatLastServiced = 0;
const int32_t ibatTimeSinceLastServiced = cmp32(currentTimeUs, ibatLastServiced);
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTimeUs;
updateCurrentMeter(ibatTimeSinceLastServiced);
batteryUpdateCurrentMeter(ibatTimeSinceLastServiced, ARMING_FLAG(ARMED));
}
}
}
void taskBatteryAlerts(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
if (!ARMING_FLAG(ARMED)) {
// the battery *might* fall out in flight, but if that happens the FC will likely be off too unless the user has battery backup.
batteryUpdatePresence();
}
batteryUpdateStates();
batteryUpdateAlarms();
}
static void taskUpdateRxMain(timeUs_t currentTimeUs)
{
processRx(currentTimeUs);
@ -237,7 +253,13 @@ void fcTasksInit(void)
setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));
setTaskEnabled(TASK_SERIAL, true);
rescheduleTask(TASK_SERIAL, TASK_PERIOD_HZ(serialConfig()->serial_update_rate_hz));
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
bool useBatterySensors = batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE || batteryConfig()->currentMeterSource != CURRENT_METER_NONE;
setTaskEnabled(TASK_BATTERY_SENSORS, useBatterySensors);
bool useBatteryAlerts = batteryConfig()->useVBatAlerts || batteryConfig()->useConsumptionAlerts || feature(FEATURE_OSD);
setTaskEnabled(TASK_BATTERY_ALERTS, useBatterySensors && useBatteryAlerts);
setTaskEnabled(TASK_RX, true);
setTaskEnabled(TASK_DISPATCH, dispatchIsEnabled());
@ -366,13 +388,19 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_HIGH,
},
[TASK_BATTERY] = {
.taskName = "BATTERY",
.taskFunc = taskUpdateBattery,
.desiredPeriod = TASK_PERIOD_HZ(50), // 50 Hz
[TASK_BATTERY_ALERTS] = {
.taskName = "BATTERY_ALERTS",
.taskFunc = taskBatteryAlerts,
.desiredPeriod = TASK_PERIOD_HZ(5), // 5 Hz
.staticPriority = TASK_PRIORITY_MEDIUM,
},
[TASK_BATTERY_SENSORS] = {
.taskName = "BATTERY_SENSORS",
.taskFunc = taskBatterySensors,
.desiredPeriod = TASK_PERIOD_HZ(50), // 50 Hz
.staticPriority = TASK_PRIORITY_MEDIUM,
},
#ifdef BEEPER
[TASK_BEEPER] = {
.taskName = "BEEPER",

7
src/main/io/beeper.c
View file

@ -208,7 +208,12 @@ static const beeperTableEntry_t *currentBeeperEntry = NULL;
*/
void beeper(beeperMode_e mode)
{
if (mode == BEEPER_SILENCE || ((getBeeperOffMask() & (1 << (BEEPER_USB-1))) && (feature(FEATURE_VBAT) && (batteryCellCount == 0)))) {
if (
mode == BEEPER_SILENCE || (
(getBeeperOffMask() & (1 << (BEEPER_USB - 1)))
&& (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && (getBatteryCellCount() == 0))
)
) {
beeperSilence();
return;
}

14
src/main/io/dashboard.c
View file

@ -452,24 +452,26 @@ void showBatteryPage(void)
{
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
if (feature(FEATURE_VBAT)) {
tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", getVbat() / 10, getVbat() % 10, batteryCellCount);
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) {
tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", getBatteryVoltage() / 10, getBatteryVoltage() % 10, getBatteryCellCount());
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
uint8_t batteryPercentage = calculateBatteryPercentage();
uint8_t batteryPercentage = calculateBatteryPercentageRemaining();
i2c_OLED_set_line(rowIndex++);
drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, batteryPercentage);
}
if (feature(FEATURE_CURRENT_METER)) {
tfp_sprintf(lineBuffer, "Amps: %d.%2d mAh: %d", amperage / 100, amperage % 100, mAhDrawn);
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
int32_t amperage = getAmperage();
tfp_sprintf(lineBuffer, "Amps: %d.%2d mAh: %d", amperage / 100, amperage % 100, getMAhDrawn());
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
uint8_t capacityPercentage = calculateBatteryPercentage();
uint8_t capacityPercentage = calculateBatteryPercentageRemaining();
i2c_OLED_set_line(rowIndex++);
drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, capacityPercentage);
}

4
src/main/io/ledstrip.c
View file

@ -472,7 +472,7 @@ static void applyLedFixedLayers()
case LED_FUNCTION_BATTERY:
color = HSV(RED);
hOffset += scaleRange(calculateBatteryPercentage(), 0, 100, -30, 120);
hOffset += scaleRange(calculateBatteryPercentageRemaining(), 0, 100, -30, 120);
break;
case LED_FUNCTION_RSSI:
@ -522,7 +522,7 @@ static void applyLedWarningLayer(bool updateNow, timeUs_t *timer)
if (warningFlashCounter == 0) { // update when old flags was processed
warningFlags = 0;
if (feature(FEATURE_VBAT) && getBatteryState() != BATTERY_OK)
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryState() != BATTERY_OK)
warningFlags |= 1 << WARNING_LOW_BATTERY;
if (feature(FEATURE_FAILSAFE) && failsafeIsActive())
warningFlags |= 1 << WARNING_FAILSAFE;

37
src/main/io/osd.c
View file

@ -192,12 +192,13 @@ static void osdDrawSingleElement(uint8_t item)
case OSD_MAIN_BATT_VOLTAGE:
{
buff[0] = SYM_BATT_5;
sprintf(buff + 1, "%d.%1dV", getVbat() / 10, getVbat() % 10);
sprintf(buff + 1, "%d.%1dV", getBatteryVoltage() / 10, getBatteryVoltage() % 10);
break;
}
case OSD_CURRENT_DRAW:
{
int32_t amperage = getAmperage();
buff[0] = SYM_AMP;
sprintf(buff + 1, "%d.%02d", abs(amperage) / 100, abs(amperage) % 100);
break;
@ -206,7 +207,7 @@ static void osdDrawSingleElement(uint8_t item)
case OSD_MAH_DRAWN:
{
buff[0] = SYM_MAH;
sprintf(buff + 1, "%d", mAhDrawn);
sprintf(buff + 1, "%d", getMAhDrawn());
break;
}
@ -401,7 +402,7 @@ static void osdDrawSingleElement(uint8_t item)
case OSD_POWER:
{
sprintf(buff, "%dW", amperage * getVbat() / 1000);
sprintf(buff, "%dW", getAmperage() * getBatteryVoltage() / 1000);
break;
}
@ -415,16 +416,24 @@ static void osdDrawSingleElement(uint8_t item)
case OSD_MAIN_BATT_WARNING:
{
if (getVbat() > (batteryWarningVoltage - 1))
return;
switch(getBatteryState()) {
case BATTERY_WARNING:
sprintf(buff, "LOW BATTERY");
break;
sprintf(buff, "LOW VOLTAGE");
case BATTERY_CRITICAL:
sprintf(buff, " LAND NOW");
break;
default:
break;
}
break;
}
case OSD_AVG_CELL_VOLTAGE:
{
uint16_t cellV = getVbat() * 10 / batteryCellCount;
uint16_t cellV = getBatteryVoltage() * 10 / getBatteryCellCount();
buff[0] = SYM_BATT_5;
sprintf(buff + 1, "%d.%dV", cellV / 100, cellV % 100);
break;
@ -582,7 +591,7 @@ void osdUpdateAlarms(void)
else
CLR_BLINK(OSD_RSSI_VALUE);
if (getVbat() <= (batteryWarningVoltage - 1)) {
if (getBatteryState() != BATTERY_OK) {
SET_BLINK(OSD_MAIN_BATT_VOLTAGE);
SET_BLINK(OSD_MAIN_BATT_WARNING);
SET_BLINK(OSD_AVG_CELL_VOLTAGE);
@ -602,7 +611,7 @@ void osdUpdateAlarms(void)
else
CLR_BLINK(OSD_FLYTIME);
if (mAhDrawn >= osdConfig()->cap_alarm)
if (getMAhDrawn() >= osdConfig()->cap_alarm)
SET_BLINK(OSD_MAH_DRAWN);
else
CLR_BLINK(OSD_MAH_DRAWN);
@ -644,10 +653,10 @@ static void osdUpdateStats(void)
if (stats.max_speed < value)
stats.max_speed = value;
if (stats.min_voltage > getVbat())
stats.min_voltage = getVbat();
if (stats.min_voltage > getBatteryVoltage())
stats.min_voltage = getBatteryVoltage();
value = amperage / 100;
value = getAmperage() / 100;
if (stats.max_current < value)
stats.max_current = value;
@ -722,14 +731,14 @@ static void osdShowStats(void)
strcat(buff, "%");
displayWrite(osdDisplayPort, 22, top++, buff);
if (feature(FEATURE_CURRENT_METER)) {
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
displayWrite(osdDisplayPort, 2, top, "MAX CURRENT :");
itoa(stats.max_current, buff, 10);
strcat(buff, "A");
displayWrite(osdDisplayPort, 22, top++, buff);
displayWrite(osdDisplayPort, 2, top, "USED MAH :");
itoa(mAhDrawn, buff, 10);
itoa(getMAhDrawn(), buff, 10);
strcat(buff, "\x07");
displayWrite(osdDisplayPort, 22, top++, buff);
}

8
src/main/msp/msp_protocol.h
View file

@ -59,7 +59,7 @@
#define MSP_PROTOCOL_VERSION 0
#define API_VERSION_MAJOR 1 // increment when major changes are made
#define API_VERSION_MINOR 32 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
#define API_VERSION_MINOR 33 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
#define API_VERSION_LENGTH 2
@ -105,6 +105,9 @@
//
// MSP commands for Cleanflight original features
//
#define MSP_BATTERY_CONFIG 32
#define MSP_SET_BATTERY_CONFIG 33
#define MSP_MODE_RANGES 34 //out message Returns all mode ranges
#define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range
@ -253,6 +256,9 @@
#define MSP_RC_DEADBAND 125 //out message deadbands for yaw alt pitch roll
#define MSP_SENSOR_ALIGNMENT 126 //out message orientation of acc,gyro,mag
#define MSP_LED_STRIP_MODECOLOR 127 //out message Get LED strip mode_color settings
#define MSP_VOLTAGE_METERS 128 //out message Voltage (per meter)
#define MSP_CURRENT_METERS 129 //out message Amperage (per meter)
#define MSP_BATTERY_STATE 130 //out message Connected/Disconnected, Voltage, Current Used
#define MSP_SET_RAW_RC 200 //in message 8 rc chan
#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed

2
src/main/rx/crsf.h
View file

@ -81,4 +81,4 @@ void crsfRxSendTelemetryData(void);
struct rxConfig_s;
struct rxRuntimeConfig_s;
bool crsfRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
bool crsfRxIsActive(void);
bool crsfRxIsActive(void);

6
src/main/rx/jetiexbus.c
View file

@ -523,10 +523,10 @@ void handleJetiExBusTelemetry(void)
}
if((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (calcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) {
jetiExSensors[EX_VOLTAGE].value = getVbat();
jetiExSensors[EX_CURRENT].value = amperage;
jetiExSensors[EX_VOLTAGE].value = getBatteryVoltage();
jetiExSensors[EX_CURRENT].value = getAmperage();
jetiExSensors[EX_ALTITUDE].value = baro.BaroAlt;
jetiExSensors[EX_CAPACITY].value = mAhDrawn;
jetiExSensors[EX_CAPACITY].value = getMAhDrawn();
jetiExSensors[EX_FRAMES_LOST].value = framesLost;
jetiExSensors[EX_TIME_DIFF].value = timeDiff;

3
src/main/scheduler/scheduler.h
View file

@ -56,7 +56,8 @@ typedef enum {
TASK_RX,
TASK_SERIAL,
TASK_DISPATCH,
TASK_BATTERY,
TASK_BATTERY_SENSORS,
TASK_BATTERY_ALERTS,
#ifdef BEEPER
TASK_BEEPER,
#endif

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

@ -35,107 +35,100 @@
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "io/beeper.h"
#include "sensors/battery.h"
#include "sensors/esc_sensor.h"
/**
* terminology: meter vs sensors
*
* sensors are used to collect data.
* - e.g. voltage at an MCU ADC input pin, value from an ESC sensor.
* sensors require very specific configuration, such as resistor values.
* meters are used to process and expose data collected from sensors to the rest of the system
* - e.g. a meter exposes normalized, and often filtered, values from a sensor.
* meters require different or little configuration.
*
*/
#define VBAT_LPF_FREQ 0.4f
#define IBAT_LPF_FREQ 0.4f
static void batteryUpdateConsumptionState(void); // temporary forward reference
#define VBAT_STABLE_MAX_DELTA 2
#define ADCVREF 3300 // in mV
#define MAX_ESC_BATTERY_AGE 10
// Battery monitoring stuff
uint8_t batteryCellCount;
uint8_t batteryCellCount; // Note: this can be 0 when no battery is detected or when the battery voltage sensor is missing or disabled.
uint16_t batteryWarningVoltage;
uint16_t batteryCriticalVoltage;
//
static currentMeter_t currentMeter;
static voltageMeter_t voltageMeter;
uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered)
uint16_t vbatLatest = 0; // most recent unsmoothed value
int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A)
int32_t amperageLatest = 0; // most recent value
int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start
static batteryState_e vBatState;
static batteryState_e batteryState;
static batteryState_e voltageState;
static batteryState_e consumptionState;
PG_REGISTER_WITH_RESET_TEMPLATE(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
#ifndef CURRENT_METER_SCALE_DEFAULT
#define CURRENT_METER_SCALE_DEFAULT 400 // for Allegro ACS758LCB-100U (40mV/A)
#ifdef BOARD_HAS_CURRENT_SENSOR
#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC
#else
#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_VIRTUAL
#endif
#ifdef BOARD_HAS_VOLTAGE_DIVIDER
#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC
#else
#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_NONE
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 1);
PG_RESET_TEMPLATE(batteryConfig_t, batteryConfig,
.vbatscale = VBAT_SCALE_DEFAULT,
.vbatresdivval = VBAT_RESDIVVAL_DEFAULT,
.vbatresdivmultiplier = VBAT_RESDIVMULTIPLIER_DEFAULT,
// voltage
.vbatmaxcellvoltage = 43,
.vbatmincellvoltage = 33,
.vbatwarningcellvoltage = 35,
.vbathysteresis = 1,
.batteryMeterType = BATTERY_SENSOR_ADC,
.currentMeterOffset = 0,
.currentMeterScale = CURRENT_METER_SCALE_DEFAULT,
.batteryNotPresentLevel = 55, // VBAT below 5.5 V will be ignored
.voltageMeterSource = DEFAULT_VOLTAGE_METER_SOURCE,
// current
.batteryCapacity = 0,
.currentMeterType = CURRENT_SENSOR_ADC,
.batterynotpresentlevel = 55, // VBAT below 5.5 V will be igonored
.currentMeterSource = DEFAULT_VOLTAGE_METER_SOURCE,
// warnings / alerts
.useVBatAlerts = true,
.useConsumptionAlerts = false,
.consumptionWarningPercentage = 10
.consumptionWarningPercentage = 10,
.vbathysteresis = 1
);
static uint16_t batteryAdcToVoltage(uint16_t src)
void batteryUpdateVoltage(void)
{
// calculate battery voltage based on ADC reading
// result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V
return ((((uint32_t)src * batteryConfig()->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig()->vbatresdivval))/batteryConfig()->vbatresdivmultiplier);
}
switch(batteryConfig()->voltageMeterSource) {
#ifdef USE_ESC_SENSOR
case VOLTAGE_METER_ESC:
if (feature(FEATURE_ESC_SENSOR)) {
voltageMeterESCUpdate(&voltageMeter);
}
break;
#endif
case VOLTAGE_METER_ADC:
voltageMeterADCRefresh();
voltageMeterADCUpdate(&voltageMeter, ADC_BATTERY);
break;
static void updateBatteryVoltage(void)
{
static biquadFilter_t vBatFilter;
static bool vBatFilterIsInitialised;
if (!vBatFilterIsInitialised) {
biquadFilterInitLPF(&vBatFilter, VBAT_LPF_FREQ, 50000); //50HZ Update
vBatFilterIsInitialised = true;
}
#ifdef USE_ESC_SENSOR
if (feature(FEATURE_ESC_SENSOR) && batteryConfig()->batteryMeterType == BATTERY_SENSOR_ESC) {
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
vbatLatest = escData->dataAge <= MAX_ESC_BATTERY_AGE ? escData->voltage / 10 : 0;
if (debugMode == DEBUG_BATTERY) {
debug[0] = -1;
}
vbat = biquadFilterApply(&vBatFilter, vbatLatest);
}
else
#endif
{
uint16_t vBatSample = adcGetChannel(ADC_BATTERY);
if (debugMode == DEBUG_BATTERY) {
debug[0] = vBatSample;
}
vbat = batteryAdcToVoltage(biquadFilterApply(&vBatFilter, vBatSample));
vbatLatest = batteryAdcToVoltage(vBatSample);
default:
case VOLTAGE_METER_NONE:
voltageMeterReset(&voltageMeter);
break;
}
if (debugMode == DEBUG_BATTERY) {
debug[1] = vbat;
debug[0] = voltageMeter.unfiltered;
debug[1] = voltageMeter.filtered;
}
}
static void updateBatteryAlert(void)
static void updateBatteryBeeperAlert(void)
{
switch(getBatteryState()) {
case BATTERY_WARNING:
@ -152,75 +145,94 @@ static void updateBatteryAlert(void)
}
}
void updateBattery(void)
void batteryUpdatePresence(void)
{
uint16_t vBatPrevious = vbatLatest;
updateBatteryVoltage();
uint16_t vBatMeasured = vbatLatest;
static uint16_t previousVoltage = 0;
/* battery has just been connected*/
if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig()->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) {
/* Actual battery state is calculated below, this is really BATTERY_PRESENT */
vBatState = BATTERY_OK;
bool isVoltageStable = (
previousVoltage > 0
&& ABS(voltageMeter.filtered - previousVoltage) <= VBAT_STABLE_MAX_DELTA
);
unsigned cells = (vBatMeasured / batteryConfig()->vbatmaxcellvoltage) + 1;
if (
voltageState == BATTERY_NOT_PRESENT
&& voltageMeter.filtered > batteryConfig()->batteryNotPresentLevel
&& isVoltageStable
) {
/* battery has just been connected - calculate cells, warning voltages and reset state */
unsigned cells = (voltageMeter.filtered / batteryConfig()->vbatmaxcellvoltage) + 1;
if (cells > 8) {
// something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells)
cells = 8;
}
consumptionState = voltageState = BATTERY_OK;
batteryCellCount = cells;
batteryWarningVoltage = batteryCellCount * batteryConfig()->vbatwarningcellvoltage;
batteryCriticalVoltage = batteryCellCount * batteryConfig()->vbatmincellvoltage;
/* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig()->batterynotpresentlevel */
} else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig()->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) {
vBatState = BATTERY_NOT_PRESENT;
} else if (
voltageState != BATTERY_NOT_PRESENT
&& voltageMeter.filtered <= batteryConfig()->batteryNotPresentLevel
&& isVoltageStable
) {
/* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig()->batterynotpresentlevel */
consumptionState = voltageState = BATTERY_NOT_PRESENT;
batteryCellCount = 0;
batteryWarningVoltage = 0;
batteryCriticalVoltage = 0;
}
if (debugMode == DEBUG_BATTERY) {
debug[2] = vBatState;
debug[2] = voltageState;
debug[3] = batteryCellCount;
}
if (batteryConfig()->useVBatAlerts) {
switch(vBatState) {
case BATTERY_OK:
if (vbat <= (batteryWarningVoltage - batteryConfig()->vbathysteresis)) {
vBatState = BATTERY_WARNING;
}
previousVoltage = voltageMeter.filtered; // record the current value so we can detect voltage stabilisation next the presence needs updating.
}
break;
case BATTERY_WARNING:
if (vbat <= (batteryCriticalVoltage - batteryConfig()->vbathysteresis)) {
vBatState = BATTERY_CRITICAL;
} else if (vbat > batteryWarningVoltage) {
vBatState = BATTERY_OK;
}
static void batteryUpdateVoltageState(void)
{
// alerts are currently used by beeper, osd and other subsystems
switch(voltageState) {
case BATTERY_OK:
if (voltageMeter.filtered <= (batteryWarningVoltage - batteryConfig()->vbathysteresis)) {
voltageState = BATTERY_WARNING;
}
break;
break;
case BATTERY_CRITICAL:
if (vbat > batteryCriticalVoltage) {
vBatState = BATTERY_WARNING;
}
case BATTERY_WARNING:
if (voltageMeter.filtered <= (batteryCriticalVoltage - batteryConfig()->vbathysteresis)) {
voltageState = BATTERY_CRITICAL;
} else if (voltageMeter.filtered > batteryWarningVoltage) {
voltageState = BATTERY_OK;
}
break;
break;
case BATTERY_NOT_PRESENT:
break;
}
case BATTERY_CRITICAL:
if (voltageMeter.filtered > batteryCriticalVoltage) {
voltageState = BATTERY_WARNING;
}
break;
updateBatteryAlert();
default:
break;
}
}
void batteryUpdateStates(void)
{
batteryUpdateVoltageState();
batteryUpdateConsumptionState();
batteryState = MAX(voltageState, consumptionState);
}
batteryState_e getBatteryState(void)
{
batteryState_e batteryState = BATTERY_NOT_PRESENT;
if (vBatState != BATTERY_NOT_PRESENT) {
batteryState = MAX(vBatState, consumptionState);
}
return batteryState;
}
@ -233,140 +245,163 @@ const char * getBatteryStateString(void)
void batteryInit(void)
{
vBatState = BATTERY_NOT_PRESENT;
consumptionState = BATTERY_OK;
//
// presence
//
batteryState = BATTERY_NOT_PRESENT;
batteryCellCount = 0;
//
// voltage
//
voltageState = BATTERY_NOT_PRESENT;
batteryWarningVoltage = 0;
batteryCriticalVoltage = 0;
}
static int32_t currentSensorToCentiamps(uint16_t src)
{
int32_t millivolts;
voltageMeterReset(&voltageMeter);
switch(batteryConfig()->voltageMeterSource) {
case VOLTAGE_METER_ESC:
#ifdef USE_ESC_SENSOR
if (feature(FEATURE_ESC_SENSOR)) {
voltageMeterESCInit();
}
#endif
break;
millivolts = ((uint32_t)src * ADCVREF) / 4096;
millivolts -= batteryConfig()->currentMeterOffset;
case VOLTAGE_METER_ADC:
voltageMeterADCInit();
break;
return (millivolts * 1000) / (int32_t)batteryConfig()->currentMeterScale; // current in 0.01A steps
}
static void updateBatteryCurrent(void)
{
static biquadFilter_t iBatFilter;
static bool iBatFilterIsInitialised;
if (!iBatFilterIsInitialised) {
biquadFilterInitLPF(&iBatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update
iBatFilterIsInitialised = true;
default:
break;
}
//
// current
//
consumptionState = BATTERY_OK;
resetCurrentMeterState(&currentMeter);
switch(batteryConfig()->currentMeterSource) {
case CURRENT_METER_ADC:
currentMeterADCInit();
break;
default:
break;
}
uint16_t iBatSample = adcGetChannel(ADC_CURRENT);
amperageLatest = currentSensorToCentiamps(iBatSample);
amperage = currentSensorToCentiamps(biquadFilterApply(&iBatFilter, iBatSample));
}
static void updateCurrentDrawn(int32_t lastUpdateAt)
static void batteryUpdateConsumptionState(void)
{
static float mAhDrawnF = 0.0f; // used to get good enough resolution
if (batteryConfig()->useConsumptionAlerts && batteryConfig()->batteryCapacity > 0 && batteryCellCount > 0) {
uint8_t batteryPercentageRemaining = calculateBatteryPercentageRemaining();
mAhDrawnF = mAhDrawnF + (amperageLatest * lastUpdateAt / (100.0f * 1000 * 3600));
mAhDrawn = mAhDrawnF;
}
void updateConsumptionWarning(void)
{
if (batteryConfig()->useConsumptionAlerts && batteryConfig()->batteryCapacity > 0 && getBatteryState() != BATTERY_NOT_PRESENT) {
if (calculateBatteryPercentage() == 0) {
vBatState = BATTERY_CRITICAL;
} else if (calculateBatteryPercentage() <= batteryConfig()->consumptionWarningPercentage) {
if (batteryPercentageRemaining == 0) {
consumptionState = BATTERY_CRITICAL;
} else if (batteryPercentageRemaining <= batteryConfig()->consumptionWarningPercentage) {
consumptionState = BATTERY_WARNING;
} else {
consumptionState = BATTERY_OK;
}
updateBatteryAlert();
}
}
void updateCurrentMeter(int32_t lastUpdateAt)
void batteryUpdateCurrentMeter(int32_t lastUpdateAt, bool armed)
{
if (getBatteryState() != BATTERY_NOT_PRESENT) {
switch(batteryConfig()->currentMeterType) {
case CURRENT_SENSOR_ADC:
updateBatteryCurrent();
updateCurrentDrawn(lastUpdateAt);
updateConsumptionWarning();
break;
case CURRENT_SENSOR_VIRTUAL:
amperageLatest = (int32_t)batteryConfig()->currentMeterOffset;
if (ARMING_FLAG(ARMED)) {
const throttleStatus_e throttleStatus = calculateThrottleStatus();
int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) {
throttleOffset = 0;
}
int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
amperageLatest += throttleFactor * (int32_t)batteryConfig()->currentMeterScale / 1000;
}
amperage = amperageLatest;
updateCurrentDrawn(lastUpdateAt);
updateConsumptionWarning();
break;
case CURRENT_SENSOR_ESC:
#ifdef USE_ESC_SENSOR
if (feature(FEATURE_ESC_SENSOR)) {
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
if (escData->dataAge <= MAX_ESC_BATTERY_AGE) {
amperageLatest = escData->current;
mAhDrawn = escData->consumption;
} else {
amperageLatest = 0;
mAhDrawn = 0;
}
amperage = amperageLatest;
if (batteryCellCount == 0) {
resetCurrentMeterState(&currentMeter);
return;
}
updateConsumptionWarning();
}
switch(batteryConfig()->currentMeterSource) {
case CURRENT_METER_ADC:
currentUpdateADCMeter(&currentMeter, lastUpdateAt);
break;
break;
#endif
case CURRENT_SENSOR_NONE:
amperage = 0;
amperageLatest = 0;
case CURRENT_METER_VIRTUAL: {
throttleStatus_e throttleStatus = calculateThrottleStatus();
bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP));
int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
break;
currentUpdateVirtualMeter(&currentMeter, lastUpdateAt, armed, throttleLowAndMotorStop, throttleOffset);
break;
}
} else {
amperage = 0;
amperageLatest = 0;
case CURRENT_METER_ESC:
#ifdef USE_ESC_SENSOR
if (feature(FEATURE_ESC_SENSOR)) {
currentUpdateESCMeter(&currentMeter, lastUpdateAt);
}
#endif
break;
default:
// case CURRENT_METER_NONE:
// resetCurrentMeterState(&currentMeter);
break;
}
}
float calculateVbatPidCompensation(void) {
float batteryScaler = 1.0f;
if (feature(FEATURE_VBAT) && batteryCellCount > 0) {
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && batteryCellCount > 0) {
// Up to 33% PID gain. Should be fine for 4,2to 3,3 difference
batteryScaler = constrainf((( (float)batteryConfig()->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.33f);
batteryScaler = constrainf((( (float)batteryConfig()->vbatmaxcellvoltage * batteryCellCount ) / (float) voltageMeter.filtered), 1.0f, 1.33f);
}
return batteryScaler;
}
uint8_t calculateBatteryPercentage(void)
uint8_t calculateBatteryPercentageRemaining(void)
{
uint8_t batteryPercentage = 0;
if (batteryCellCount > 0) {
uint16_t batteryCapacity = batteryConfig()->batteryCapacity;
if (batteryCapacity > 0) {
batteryPercentage = constrain(((float)batteryCapacity - mAhDrawn) * 100 / batteryCapacity, 0, 100);
batteryPercentage = constrain(((float)batteryCapacity - currentMeter.mAhDrawn) * 100 / batteryCapacity, 0, 100);
} else {
batteryPercentage = constrain((((uint32_t)vbat - (batteryConfig()->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig()->vbatmaxcellvoltage - batteryConfig()->vbatmincellvoltage) * batteryCellCount), 0, 100);
batteryPercentage = constrain((((uint32_t)voltageMeter.filtered - (batteryConfig()->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig()->vbatmaxcellvoltage - batteryConfig()->vbatmincellvoltage) * batteryCellCount), 0, 100);
}
}
return batteryPercentage;
}
uint16_t getVbat(void)
void batteryUpdateAlarms(void)
{
return vbat;
// use the state to trigger beeper alerts
if (batteryConfig()->useVBatAlerts) {
updateBatteryBeeperAlert();
}
}
uint16_t getBatteryVoltage(void)
{
return voltageMeter.filtered;
}
uint16_t getBatteryVoltageLatest(void)
{
return voltageMeter.unfiltered;
}
uint8_t getBatteryCellCount(void)
{
return batteryCellCount;
}
int32_t getAmperage(void) {
return currentMeter.amperage;
}
int32_t getAmperageLatest(void)
{
return currentMeter.amperageLatest;
}
int32_t getMAhDrawn(void)
{
return currentMeter.mAhDrawn;
}

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

@ -19,48 +19,37 @@
#include "config/parameter_group.h"
#ifndef VBAT_SCALE_DEFAULT
#define VBAT_SCALE_DEFAULT 110
#endif
#include "common/filter.h"
#include "sensors/current.h"
#include "sensors/voltage.h"
#define VBAT_RESDIVVAL_DEFAULT 10
#define VBAT_RESDIVMULTIPLIER_DEFAULT 1
#define VBAT_SCALE_MIN 0
#define VBAT_SCALE_MAX 255
typedef enum {
CURRENT_SENSOR_NONE = 0,
CURRENT_SENSOR_ADC,
CURRENT_SENSOR_VIRTUAL,
CURRENT_SENSOR_ESC,
CURRENT_SENSOR_MAX = CURRENT_SENSOR_ESC
} currentSensor_e;
typedef enum {
BATTERY_SENSOR_ADC = 0,
BATTERY_SENSOR_ESC
} batterySensor_e;
typedef struct batteryConfig_s {
uint8_t vbatscale; // adjust this to match battery voltage to reported value
uint8_t vbatresdivval; // resistor divider R2 (default NAZE 10(K))
uint8_t vbatresdivmultiplier; // multiplier for scale (e.g. 2.5:1 ratio with multiplier of 4 can use '100' instead of '25' in ratio) to get better precision
// 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 vbathysteresis; // hysteresis for alarm, default 1 = 0.1V
batterySensor_e batteryMeterType; // type of battery meter uses, either ADC or ESC
uint8_t batteryNotPresentLevel; // Below this level battery is considered as not present
int16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
int16_t currentMeterOffset; // offset of the current sensor in millivolt steps
currentSensor_e currentMeterType; // type of current meter used, either ADC, Virtual or ESC
voltageMeterSource_e voltageMeterSource; // source of battery voltage meter used, either ADC or ESC
// FIXME this doesn't belong in here since it's a concern of MSP, not of the battery code.
uint8_t multiwiiCurrentMeterOutput; // if set to 1 output the amperage in milliamp steps instead of 0.01A steps via msp
// current
uint16_t batteryCapacity; // mAh
uint8_t batterynotpresentlevel; // Below this level battery is considered as not present
currentMeterSource_e currentMeterSource; // source of battery current meter used, either ADC, Virtual or ESC
// warnings / alerts
bool useVBatAlerts; // Issue alerts based on VBat readings
bool useConsumptionAlerts; // Issue alerts based on total power consumption
uint8_t consumptionWarningPercentage; // Percentage of remaining capacity that should trigger a battery warning
uint8_t vbathysteresis; // hysteresis for alarm, default 1 = 0.1V
// FIXME this doesn't belong in here since it's a concern of MSP, not of the battery code.
uint8_t multiwiiCurrentMeterOutput; // if set to 1 output the amperage in milliamp steps instead of 0.01A steps via msp
} batteryConfig_t;
PG_DECLARE(batteryConfig_t, batteryConfig);
@ -72,23 +61,26 @@ typedef enum {
BATTERY_NOT_PRESENT
} batteryState_e;
extern uint16_t vbatRaw;
extern uint16_t vbatLatest;
extern uint8_t batteryCellCount;
extern uint16_t batteryWarningVoltage;
extern int32_t amperageLatest;
extern int32_t amperage;
extern int32_t mAhDrawn;
void batteryInit(void);
void batteryUpdateVoltage(void);
void batteryUpdatePresence(void);
batteryState_e getBatteryState(void);
const char * getBatteryStateString(void);
void updateBattery(void);
void batteryInit(void);
void batteryUpdateStates(void);
void batteryUpdateAlarms(void);
struct rxConfig_s;
void updateCurrentMeter(int32_t lastUpdateAt);
int32_t currentMeterToCentiamps(uint16_t src);
float calculateVbatPidCompensation(void);
uint8_t calculateBatteryPercentage(void);
uint16_t getVbat(void);
uint8_t calculateBatteryPercentageRemaining(void);
uint16_t getBatteryVoltage(void);
uint16_t getBatteryVoltageLatest(void);
uint8_t getBatteryCellCount(void);
int32_t getAmperage(void);
int32_t getAmperageLatest(void);
int32_t getMAhDrawn(void);
void batteryUpdateCurrentMeter(int32_t lastUpdateAt, bool armed);

129
src/main/sensors/current.c Normal file
View file

@ -0,0 +1,129 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it 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.
*
* Cleanflight 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.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include "stdbool.h"
#include "stdint.h"
#include "string.h"
#include <platform.h>
#include "build/build_config.h"
#include "common/maths.h"
#include "common/utils.h"
#include "common/filter.h"
#include "drivers/adc.h"
#include "drivers/system.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "config/config_reset.h"
#include "sensors/current.h"
#include "sensors/esc_sensor.h"
#define ADCVREF 3300 // in mV
#define IBAT_LPF_FREQ 0.4f
static biquadFilter_t adciBatFilter;
#ifndef CURRENT_METER_SCALE_DEFAULT
#define CURRENT_METER_SCALE_DEFAULT 400 // for Allegro ACS758LCB-100U (40mV/A)
#endif
PG_REGISTER_ARRAY_WITH_RESET_FN(currentMeterADCOrVirtualConfig_t, MAX_ADC_OR_VIRTUAL_CURRENT_METERS, currentMeterADCOrVirtualConfig, PG_CURRENT_SENSOR_ADC_OR_VIRTUAL_CONFIG, 0);
void pgResetFn_currentMeterADCOrVirtualConfig(currentMeterADCOrVirtualConfig_t *instance)
{
for (int i = 0; i < MAX_ADC_OR_VIRTUAL_CURRENT_METERS; i++) {
if (i == CURRENT_METER_ADC) {
RESET_CONFIG(currentMeterADCOrVirtualConfig_t, &instance[i],
.scale = CURRENT_METER_SCALE_DEFAULT,
);
}
}
}
static int32_t currentMeterADCToCentiamps(const uint16_t src)
{
int32_t millivolts;
const currentMeterADCOrVirtualConfig_t *config = currentMeterADCOrVirtualConfig(CURRENT_SENSOR_ADC);
millivolts = ((uint32_t)src * ADCVREF) / 4096;
millivolts -= config->offset;
return (millivolts * 1000) / (int32_t)config->scale; // current in 0.01A steps
}
void updateCurrentDrawn(currentMeter_t *state, int32_t lastUpdateAt)
{
state->mAhDrawnF = state->mAhDrawnF + (state->amperageLatest * lastUpdateAt / (100.0f * 1000 * 3600));
state->mAhDrawn = state->mAhDrawnF;
}
void currentUpdateADCMeter(currentMeter_t *state, int32_t lastUpdateAt)
{
uint16_t iBatSample = adcGetChannel(ADC_CURRENT);
state->amperageLatest = currentMeterADCToCentiamps(iBatSample);
state->amperage = currentMeterADCToCentiamps(biquadFilterApply(&adciBatFilter, iBatSample));
updateCurrentDrawn(state, lastUpdateAt);
}
void currentUpdateVirtualMeter(currentMeter_t *state, int32_t lastUpdateAt, bool armed, bool throttleLowAndMotorStop, int32_t throttleOffset)
{
state->amperage = (int32_t)currentMeterADCOrVirtualConfig(CURRENT_SENSOR_VIRTUAL)->offset;
if (armed) {
if (throttleLowAndMotorStop) {
throttleOffset = 0;
}
int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); // FIXME magic number 50, 50hz?
state->amperageLatest = state->amperage += throttleFactor * (int32_t)currentMeterADCOrVirtualConfig(CURRENT_SENSOR_VIRTUAL)->scale / 1000;
}
updateCurrentDrawn(state, lastUpdateAt);
}
void currentUpdateESCMeter(currentMeter_t *state, int32_t lastUpdateAt)
{
UNUSED(lastUpdateAt);
#ifndef USE_ESC_SENSOR
UNUSED(state);
#else
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
if (escData->dataAge <= ESC_BATTERY_AGE_MAX) {
state->amperageLatest = escData->current;
state->mAhDrawn = escData->consumption;
} else {
state->amperageLatest = 0;
state->mAhDrawn = 0;
}
state->amperage = state->amperageLatest;
#endif
}
void resetCurrentMeterState(currentMeter_t *state)
{
state->amperage = 0;
state->amperageLatest = 0;
}
void currentMeterADCInit(void)
{
biquadFilterInitLPF(&adciBatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update
}

59
src/main/sensors/current.h Normal file
View file

@ -0,0 +1,59 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it 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.
*
* Cleanflight 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.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
typedef enum {
CURRENT_METER_NONE = 0,
CURRENT_METER_ADC,
CURRENT_METER_VIRTUAL,
CURRENT_METER_ESC,
CURRENT_METER_MAX = CURRENT_METER_ESC
} currentMeterSource_e;
typedef struct currentMeter_s {
int32_t amperage; // current read by current sensor in centiampere (1/100th A)
int32_t amperageLatest; // current read by current sensor in centiampere (1/100th A) (unfiltered)
int32_t mAhDrawn; // milliampere hours drawn from the battery since start
float mAhDrawnF;
} currentMeter_t;
// NOTE: currentMeterConfig is only used by physical and virtual current meters, not ESC based current meters.
#define MAX_ADC_OR_VIRTUAL_CURRENT_METERS 2 // 1x ADC, 1x Virtual
typedef enum {
CURRENT_SENSOR_VIRTUAL = 0, // virtual is FIRST because it should be possible to build without ADC current sensors.
CURRENT_SENSOR_ADC,
} currentSensor_e;
// WARNING - do not mix usage of CURRENT_SENSOR_* and CURRENT_METER_*, they are separate concerns.
typedef struct currentMeterADCOrVirtualConfig_s {
int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
uint16_t offset; // offset of the current sensor in millivolt steps
} currentMeterADCOrVirtualConfig_t;
PG_DECLARE_ARRAY(currentMeterADCOrVirtualConfig_t, MAX_ADC_OR_VIRTUAL_CURRENT_METERS, currentMeterADCOrVirtualConfig);
void currentMeterADCInit(void);
void currentUpdateADCMeter(currentMeter_t *state, int32_t lastUpdateAt);
void currentUpdateESCMeter(currentMeter_t *state, int32_t lastUpdateAt);
void currentUpdateVirtualMeter(currentMeter_t *state, int32_t lastUpdateAt, bool armed, bool throttleLowAndMotorStop, int32_t throttleOffset);
void resetCurrentMeterState(currentMeter_t *state);

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

@ -43,8 +43,6 @@
#include "flight/mixer.h"
#include "sensors/battery.h"
#include "io/serial.h"
/*

2
src/main/sensors/esc_sensor.h
View file

@ -30,6 +30,8 @@ typedef struct {
#define ESC_DATA_INVALID 255
#define ESC_BATTERY_AGE_MAX 10
bool escSensorInit(void);
void escSensorProcess(timeUs_t currentTime);

151
src/main/sensors/voltage.c Normal file
View file

@ -0,0 +1,151 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it 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.
*
* Cleanflight 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.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include "stdbool.h"
#include "stdint.h"
#include "string.h"
#include <platform.h>
#include "build/build_config.h"
#include "common/maths.h"
#include "common/filter.h"
#include "common/utils.h"
#include "drivers/adc.h"
#include "drivers/system.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "config/config_reset.h"
#include "sensors/voltage.h"
#include "sensors/esc_sensor.h"
#ifndef VBAT_SCALE_DEFAULT
#define VBAT_SCALE_DEFAULT 110
#endif
#ifdef USE_ESC_SENSOR
static biquadFilter_t escvBatFilter;
#endif
voltageMeterADCState_t voltageMeterADCStates[MAX_VOLTAGE_SENSOR_ADC];
voltageMeterADCState_t *getVoltageMeterADC(uint8_t index)
{
return &voltageMeterADCStates[index];
}
PG_REGISTER_ARRAY_WITH_RESET_FN(voltageSensorADCConfig_t, MAX_VOLTAGE_SENSOR_ADC, voltageSensorADCConfig, PG_VOLTAGE_SENSOR_ADC_CONFIG, 0);
void pgResetFn_voltageSensorADCConfig(voltageSensorADCConfig_t *instance)
{
for (int i = 0; i < MAX_VOLTAGE_SENSOR_ADC; i++) {
RESET_CONFIG(voltageSensorADCConfig_t, &instance[i],
.vbatscale = VBAT_SCALE_DEFAULT,
.vbatresdivval = VBAT_RESDIVVAL_DEFAULT,
.vbatresdivmultiplier = VBAT_RESDIVMULTIPLIER_DEFAULT,
);
}
}
static const uint8_t voltageMeterAdcChannelMap[] = {
ADC_BATTERY,
#ifdef ADC_POWER_12V
ADC_POWER_12V,
#endif
#ifdef ADC_POWER_5V
ADC_POWER_5V,
#endif
};
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 * 33 + (0xFFF * 5)) / (0xFFF * config->vbatresdivval)) / config->vbatresdivmultiplier);
}
void voltageMeterADCRefresh(void)
{
for (uint8_t i = 0; i < MAX_VOLTAGE_SENSOR_ADC && i < ARRAYLEN(voltageMeterAdcChannelMap); i++) {
// store the battery voltage with some other recent battery voltage readings
voltageMeterADCState_t *state = &voltageMeterADCStates[i];
const voltageSensorADCConfig_t *config = voltageSensorADCConfig(i);
uint8_t channel = voltageMeterAdcChannelMap[i];
uint16_t rawSample = adcGetChannel(channel);
uint16_t filteredSample = biquadFilterApply(&state->vbatFilterState, rawSample);
// always calculate the latest voltage, see getLatestVoltage() which does the calculation on demand.
state->voltageFiltered = voltageAdcToVoltage(filteredSample, config);
state->voltageUnfiltered = voltageAdcToVoltage(rawSample, config);
}
}
void voltageMeterADCUpdate(voltageMeter_t *voltageMeter, voltageSensorADC_e adcChannel)
{
voltageMeterADCState_t *state = &voltageMeterADCStates[adcChannel];
voltageMeter->filtered = state->voltageFiltered;
voltageMeter->unfiltered = state->voltageUnfiltered;
}
void voltageMeterADCInit(void)
{
for (uint8_t i = 0; i < MAX_VOLTAGE_SENSOR_ADC && i < ARRAYLEN(voltageMeterAdcChannelMap); i++) {
// store the battery voltage with some other recent battery voltage readings
voltageMeterADCState_t *state = &voltageMeterADCStates[i];
memset(state, 0, sizeof(voltageMeterADCState_t));
biquadFilterInitLPF(&state->vbatFilterState, VBATT_LPF_FREQ, 50000);
}
}
void voltageMeterReset(voltageMeter_t *meter)
{
meter->filtered = 0;
meter->unfiltered = 0;
}
#define VBAT_LPF_FREQ 0.4f
void voltageMeterESCInit(void)
{
#ifdef USE_ESC_SENSOR
biquadFilterInitLPF(&escvBatFilter, VBAT_LPF_FREQ, 50000); //50HZ Update
#endif
}
void voltageMeterESCUpdate(voltageMeter_t *voltageMeter)
{
#ifndef USE_ESC_SENSOR
UNUSED(voltageMeter);
#else
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
voltageMeter->unfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage / 10 : 0;
voltageMeter->filtered = biquadFilterApply(&escvBatFilter, voltageMeter->unfiltered);
#endif
}

91
src/main/sensors/voltage.h Normal file
View file

@ -0,0 +1,91 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it 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.
*
* Cleanflight 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.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
//
// meters
//
typedef enum {
VOLTAGE_METER_NONE = 0,
VOLTAGE_METER_ADC,
VOLTAGE_METER_ESC
} voltageMeterSource_e;
typedef struct voltageMeter_s {
uint16_t filtered; // voltage in 0.1V steps
uint16_t unfiltered; // voltage in 0.1V steps
} voltageMeter_t;
//
// sensors
//
typedef enum {
VOLTAGE_SENSOR_TYPE_ADC_RESISTOR_DIVIDER = 0,
VOLTAGE_SENSOR_TYPE_ESC
} voltageSensorType_e;
//
// adc sensors
//
#ifndef MAX_VOLTAGE_SENSOR_ADC
#define MAX_VOLTAGE_SENSOR_ADC 1 // VBAT - some boards have external, 12V and 5V meters.
#endif
typedef enum {
VOLTAGE_SENSOR_ADC_VBAT = 0,
VOLTAGE_SENSOR_ADC_5V = 1,
VOLTAGE_SENSOR_ADC_12V = 2
} voltageSensorADC_e;
// WARNING - do not mix usage of VOLTAGE_METER_* and VOLTAGE_METER_*, they are separate concerns.
#define VBAT_RESDIVVAL_DEFAULT 10
#define VBAT_RESDIVMULTIPLIER_DEFAULT 1
#define VBAT_SCALE_MIN 0
#define VBAT_SCALE_MAX 255
#define VBATT_LPF_FREQ 1.0f
typedef struct voltageSensorADCConfig_s {
uint8_t vbatscale; // adjust this to match battery voltage to reported value
uint8_t vbatresdivval; // resistor divider R2 (default NAZE 10(K))
uint8_t vbatresdivmultiplier; // multiplier for scale (e.g. 2.5:1 ratio with multiplier of 4 can use '100' instead of '25' in ratio) to get better precision
} voltageSensorADCConfig_t;
PG_DECLARE_ARRAY(voltageSensorADCConfig_t, MAX_VOLTAGE_SENSOR_ADC, voltageSensorADCConfig);
typedef struct voltageMeterADCState_s {
uint16_t voltageFiltered; // battery voltage in 0.1V steps (filtered)
uint16_t voltageUnfiltered; // battery voltage in 0.1V steps (unfiltered)
biquadFilter_t vbatFilterState;
} voltageMeterADCState_t; // TODO rename to voltageMeter_t
extern voltageMeterADCState_t voltageMeterADCStates[MAX_VOLTAGE_SENSOR_ADC];
void voltageMeterADCInit(void);
void voltageMeterESCInit(void);
void voltageMeterADCRefresh(void);
void voltageMeterReset(voltageMeter_t *voltageMeter);
void voltageMeterADCUpdate(voltageMeter_t *voltageMeter, voltageSensorADC_e adcChannel);
void voltageMeterESCUpdate(voltageMeter_t *voltageMeter);

1
src/main/target/AIRHEROF3/target.h
View file

@ -83,7 +83,6 @@
#define ADC_INSTANCE ADC2
#define VBAT_ADC_PIN PA4
#define DEFAULT_FEATURES FEATURE_VBAT
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART2

8
src/main/target/ALIENFLIGHTF4/config.c
View file

@ -56,8 +56,8 @@
// alternative defaults settings for AlienFlight targets
void targetConfiguration(void)
{
batteryConfigMutable()->currentMeterOffset = CURRENTOFFSET;
batteryConfigMutable()->currentMeterScale = CURRENTSCALE;
currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->offset = CURRENTOFFSET;
currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->scale = CURRENTSCALE;
compassConfigMutable()->mag_hardware = MAG_NONE; // disabled by default
if (hardwareMotorType == MOTOR_BRUSHED) {
@ -74,7 +74,9 @@ void targetConfiguration(void)
rxConfigMutable()->sbus_inversion = 0;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY;
telemetryConfigMutable()->telemetry_inversion = 0;
featureSet(FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY);
batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_ADC;
batteryConfigMutable()->currentMeterSource = CURRENT_METER_ADC;
featureSet(FEATURE_TELEMETRY);
}
pidProfilesMutable(0)->P8[FD_ROLL] = 53;

8
src/main/target/ALIENFLIGHTNGF7/config.c
View file

@ -55,8 +55,8 @@
// alternative defaults settings for AlienFlight targets
void targetConfiguration(void)
{
batteryConfigMutable()->currentMeterOffset = CURRENTOFFSET;
batteryConfigMutable()->currentMeterScale = CURRENTSCALE;
currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->offset = CURRENTOFFSET;
currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->scale = CURRENTSCALE;
compassConfigMutable()->mag_hardware = MAG_NONE; // disabled by default
if (hardwareMotorType == MOTOR_BRUSHED) {
@ -73,7 +73,9 @@ void targetConfiguration(void)
rxConfigMutable()->sbus_inversion = 0;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY;
telemetryConfigMutable()->telemetry_inversion = 0;
featureSet(FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY);
batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_ADC;
batteryConfigMutable()->currentMeterSource = CURRENT_METER_ADC;
featureSet(FEATURE_TELEMETRY);
}
pidProfilesMutable(0)->P8[FD_ROLL] = 53;

3
src/main/target/BETAFLIGHTF3/config.c
View file

@ -25,8 +25,9 @@
#include "fc/config.h"
#include "sensors/battery.h"
#define CURRENTSCALE 220
void targetConfiguration(void)
{
batteryConfigMutable()->currentMeterScale = 220;
currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->scale = CURRENTSCALE;
}
#endif

3
src/main/target/BETAFLIGHTF3/target.h
View file

@ -112,6 +112,7 @@
#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5
#define BOARD_HAS_VOLTAGE_DIVIDER
#define BOARD_HAS_CURRENT_SENSOR
#define USE_ADC
#define ADC_INSTANCE ADC2
#define VBAT_ADC_PIN PA4
@ -124,7 +125,7 @@
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART2
#define SBUS_TELEMETRY_UART SERIAL_PORT_USART1
#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_OSD)
#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_TELEMETRY | FEATURE_OSD)
#define SPEKTRUM_BIND_PIN UART2_RX_PIN

111
src/main/target/COLIBRI_RACE/i2c_bst.c
View file

@ -78,7 +78,7 @@
#define BST_PROTOCOL_VERSION 0
#define API_VERSION_MAJOR 1 // increment when major changes are made
#define API_VERSION_MINOR 12 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_MINOR 13 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_LENGTH 2
@ -120,6 +120,9 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
//
// MSP commands for Cleanflight original features
//
#define BST_BATTERY_CONFIG 32
#define BST_SET_BATTERY_CONFIG 33
#define BST_MODE_RANGES 34 //out message Returns all mode ranges
#define BST_SET_MODE_RANGE 35 //in message Sets a single mode range
@ -674,13 +677,14 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
#endif
break;
case BST_ANALOG:
bstWrite8((uint8_t)constrain(getVbat(), 0, 255));
bstWrite16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
bstWrite8((uint8_t)constrain(getBatteryVoltage(), 0, 255));
bstWrite16((uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
bstWrite16(rssi);
// FIXME - what does the TBS OSD actually need? the 'multiwiiCurrentMeterOutput' setting pre-dates the TBS i2c_bst code so likely we can just output exactly what we need.
if(batteryConfig()->multiwiiCurrentMeterOutput) {
bstWrite16((uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
bstWrite16((uint16_t)constrain(getAmperage() * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
} else
bstWrite16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
bstWrite16((int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
break;
case BST_ARMING_CONFIG:
bstWrite8(armingConfig()->auto_disarm_delay);
@ -773,7 +777,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
bstWrite16(compassConfig()->mag_declination / 10);
bstWrite8(batteryConfig()->vbatscale);
bstWrite8(voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale);
bstWrite8(batteryConfig()->vbatmincellvoltage);
bstWrite8(batteryConfig()->vbatmaxcellvoltage);
bstWrite8(batteryConfig()->vbatwarningcellvoltage);
@ -855,18 +859,40 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
bstWrite16(boardAlignment()->yawDegrees);
break;
case BST_VOLTAGE_METER_CONFIG:
bstWrite8(batteryConfig()->vbatscale);
bstWrite8(batteryConfig()->vbatmincellvoltage);
bstWrite8(batteryConfig()->vbatmaxcellvoltage);
bstWrite8(batteryConfig()->vbatwarningcellvoltage);
BUILD_BUG_ON(VOLTAGE_SENSOR_ADC_VBAT != 0);
for (int i = VOLTAGE_SENSOR_ADC_VBAT; i < MAX_VOLTAGE_SENSOR_ADC; i++) {
// note, by indicating a sensor type and a sub-frame length it's possible to configure any type of voltage meter, i.e. all sensors not built directly into the FC such as ESC/RX/SPort/SBus
bstWrite8(VOLTAGE_METER_ADC); // indicate the type of sensor that the next part of the payload is for
bstWrite8(3); // ADC sensor sub-frame length
bstWrite8(voltageSensorADCConfig(i)->vbatscale);
bstWrite8(voltageSensorADCConfig(i)->vbatresdivval);
bstWrite8(voltageSensorADCConfig(i)->vbatresdivmultiplier);
}
// if we had any other voltage sensors, this is where we would output any needed configuration
break;
case BST_CURRENT_METER_CONFIG:
bstWrite16(batteryConfig()->currentMeterScale);
bstWrite16(batteryConfig()->currentMeterOffset);
bstWrite8(batteryConfig()->currentMeterType);
BUILD_BUG_ON(CURRENT_SENSOR_VIRTUAL != 0);
BUILD_BUG_ON(CURRENT_SENSOR_ADC != 1);
for (int i = CURRENT_SENSOR_VIRTUAL; i < MAX_ADC_OR_VIRTUAL_CURRENT_METERS; i++) {
bstWrite8(i); // indicate the type of sensor that the next part of the payload is for
bstWrite8(4); // ADC or Virtual sensor sub-frame length
bstWrite16(currentMeterADCOrVirtualConfig(i)->scale);
bstWrite16(currentMeterADCOrVirtualConfig(i)->offset);
}
// if we had any other voltage sensors, this is where we would output any needed configuration
break;
case BST_BATTERY_CONFIG:
bstWrite8(batteryConfig()->vbatmincellvoltage);
bstWrite8(batteryConfig()->vbatmaxcellvoltage);
bstWrite8(batteryConfig()->vbatwarningcellvoltage);
bstWrite16(batteryConfig()->batteryCapacity);
bstWrite8(batteryConfig()->voltageMeterSource);
bstWrite8(batteryConfig()->currentMeterSource);
break;
case BST_MIXER:
@ -916,8 +942,8 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
bstWrite16(boardAlignment()->pitchDegrees);
bstWrite16(boardAlignment()->yawDegrees);
bstWrite16(batteryConfig()->currentMeterScale);
bstWrite16(batteryConfig()->currentMeterOffset);
bstWrite16(0); // was batteryConfig()->currentMeterScale);
bstWrite16(0); // was batteryConfig()->currentMeterOffset);
break;
case BST_CF_SERIAL_CONFIG:
@ -1134,7 +1160,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
compassConfigMutable()->mag_declination = bstRead16() * 10;
batteryConfigMutable()->vbatscale = bstRead8(); // actual vbatscale as intended
bstRead8(); // was batteryConfigMutable()->vbatscale // 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
@ -1265,17 +1291,48 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
boardAlignmentMutable()->pitchDegrees = bstRead16();
boardAlignmentMutable()->yawDegrees = bstRead16();
break;
case BST_SET_VOLTAGE_METER_CONFIG:
batteryConfigMutable()->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
case BST_SET_VOLTAGE_METER_CONFIG: {
int sensor = bstRead8();
if (sensor != VOLTAGE_METER_ADC) {
return -1;
}
int index = bstRead8();
if (index >= MAX_VOLTAGE_SENSOR_ADC) {
return -1;
}
voltageSensorADCConfigMutable(index)->vbatscale = bstRead8();
voltageSensorADCConfigMutable(index)->vbatresdivval = bstRead8();
voltageSensorADCConfigMutable(index)->vbatresdivmultiplier = bstRead8();
break;
case BST_SET_CURRENT_METER_CONFIG:
batteryConfigMutable()->currentMeterScale = bstRead16();
batteryConfigMutable()->currentMeterOffset = bstRead16();
batteryConfigMutable()->currentMeterType = bstRead8();
}
case BST_SET_CURRENT_METER_CONFIG: {
int sensor = bstRead8();
if (sensor != CURRENT_SENSOR_VIRTUAL || sensor != CURRENT_SENSOR_ADC) {
return -1;
}
int index = bstRead8();
if (index >= MAX_ADC_OR_VIRTUAL_CURRENT_METERS) {
return -1;
}
currentMeterADCOrVirtualConfigMutable(index)->scale = bstRead16();
currentMeterADCOrVirtualConfigMutable(index)->offset = bstRead16();
break;
}
case BST_SET_BATTERY_CONFIG:
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()->batteryCapacity = bstRead16();
batteryConfigMutable()->currentMeterSource = bstRead8();
break;
#ifndef USE_QUAD_MIXER_ONLY
@ -1338,8 +1395,8 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
boardAlignmentMutable()->pitchDegrees = bstRead16(); // board_align_pitch
boardAlignmentMutable()->yawDegrees = bstRead16(); // board_align_yaw
batteryConfigMutable()->currentMeterScale = bstRead16();
batteryConfigMutable()->currentMeterOffset = bstRead16();
bstRead16(); // was batteryConfigMutable()->currentMeterScale
bstRead16(); // was batteryConfigMutable()->currentMeterOffset
break;
case BST_SET_CF_SERIAL_CONFIG:
{

2
src/main/target/COLIBRI_RACE/target.h
View file

@ -117,7 +117,7 @@
#define RSSI_ADC_PIN PC2
#define EXTERNAL1_ADC_PIN PC3
#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_FAILSAFE | FEATURE_AIRMODE | FEATURE_LED_STRIP)
#define DEFAULT_FEATURES (FEATURE_FAILSAFE | FEATURE_AIRMODE | FEATURE_LED_STRIP)
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART2

1
src/main/target/ELLE0/target.h
View file

@ -94,7 +94,6 @@
#undef LED_STRIP
#define DEFAULT_FEATURES (FEATURE_VBAT)
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048
#define SERIALRX_UART SERIAL_PORT_USART2

2
src/main/target/F4BY/target.h
View file

@ -141,7 +141,7 @@
#undef LED_STRIP
#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX)
#define DEFAULT_FEATURES (FEATURE_BLACKBOX)
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS

2
src/main/target/FISHDRONEF4/target.h
View file

@ -127,7 +127,7 @@
#define RSSI_ADC_PIN PC1
// *************** FEATURES ************************
#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX | FEATURE_VTX)
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_BLACKBOX | FEATURE_VTX)
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART3

2
src/main/target/FURYF4/target.h
View file

@ -166,7 +166,7 @@
#define RSSI_ADC_PIN PC2
#define CURRENT_METER_ADC_PIN PC3
#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX)
#define DEFAULT_FEATURES (FEATURE_BLACKBOX)
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS

2
src/main/target/IMPULSERCF3/target.h
View file

@ -89,7 +89,7 @@
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX)
#define DEFAULT_FEATURES (FEATURE_BLACKBOX)
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_UART SERIAL_PORT_USART2

1
src/main/target/KISSFC/target.h
View file

@ -95,7 +95,6 @@
#define CURRENT_METER_ADC_PIN PA2
//#define RSSI_ADC_PIN PB2
#define DEFAULT_FEATURES FEATURE_VBAT
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART2

2
src/main/target/KIWIF4/target.h
View file

@ -119,7 +119,7 @@
#define RSSI_ADC_PIN PC2
#define CURRENT_METER_ADC_PIN PC3
#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_OSD)
#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_OSD)
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS

10
src/main/target/KROOZX/config.c
View file

@ -29,17 +29,17 @@
#include "io/osd.h"
#define VBAT_SCALE 113
#define CURRENT_SCALE 1000
#define CURRENT_OFFSET 0
#define CURRENTSCALE 1000
#define CURRENTOFFSET 0
#define OSD_POS(x,y) (x | (y << 5))
#ifdef TARGET_CONFIG
void targetConfiguration(void)
{
batteryConfigMutable()->vbatscale = VBAT_SCALE;
batteryConfigMutable()->currentMeterScale = CURRENT_SCALE;
batteryConfigMutable()->currentMeterOffset = CURRENT_OFFSET;
voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale = VBAT_SCALE;
currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->offset = CURRENTOFFSET;
currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->scale = CURRENTSCALE;
barometerConfigMutable()->baro_hardware = 0;
compassConfigMutable()->mag_hardware = 0;
osdConfigMutable()->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1) | VISIBLE_FLAG;

3
src/main/target/KROOZX/target.h
View file

@ -86,6 +86,7 @@
#define OSD_CH_SWITCH PC5
#define BOARD_HAS_VOLTAGE_DIVIDER
#define BOARD_HAS_CURRENT_SENSOR
#define USE_ADC
#define ADC_INSTANCE ADC1
#define VBAT_ADC_PIN PC3
@ -144,7 +145,7 @@
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define RX_CHANNELS_TAER
#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_OSD)
#define DEFAULT_FEATURES (FEATURE_OSD)
#define LED_STRIP

7
src/main/target/MULTIFLITEPICO/config.c
View file

@ -39,15 +39,16 @@
#include "sensors/compass.h"
#include "sensors/gyro.h"
#define VBAT_SCALE 100
// alternative defaults settings for MULTIFLITEPICO targets
void targetConfiguration(void)
{
compassConfigMutable()->mag_hardware = MAG_NONE; // disabled by default
batteryConfigMutable()->vbatscale = 100;
batteryConfigMutable()->vbatresdivval = 15;
batteryConfigMutable()->vbatresdivmultiplier = 4;
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;

2
src/main/target/MULTIFLITEPICO/target.h
View file

@ -116,8 +116,8 @@
//#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
#define DEFAULT_FEATURES (FEATURE_MOTOR_STOP)
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define DEFAULT_FEATURES (FEATURE_MOTOR_STOP|FEATURE_VBAT)
#define BRUSHED_MOTORS
#define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048
#define SERIALRX_UART SERIAL_PORT_USART3

5
src/main/target/OMNIBUS/target.h
View file

@ -145,7 +145,8 @@
// #define AFATFS_USE_INTROSPECTIVE_LOGGING
#define USE_ADC
//#define BOARD_HAS_VOLTAGE_DIVIDER
#define BOARD_HAS_VOLTAGE_DIVIDER
#define BOARD_HAS_CURRENT_SENSOR
#define VBAT_ADC_PIN PA0
#define CURRENT_METER_ADC_PIN PA1
#define ADC_INSTANCE ADC1
@ -159,7 +160,7 @@
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_OSD)
#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_OSD)
#define BUTTONS
#define BUTTON_A_PIN PB1

4
src/main/target/OMNIBUSF4/target.h
View file

@ -181,9 +181,9 @@
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define AVOID_UART1_FOR_PWM_PPM
#if defined(CL_RACINGF4)
#define DEFAULT_FEATURES (FEATURE_BLACKBOX |FEATURE_CURRENT_METER | FEATURE_TELEMETRY| FEATURE_VBAT | FEATURE_OSD )
#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_TELEMETRY | FEATURE_OSD )
#else
#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD)
#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_OSD)
#endif
#define SPEKTRUM_BIND_PIN UART1_RX_PIN

4
src/main/target/PIKOBLX/config.c
View file

@ -25,8 +25,10 @@
#include "sensors/battery.h"
#define CURRENTSCALE 125
void targetConfiguration(void)
{
batteryConfigMutable()->currentMeterScale = 125;
currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->scale = CURRENTSCALE;
}
#endif

2
src/main/target/RACEBASE/target.h
View file

@ -105,7 +105,7 @@
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_VBAT | FEATURE_RSSI_ADC | FEATURE_OSD)
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_RSSI_ADC | FEATURE_OSD)
#define USE_SERIAL_4WAY_BLHELI_INTERFACE

1
src/main/target/RCEXPLORERF3/target.h
View file

@ -102,7 +102,6 @@
#define CURRENT_METER_ADC_PIN PB2
#define RSSI_ADC_PIN PA6
#define DEFAULT_FEATURES FEATURE_VBAT
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART1

1
src/main/target/RG_SSD_F3/config.c
View file

@ -28,6 +28,5 @@
void targetConfiguration(void)
{
batteryConfigMutable()->vbatmaxcellvoltage = 45;
batteryConfigMutable()->vbatscale = VBAT_SCALE_DEFAULT;
}
#endif

2
src/main/target/SINGULARITY/target.h
View file

@ -89,7 +89,7 @@
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX)
#define DEFAULT_FEATURES (FEATURE_BLACKBOX)
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_UART SERIAL_PORT_USART2

3
src/main/target/SPRACINGF3/target.h
View file

@ -164,6 +164,7 @@
#define M25P16_SPI_INSTANCE SPI2
#define BOARD_HAS_VOLTAGE_DIVIDER
#define BOARD_HAS_CURRENT_SENSOR
#define USE_ADC
#define ADC_INSTANCE ADC2
#define VBAT_ADC_PIN PA4
@ -181,7 +182,7 @@
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY)
#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_TELEMETRY)
#define SPEKTRUM_BIND_PIN UART3_RX_PIN

3
src/main/target/SPRACINGF3EVO/target.h
View file

@ -132,6 +132,7 @@
#define MPU6500_SPI_INSTANCE SPI1
#define BOARD_HAS_VOLTAGE_DIVIDER
#define USE_ADC
#define ADC_INSTANCE ADC2
#define RSSI_ADC_PIN PB2
@ -148,7 +149,7 @@
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY)
#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_TELEMETRY)
#define SPEKTRUM_BIND_PIN UART3_RX_PIN

4
src/main/target/SPRACINGF3NEO/target.h
View file

@ -136,6 +136,8 @@
#define MPU6500_SPI_INSTANCE SPI1
#define BOARD_HAS_VOLTAGE_DIVIDER
#define BOARD_HAS_CURRENT_SENSOR
#define USE_ADC
#define ADC_INSTANCE ADC1
#define VBAT_ADC_PIN PC1
@ -158,7 +160,7 @@
#define OSD
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_OSD)
#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_TELEMETRY)
#define BUTTONS
#define BUTTON_A_PIN PD2

19
src/main/target/TINYFISH/config.c
View file

@ -38,24 +38,25 @@
#define TARGET_CPU_VOLTAGE 3.0
#define CURRENTOFFSET 0
// board uses an ina139, RL=0.005, Rs=30000
// V/A = (0.005 * 0.001 * 30000) * I
// rescale to 1/10th mV / A -> * 1000 * 10
// use 3.0V as cpu and adc voltage -> rescale by 3.0/3.3
#define CURRENTSCALE (0.005 * 0.001 * 30000) * 1000 * 10 * (TARGET_CPU_VOLTAGE / 3.3)
// set default settings to match our target
void targetConfiguration(void)
{
batteryConfigMutable()->currentMeterOffset = 0;
// we use an ina139, RL=0.005, Rs=30000
// V/A = (0.005 * 0.001 * 30000) * I
// rescale to 1/10th mV / A -> * 1000 * 10
// we use 3.0V as cpu and adc voltage -> rescale by 3.0/3.3
batteryConfigMutable()->currentMeterScale = (0.005 * 0.001 * 30000) * 1000 * 10 * (TARGET_CPU_VOLTAGE / 3.3);
currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->offset = CURRENTOFFSET;
currentMeterADCOrVirtualConfigMutable(CURRENT_SENSOR_ADC)->scale = CURRENTSCALE;
// we use the same uart for frsky telemetry and SBUS, both non inverted
// use the same uart for frsky telemetry and SBUS, both non inverted
const int index = findSerialPortIndexByIdentifier(SBUS_TELEMETRY_UART);
serialConfigMutable()->portConfigs[index].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL;
rxConfigMutable()->serialrx_provider = SERIALRX_SBUS;
rxConfigMutable()->sbus_inversion = 0;
telemetryConfigMutable()->telemetry_inversion = 0;
featureSet(FEATURE_CURRENT_METER | FEATURE_VBAT);
}
#endif

3
src/main/target/TINYFISH/target.h
View file

@ -94,6 +94,7 @@
#define USE_ADC
#define BOARD_HAS_VOLTAGE_DIVIDER
#define BOARD_HAS_CURRENT_SENSOR
#define VBAT_ADC_PIN PB1
#define CURRENT_METER_ADC_PIN PB0
#define ADC_INSTANCE ADC3
@ -107,7 +108,7 @@
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_TELEMETRY)
#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_TELEMETRY)
#define TARGET_CONFIG
#define USE_SERIAL_4WAY_BLHELI_INTERFACE

2
src/main/target/VRRACE/target.h
View file

@ -159,7 +159,7 @@
#undef LED_STRIP
#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_SOFTSERIAL | FEATURE_TELEMETRY)
#define DEFAULT_FEATURES (FEATURE_SOFTSERIAL | FEATURE_TELEMETRY)
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS

6
src/main/telemetry/crsf.c
View file

@ -174,7 +174,7 @@ void crsfFrameBatterySensor(sbuf_t *dst)
// use sbufWrite since CRC does not include frame length
sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
crsfSerialize8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR);
crsfSerialize16(dst, getVbat()); // vbat is in units of 0.1V
crsfSerialize16(dst, getBatteryVoltage()); // vbat is in units of 0.1V
#ifdef CLEANFLIGHT
const amperageMeter_t *amperageMeter = getAmperageMeter(batteryConfig()->amperageMeterSource);
const int16_t amperage = constrain(amperageMeter->amperage, -0x8000, 0x7FFF) / 10; // send amperage in 0.01 A steps, range is -320A to 320A
@ -182,9 +182,9 @@ void crsfFrameBatterySensor(sbuf_t *dst)
const uint32_t batteryCapacity = batteryConfig()->batteryCapacity;
const uint8_t batteryRemainingPercentage = batteryCapacityRemainingPercentage();
#else
crsfSerialize16(dst, amperage / 10);
crsfSerialize16(dst, getAmperage() / 10);
const uint32_t batteryCapacity = batteryConfig()->batteryCapacity;
const uint8_t batteryRemainingPercentage = calculateBatteryPercentage();
const uint8_t batteryRemainingPercentage = calculateBatteryPercentageRemaining();
#endif
crsfSerialize8(dst, (batteryCapacity >> 16));
crsfSerialize8(dst, (batteryCapacity >> 8));

20
src/main/telemetry/frsky.c
View file

@ -371,6 +371,7 @@ static void sendVoltage(void)
uint32_t cellVoltage;
uint16_t payload;
uint8_t cellCount = getBatteryCellCount();
/*
* Format for Voltage Data for single cells is like this:
*
@ -382,7 +383,7 @@ static void sendVoltage(void)
* The actual value sent for cell voltage has resolution of 0.002 volts
* Since vbat has resolution of 0.1 volts it has to be multiplied by 50
*/
cellVoltage = ((uint32_t)getVbat() * 100 + batteryCellCount) / (batteryCellCount * 2);
cellVoltage = ((uint32_t)getBatteryVoltage() * 100 + cellCount) / (cellCount * 2);
// Cell number is at bit 9-12
payload = (currentCell << 4);
@ -397,7 +398,7 @@ static void sendVoltage(void)
serialize16(payload);
currentCell++;
currentCell %= batteryCellCount;
currentCell %= cellCount;
}
/*
@ -405,17 +406,18 @@ static void sendVoltage(void)
*/
static void sendVoltageAmp(void)
{
uint16_t batteryVoltage = getBatteryVoltage();
if (telemetryConfig()->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) {
/*
* Use new ID 0x39 to send voltage directly in 0.1 volts resolution
*/
sendDataHead(ID_VOLTAGE_AMP);
serialize16(getVbat());
serialize16(batteryVoltage);
} else {
uint16_t voltage = (getVbat() * 110) / 21;
uint16_t voltage = (batteryVoltage * 110) / 21;
uint16_t vfasVoltage;
if (telemetryConfig()->frsky_vfas_cell_voltage) {
vfasVoltage = voltage / batteryCellCount;
vfasVoltage = voltage / getBatteryCellCount();
} else {
vfasVoltage = voltage;
}
@ -429,7 +431,7 @@ static void sendVoltageAmp(void)
static void sendAmperage(void)
{
sendDataHead(ID_CURRENT);
serialize16((uint16_t)(amperage / 10));
serialize16((uint16_t)(getAmperage() / 10));
}
static void sendFuelLevel(void)
@ -437,9 +439,9 @@ static void sendFuelLevel(void)
sendDataHead(ID_FUEL_LEVEL);
if (batteryConfig()->batteryCapacity > 0) {
serialize16((uint16_t)calculateBatteryPercentage());
serialize16((uint16_t)calculateBatteryPercentageRemaining());
} else {
serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF));
serialize16((uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF));
}
}
@ -537,7 +539,7 @@ void handleFrSkyTelemetry(void)
sendTemperature1();
sendThrottleOrBatterySizeAsRpm();
if ((feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) && batteryCellCount > 0) {
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryCellCount() > 0) {
sendVoltage();
sendVoltageAmp();
sendAmperage();

12
src/main/telemetry/hott.c
View file

@ -247,24 +247,24 @@ static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage)
static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage)
{
hottEAMMessage->main_voltage_L = getVbat() & 0xFF;
hottEAMMessage->main_voltage_H = getVbat() >> 8;
hottEAMMessage->batt1_voltage_L = getVbat() & 0xFF;
hottEAMMessage->batt1_voltage_H = getVbat() >> 8;
hottEAMMessage->main_voltage_L = getBatteryVoltage() & 0xFF;
hottEAMMessage->main_voltage_H = getBatteryVoltage() >> 8;
hottEAMMessage->batt1_voltage_L = getBatteryVoltage() & 0xFF;
hottEAMMessage->batt1_voltage_H = getBatteryVoltage() >> 8;
updateAlarmBatteryStatus(hottEAMMessage);
}
static inline void hottEAMUpdateCurrentMeter(HOTT_EAM_MSG_t *hottEAMMessage)
{
int32_t amp = amperage / 10;
int32_t amp = getAmperage() / 10;
hottEAMMessage->current_L = amp & 0xFF;
hottEAMMessage->current_H = amp >> 8;
}
static inline void hottEAMUpdateBatteryDrawnCapacity(HOTT_EAM_MSG_t *hottEAMMessage)
{
int32_t mAh = mAhDrawn / 10;
int32_t mAh = getMAhDrawn() / 10;
hottEAMMessage->batt_cap_L = mAh & 0xFF;
hottEAMMessage->batt_cap_H = mAh >> 8;
}

4
src/main/telemetry/ibus_shared.c
View file

@ -125,9 +125,9 @@ static uint8_t dispatchMeasurementReply(ibusAddress_t address)
switch (sensorAddressTypeLookup[address - ibusBaseAddress]) {
case IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE:
value = getVbat() * 10;
value = getBatteryVoltage() * 10;
if (telemetryConfig()->report_cell_voltage) {
value /= batteryCellCount;
value /= getBatteryCellCount();
}
return sendIbusMeasurement(address, value);

2
src/main/telemetry/ltm.c
View file

@ -190,7 +190,7 @@ static void ltm_sframe(void)
if (failsafeIsActive())
lt_statemode |= 2;
ltm_initialise_packet('S');
ltm_serialise_16(getVbat() * 100); //vbat converted to mv
ltm_serialise_16(getBatteryVoltage() * 100); //vbat converted to mv
ltm_serialise_16(0); // current, not implemented
ltm_serialise_8((uint8_t)((rssi * 254) / 1023)); // scaled RSSI (uchar)
ltm_serialise_8(0); // no airspeed

6
src/main/telemetry/mavlink.c
View file

@ -223,11 +223,11 @@ void mavlinkSendSystemStatus(void)
// load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
0,
// voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
feature(FEATURE_VBAT) ? getVbat() * 100 : 0,
(batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) ? getBatteryVoltage() * 100 : 0,
// current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
feature(FEATURE_VBAT) ? amperage : -1,
(batteryConfig()->currentMeterSource != CURRENT_METER_NONE) ? getAmperage() : -1,
// battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
feature(FEATURE_VBAT) ? calculateBatteryPercentage() : 100,
(batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) ? calculateBatteryPercentageRemaining() : 100,
// drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
0,
// errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)

18
src/main/telemetry/smartport.c
View file

@ -618,20 +618,20 @@ void handleSmartPortTelemetry(void)
break;
#endif
case FSSP_DATAID_VFAS :
if (feature(FEATURE_VBAT) && batteryCellCount > 0) {
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryCellCount() > 0) {
uint16_t vfasVoltage;
if (telemetryConfig()->frsky_vfas_cell_voltage) {
vfasVoltage = getVbat() / batteryCellCount;
vfasVoltage = getBatteryVoltage() / getBatteryCellCount();
} else {
vfasVoltage = getVbat();
vfasVoltage = getBatteryVoltage();
}
smartPortSendPackage(id, vfasVoltage * 10); // given in 0.1V, convert to volts
smartPortHasRequest = 0;
}
break;
case FSSP_DATAID_CURRENT :
if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_SENSOR)) {
smartPortSendPackage(id, amperage / 10); // given in 10mA steps, unknown requested unit
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
smartPortSendPackage(id, getAmperage() / 10); // given in 10mA steps, unknown requested unit
smartPortHasRequest = 0;
}
break;
@ -643,8 +643,8 @@ void handleSmartPortTelemetry(void)
}
break;
case FSSP_DATAID_FUEL :
if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_SENSOR)) {
smartPortSendPackage(id, mAhDrawn); // given in mAh, unknown requested unit
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
smartPortSendPackage(id, getMAhDrawn()); // given in mAh, unknown requested unit
smartPortHasRequest = 0;
}
break;
@ -791,8 +791,8 @@ void handleSmartPortTelemetry(void)
break;
#endif
case FSSP_DATAID_A4 :
if (feature(FEATURE_VBAT) && batteryCellCount > 0) {
smartPortSendPackage(id, getVbat() * 10 / batteryCellCount ); // given in 0.1V, convert to volts
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryCellCount() > 0) {
smartPortSendPackage(id, getBatteryVoltage() * 10 / getBatteryCellCount()); // given in 0.1V, convert to volts
smartPortHasRequest = 0;
}
break;

8
src/main/telemetry/srxl.c
View file

@ -161,7 +161,7 @@ void srxlFrameRpm(sbuf_t *dst)
srxlSerialize8(dst, SRXL_FRAMETYPE_TELE_RPM);
srxlSerialize8(dst, SRXL_FRAMETYPE_SID);
srxlSerialize16(dst, 0xFFFF); // pulse leading edges
srxlSerialize16(dst, getVbat() * 10); // vbat is in units of 0.1V
srxlSerialize16(dst, getBatteryVoltage() * 10); // vbat is in units of 0.1V
srxlSerialize16(dst, 0x7FFF); // temperature
srxlSerialize8(dst, 0xFF); // dbmA
srxlSerialize8(dst, 0xFF); // dbmB
@ -200,9 +200,9 @@ void srxlFramePowerBox(sbuf_t *dst)
{
srxlSerialize8(dst, SRXL_FRAMETYPE_POWERBOX);
srxlSerialize8(dst, SRXL_FRAMETYPE_SID);
srxlSerialize16(dst, getVbat() * 10); // vbat is in units of 0.1V - vbat1
srxlSerialize16(dst, getVbat() * 10); // vbat is in units of 0.1V - vbat2
srxlSerialize16(dst, amperage / 10);
srxlSerialize16(dst, getBatteryVoltage() * 10); // vbat is in units of 0.1V - vbat1
srxlSerialize16(dst, getBatteryVoltage() * 10); // vbat is in units of 0.1V - vbat2
srxlSerialize16(dst, getAmperage() / 10);
srxlSerialize16(dst, 0xFFFF);
srxlSerialize16(dst, 0xFFFF); // spare

36
src/test/unit/telemetry_crsf_unittest.cc
View file

@ -54,7 +54,10 @@ extern "C" {
#include "telemetry/telemetry.h"
bool airMode;
uint16_t vbat;
uint16_t testBatteryVoltage = 0;
int32_t testAmperage = 0;
serialPort_t *telemetrySharedPort;
PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
@ -138,7 +141,7 @@ TEST(TelemetryCrsfTest, TestBattery)
{
uint8_t frame[CRSF_FRAME_SIZE_MAX];
vbat = 0; // 0.1V units
testBatteryVoltage = 0; // 0.1V units
int frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR);
EXPECT_EQ(CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen);
EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address
@ -154,8 +157,8 @@ TEST(TelemetryCrsfTest, TestBattery)
EXPECT_EQ(67, remaining);
EXPECT_EQ(crfsCrc(frame, frameLen), frame[11]);
vbat = 33; // 3.3V = 3300 mv
amperage = 2960; // = 29.60A = 29600mA - amperage is in 0.01A steps
testBatteryVoltage = 33; // 3.3V = 3300 mv
testAmperage = 2960; // = 29.60A = 29600mA - amperage is in 0.01A steps
batteryConfigMutable()->batteryCapacity = 1234;
frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR);
voltage = frame[3] << 8 | frame[4]; // mV * 100
@ -283,9 +286,6 @@ uint16_t GPS_altitude; // altitude in m
uint16_t GPS_speed; // speed in 0.1m/s
uint16_t GPS_ground_course = 0; // degrees * 10
int32_t amperage;
int32_t mAhDrawn;
void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);}
uint32_t micros(void) {return 0;}
@ -308,11 +308,23 @@ bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return true;}
portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;}
uint8_t batteryCapacityRemainingPercentage(void) {return 67;}
uint8_t calculateBatteryCapacityRemainingPercentage(void) {return 67;}
uint8_t calculateBatteryPercentage(void) {return 67;}
batteryState_e getBatteryState(void) {return BATTERY_OK;}
bool isAirmodeActive(void) {return airMode;}
uint16_t getVbat(void) { return vbat; }
int32_t getAmperage(void) {
return testAmperage;
}
uint16_t getBatteryVoltage(void) {
return testBatteryVoltage;
}
batteryState_e getBatteryState(void) {
return BATTERY_OK;
}
uint8_t calculateBatteryPercentageRemaining(void) {
return 67;
}
}

21
src/test/unit/telemetry_hott_unittest.cc
View file

@ -51,6 +51,11 @@ extern "C" {
#include "telemetry/hott.h"
PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
uint16_t testBatteryVoltage = 0;
int32_t testAmperage = 0;
int32_t testMAhDrawn = 0;
}
#include "unittest_macros.h"
@ -172,10 +177,7 @@ uint16_t GPS_speed; // speed in 0.1m/s
uint16_t GPS_distanceToHome; // distance to home point in meters
uint16_t GPS_altitude; // altitude in 0.1m
int16_t GPS_directionToHome; // direction to home or hol point in degrees
uint16_t vbat;
int32_t amperage;
int32_t mAhDrawn;
uint32_t fixedMillis = 0;
@ -264,8 +266,17 @@ batteryState_e getBatteryState(void)
return BATTERY_OK;
}
uint16_t getVbat(void)
uint16_t getBatteryVoltage(void)
{
return vbat;
return testBatteryVoltage;
}
int32_t getAmperage(void) {
return testAmperage;
}
int32_t getMAhDrawn(void) {
return testMAhDrawn;
}
}

34
src/test/unit/telemetry_ibus_unittest.cc
View file

@ -37,7 +37,7 @@ extern "C" {
extern "C" {
uint8_t batteryCellCount = 3;
uint8_t testBatteryCellCount =3;
int16_t rcCommand[4] = {0, 0, 0, 0};
telemetryConfig_t telemetryConfig_System;
}
@ -62,6 +62,16 @@ typedef struct serialPortStub_s {
} serialPortStub_t;
static uint16_t testBatteryVoltage = 100;
uint16_t getBatteryVoltage(void)
{
return testBatteryVoltage;
}
uint8_t getBatteryCellCount(void) {
return testBatteryCellCount;
}
static serialPortStub_t serialWriteStub;
static serialPortStub_t serialReadStub;
@ -367,7 +377,7 @@ TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToGetMeasurementVbattZero)
{
//Given ibus command: Sensor at address 1, please send your measurement
//then we respond with: I'm reading 0 volts
vbat = 0;
testBatteryVoltage = 0;
checkResponseToCommand("\x04\xA1\x5a\xff", 4, "\x06\xA1\x00\x00\x58\xFF", 6);
}
@ -377,14 +387,14 @@ 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
batteryCellCount = 3;
vbat = 30;
testBatteryCellCount =3;
testBatteryVoltage = 30;
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
batteryCellCount = 1;
vbat = 10;
testBatteryCellCount =1;
testBatteryVoltage = 10;
checkResponseToCommand("\x04\xA1\x5a\xff", 4, "\x06\xA1\x64\x00\xf4\xFe", 6);
}
@ -394,14 +404,14 @@ 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
batteryCellCount = 3;
vbat = 10;
testBatteryCellCount =3;
testBatteryVoltage = 10;
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
batteryCellCount = 1;
vbat = 10;
testBatteryCellCount =1;
testBatteryVoltage = 10;
checkResponseToCommand("\x04\xA1\x5a\xff", 4, "\x06\xA1\x64\x00\xf4\xFe", 6);
}
@ -488,8 +498,8 @@ 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
batteryCellCount = 1;
vbat = 10;
testBatteryCellCount = 1;
testBatteryVoltage = 10;
checkResponseToCommand("\x04\xA3\x58\xff", 4, "\x06\xA3\x64\x00\xf2\xfe", 6);
//Given ibus command: Sensor at address 4, please send your measurement