Battery auto-detect and LPF for battery monitoring
This commit is contained in:
parent
26ac6115e7
commit
942c89237e
|
@ -656,7 +656,7 @@ void applyLedWarningLayer(uint8_t updateNow)
|
|||
|
||||
if (updateNow && warningFlashCounter == 0) {
|
||||
warningFlags = WARNING_FLAG_NONE;
|
||||
if (feature(FEATURE_VBAT) && calculateBatteryState() != BATTERY_OK) {
|
||||
if (feature(FEATURE_VBAT) && getBatteryState() != BATTERY_OK) {
|
||||
warningFlags |= WARNING_FLAG_LOW_BATTERY;
|
||||
}
|
||||
if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) {
|
||||
|
|
|
@ -2042,8 +2042,8 @@ static void cliStatus(char *cmdline)
|
|||
{
|
||||
UNUSED(cmdline);
|
||||
|
||||
printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery)\r\n",
|
||||
millis() / 1000, vbat, batteryCellCount);
|
||||
printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery - %s)\r\n",
|
||||
millis() / 1000, vbat, batteryCellCount,getBatteryStateString( ));
|
||||
|
||||
|
||||
printf("CPU Clock=%dMHz", (SystemCoreClock / 1000000));
|
||||
|
|
|
@ -88,8 +88,10 @@ enum {
|
|||
ALIGN_MAG = 2
|
||||
};
|
||||
|
||||
/* for VBAT monitoring frequency */
|
||||
#define VBATFREQ 6 // to read battery voltage - nth number of loop iterations
|
||||
/* VBAT monitoring interval (in microseconds) - 1s*/
|
||||
#define VBATINTERVAL (6 * 3500)
|
||||
/* IBat monitoring interval (in microseconds) - 6 default looptimes */
|
||||
#define IBATINTERVAL (6 * 3500)
|
||||
|
||||
uint32_t currentTime = 0;
|
||||
uint32_t previousTime = 0;
|
||||
|
@ -174,10 +176,9 @@ void annexCode(void)
|
|||
int32_t tmp, tmp2;
|
||||
int32_t axis, prop1 = 0, prop2;
|
||||
|
||||
static batteryState_e batteryState = BATTERY_OK;
|
||||
static uint8_t vbatTimer = 0;
|
||||
static int32_t vbatCycleTime = 0;
|
||||
|
||||
static uint32_t vbatLastServiced = 0;
|
||||
static uint32_t ibatLastServiced = 0;
|
||||
uint32_t ibatTimeSinceLastServiced;
|
||||
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
|
||||
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
|
||||
prop2 = 100;
|
||||
|
@ -247,24 +248,20 @@ void annexCode(void)
|
|||
rcCommand[PITCH] = rcCommand_PITCH;
|
||||
}
|
||||
|
||||
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) {
|
||||
vbatCycleTime += cycleTime;
|
||||
if (!(++vbatTimer % VBATFREQ)) {
|
||||
if (feature(FEATURE_VBAT)) {
|
||||
/* currentTime will rollover @ 70 minutes */
|
||||
if ((currentTime - vbatLastServiced) >= VBATINTERVAL) {
|
||||
vbatLastServiced = currentTime;
|
||||
updateBattery();
|
||||
}
|
||||
}
|
||||
|
||||
if (feature(FEATURE_VBAT)) {
|
||||
updateBatteryVoltage();
|
||||
batteryState = calculateBatteryState();
|
||||
//handle beepers for battery levels
|
||||
if (batteryState == BATTERY_CRITICAL)
|
||||
beeper(BEEPER_BAT_CRIT_LOW); //critically low battery
|
||||
else if (batteryState == BATTERY_WARNING)
|
||||
beeper(BEEPER_BAT_LOW); //low battery
|
||||
}
|
||||
|
||||
if (feature(FEATURE_CURRENT_METER)) {
|
||||
updateCurrentMeter(vbatCycleTime, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
|
||||
}
|
||||
vbatCycleTime = 0;
|
||||
if (feature(FEATURE_CURRENT_METER)) {
|
||||
/* currentTime will rollover @ 70 minutes */
|
||||
ibatTimeSinceLastServiced = (currentTime - ibatLastServiced);
|
||||
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
|
||||
ibatLastServiced = currentTime;
|
||||
updateCurrentMeter((ibatTimeSinceLastServiced / 1000), &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -31,13 +31,18 @@
|
|||
#include "rx/rx.h"
|
||||
|
||||
#include "io/rc_controls.h"
|
||||
#include "flight/lowpass.h"
|
||||
#include "io/beeper.h"
|
||||
|
||||
#define VBATT_DETECT 10
|
||||
#define VBATT_LPF_FREQ 10
|
||||
|
||||
// Battery monitoring stuff
|
||||
uint8_t batteryCellCount = 3; // cell count
|
||||
uint16_t batteryWarningVoltage;
|
||||
uint16_t batteryCriticalVoltage;
|
||||
|
||||
uint8_t vbat = 0; // battery voltage in 0.1V steps
|
||||
uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered)
|
||||
uint16_t vbatLatestADC = 0; // most recent unsmoothed raw reading from vbat ADC
|
||||
uint16_t amperageLatestADC = 0; // most recent raw reading from current ADC
|
||||
|
||||
|
@ -46,6 +51,9 @@ int32_t mAhDrawn = 0; // milliampere hours drawn from the battery
|
|||
|
||||
batteryConfig_t *batteryConfig;
|
||||
|
||||
static batteryState_e batteryState;
|
||||
static lowpass_t lowpassFilter;
|
||||
|
||||
uint16_t batteryAdcToVoltage(uint16_t src)
|
||||
{
|
||||
// calculate battery voltage based on ADC reading
|
||||
|
@ -53,53 +61,106 @@ uint16_t batteryAdcToVoltage(uint16_t src)
|
|||
return ((uint32_t)src * batteryConfig->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * 10);
|
||||
}
|
||||
|
||||
#define BATTERY_SAMPLE_COUNT 8
|
||||
|
||||
void updateBatteryVoltage(void)
|
||||
static void updateBatteryVoltage(void)
|
||||
{
|
||||
static uint16_t vbatSamples[BATTERY_SAMPLE_COUNT];
|
||||
static uint8_t currentSampleIndex = 0;
|
||||
uint8_t index;
|
||||
uint16_t vbatSampleTotal = 0;
|
||||
uint16_t vbatSample;
|
||||
uint16_t vbatFiltered;
|
||||
|
||||
// store the battery voltage with some other recent battery voltage readings
|
||||
vbatSamples[(currentSampleIndex++) % BATTERY_SAMPLE_COUNT] = vbatLatestADC = adcGetChannel(ADC_BATTERY);
|
||||
|
||||
// calculate vbat based on the average of recent readings
|
||||
for (index = 0; index < BATTERY_SAMPLE_COUNT; index++) {
|
||||
vbatSampleTotal += vbatSamples[index];
|
||||
}
|
||||
vbat = batteryAdcToVoltage(vbatSampleTotal / BATTERY_SAMPLE_COUNT);
|
||||
vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
|
||||
vbatFiltered = (uint16_t)lowpassFixed(&lowpassFilter, vbatSample, VBATT_LPF_FREQ);
|
||||
vbat = batteryAdcToVoltage(vbatFiltered);
|
||||
}
|
||||
|
||||
batteryState_e calculateBatteryState(void)
|
||||
#define VBATTERY_STABLE_DELAY 40
|
||||
/* Batt Hysteresis of +/-100mV */
|
||||
#define VBATT_HYSTERESIS 1
|
||||
|
||||
void updateBattery(void)
|
||||
{
|
||||
if (vbat <= batteryCriticalVoltage) {
|
||||
return BATTERY_CRITICAL;
|
||||
updateBatteryVoltage();
|
||||
|
||||
/* battery has just been connected*/
|
||||
if(batteryState == BATTERY_NOTPRESENT && vbat > VBATT_DETECT)
|
||||
{
|
||||
/* Actual battery state is calculated below, this is really BATTERY_PRESENT */
|
||||
batteryState = BATTERY_OK;
|
||||
/* wait for VBatt to stabilise then we can calc number of cells
|
||||
(using the filtered value takes a long time to ramp up)
|
||||
We only do this on the ground so don't care if we do block, not
|
||||
worse than original code anyway*/
|
||||
delay(VBATTERY_STABLE_DELAY);
|
||||
updateBatteryVoltage();
|
||||
|
||||
unsigned cells = (batteryAdcToVoltage(vbatLatestADC) / batteryConfig->vbatmaxcellvoltage) + 1;
|
||||
if(cells > 8) // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells)
|
||||
cells = 8;
|
||||
batteryCellCount = cells;
|
||||
batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage;
|
||||
batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage;
|
||||
}
|
||||
if (vbat <= batteryWarningVoltage) {
|
||||
return BATTERY_WARNING;
|
||||
/* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_DETECT */
|
||||
else if(batteryState != BATTERY_NOTPRESENT && vbat <= VBATT_DETECT)
|
||||
{
|
||||
batteryState = BATTERY_NOTPRESENT;
|
||||
batteryCellCount = 0;
|
||||
batteryWarningVoltage = 0;
|
||||
batteryCriticalVoltage = 0;
|
||||
}
|
||||
|
||||
switch(batteryState)
|
||||
{
|
||||
case BATTERY_OK:
|
||||
if(vbat <= (batteryWarningVoltage - VBATT_HYSTERESIS)){
|
||||
batteryState = BATTERY_WARNING;
|
||||
beeper(BEEPER_BAT_LOW);
|
||||
}
|
||||
break;
|
||||
case BATTERY_WARNING:
|
||||
if(vbat <= (batteryCriticalVoltage - VBATT_HYSTERESIS)){
|
||||
batteryState = BATTERY_CRITICAL;
|
||||
beeper(BEEPER_BAT_CRIT_LOW);
|
||||
}
|
||||
else if(vbat > (batteryWarningVoltage + VBATT_HYSTERESIS)){
|
||||
batteryState = BATTERY_OK;
|
||||
}
|
||||
else{
|
||||
beeper(BEEPER_BAT_LOW);
|
||||
}
|
||||
break;
|
||||
case BATTERY_CRITICAL:
|
||||
if(vbat > (batteryCriticalVoltage + VBATT_HYSTERESIS)){
|
||||
batteryState = BATTERY_WARNING;
|
||||
beeper(BEEPER_BAT_LOW);
|
||||
}
|
||||
else{
|
||||
beeper(BEEPER_BAT_CRIT_LOW);
|
||||
}
|
||||
break;
|
||||
case BATTERY_NOTPRESENT:
|
||||
break;
|
||||
}
|
||||
return BATTERY_OK;
|
||||
}
|
||||
|
||||
batteryState_e getBatteryState(void)
|
||||
{
|
||||
return batteryState;
|
||||
}
|
||||
|
||||
const char * batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PRESENT"};
|
||||
|
||||
const char * getBatteryStateString(void)
|
||||
{
|
||||
return batteryStateStrings[batteryState];
|
||||
}
|
||||
|
||||
void batteryInit(batteryConfig_t *initialBatteryConfig)
|
||||
{
|
||||
batteryConfig = initialBatteryConfig;
|
||||
|
||||
uint32_t i;
|
||||
|
||||
for (i = 0; i < BATTERY_SAMPLE_COUNT; i++) {
|
||||
updateBatteryVoltage();
|
||||
delay((32 / BATTERY_SAMPLE_COUNT) * 10);
|
||||
}
|
||||
|
||||
unsigned cells = (vbat / batteryConfig->vbatmaxcellvoltage) + 1;
|
||||
if(cells > 8) // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells)
|
||||
cells = 8;
|
||||
batteryCellCount = cells;
|
||||
batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage;
|
||||
batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage;
|
||||
batteryState = BATTERY_NOTPRESENT;
|
||||
batteryCellCount = 1;
|
||||
batteryWarningVoltage = 0;
|
||||
batteryCriticalVoltage = 0;
|
||||
}
|
||||
|
||||
#define ADCVREF 3300 // in mV
|
||||
|
|
|
@ -48,10 +48,12 @@ typedef struct batteryConfig_s {
|
|||
typedef enum {
|
||||
BATTERY_OK = 0,
|
||||
BATTERY_WARNING,
|
||||
BATTERY_CRITICAL
|
||||
BATTERY_CRITICAL,
|
||||
BATTERY_NOTPRESENT
|
||||
} batteryState_e;
|
||||
|
||||
extern uint8_t vbat;
|
||||
extern uint16_t vbat;
|
||||
extern uint16_t vbatRaw;
|
||||
extern uint16_t vbatLatestADC;
|
||||
extern uint8_t batteryCellCount;
|
||||
extern uint16_t batteryWarningVoltage;
|
||||
|
@ -60,8 +62,9 @@ extern int32_t amperage;
|
|||
extern int32_t mAhDrawn;
|
||||
|
||||
uint16_t batteryAdcToVoltage(uint16_t src);
|
||||
batteryState_e calculateBatteryState(void);
|
||||
void updateBatteryVoltage(void);
|
||||
batteryState_e getBatteryState(void);
|
||||
const char * getBatteryStateString(void);
|
||||
void updateBattery(void);
|
||||
void batteryInit(batteryConfig_t *initialBatteryConfig);
|
||||
|
||||
void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle);
|
||||
|
|
|
@ -217,9 +217,12 @@ static bool shouldTriggerBatteryAlarmNow(void)
|
|||
|
||||
static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage)
|
||||
{
|
||||
batteryState_e batteryState;
|
||||
|
||||
if (shouldTriggerBatteryAlarmNow()){
|
||||
lastHottAlarmSoundTime = millis();
|
||||
if (vbat <= batteryWarningVoltage){
|
||||
batteryState = getBatteryState();
|
||||
if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL){
|
||||
hottEAMMessage->warning_beeps = 0x10;
|
||||
hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_BATTERY_1;
|
||||
}
|
||||
|
|
|
@ -24,6 +24,8 @@ extern "C" {
|
|||
#include "sensors/battery.h"
|
||||
|
||||
#include "io/rc_controls.h"
|
||||
#include "flight/lowpass.h"
|
||||
#include "io/beeper.h"
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
|
@ -83,6 +85,109 @@ TEST(BatteryTest, BatteryADCToVoltage)
|
|||
}
|
||||
}
|
||||
|
||||
uint16_t currentADCReading;
|
||||
|
||||
|
||||
typedef struct batteryAdcToBatteryStateExpectation_s
|
||||
{
|
||||
uint16_t adcReading;
|
||||
uint16_t expectedVoltageInDeciVoltSteps;
|
||||
batteryState_e expectedBatteryState;
|
||||
uint8_t scale;
|
||||
} batteryAdcToBatteryStateExpectation_t;
|
||||
|
||||
/* Test the battery state and hysteresis code */
|
||||
TEST(BatteryTest, BatteryState)
|
||||
{
|
||||
// batteryInit() reads a bunch of fields including vbatscale, so set up the config with useful initial values:
|
||||
batteryConfig_t batteryConfig = {
|
||||
.vbatscale = VBAT_SCALE_DEFAULT,
|
||||
.vbatmaxcellvoltage = 43,
|
||||
.vbatmincellvoltage = 33,
|
||||
.vbatwarningcellvoltage = 35,
|
||||
.currentMeterScale = 400,
|
||||
.currentMeterOffset = 0,
|
||||
.currentMeterType = CURRENT_SENSOR_NONE,
|
||||
.multiwiiCurrentMeterOutput = 0,
|
||||
.batteryCapacity = 2200,
|
||||
};
|
||||
|
||||
batteryInit(&batteryConfig);
|
||||
|
||||
batteryAdcToBatteryStateExpectation_t batteryAdcToBatteryStateExpectations[] = {
|
||||
{1420, 126, BATTERY_OK, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
|
||||
/* fall down to battery warning level */
|
||||
{1185, 105, BATTERY_OK, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
|
||||
{1175, 104, BATTERY_WARNING, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
|
||||
/* creep back up to battery ok */
|
||||
{1185, 105, BATTERY_WARNING, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
|
||||
{1195, 106, BATTERY_WARNING, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
|
||||
{1207, 107, BATTERY_OK, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
|
||||
/* fall down to battery critical level */
|
||||
{1175, 104, BATTERY_WARNING, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
|
||||
{1108, 98, BATTERY_CRITICAL, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
|
||||
/* creep back up to battery warning */
|
||||
{1115, 99, BATTERY_CRITICAL, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
|
||||
{1130, 100, BATTERY_CRITICAL, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
|
||||
{1145, 101, BATTERY_WARNING, ELEVEN_TO_ONE_VOLTAGE_DIVIDER},
|
||||
|
||||
};
|
||||
uint8_t testIterationCount = sizeof(batteryAdcToBatteryStateExpectations) / sizeof(batteryAdcToBatteryStateExpectation_t);
|
||||
|
||||
// expect
|
||||
for (uint8_t index = 0; index < testIterationCount; index ++) {
|
||||
batteryAdcToBatteryStateExpectation_t *batteryAdcToBatteryStateExpectation = &batteryAdcToBatteryStateExpectations[index];
|
||||
batteryConfig.vbatscale = batteryAdcToBatteryStateExpectation->scale;
|
||||
currentADCReading = batteryAdcToBatteryStateExpectation->adcReading;
|
||||
updateBattery( );
|
||||
batteryState_e batteryState = getBatteryState();
|
||||
EXPECT_EQ(batteryAdcToBatteryStateExpectation->expectedBatteryState, batteryState);
|
||||
}
|
||||
}
|
||||
|
||||
typedef struct batteryAdcToCellCountExpectation_s
|
||||
{
|
||||
uint16_t adcReading;
|
||||
uint16_t expectedVoltageInDeciVoltSteps;
|
||||
uint8_t scale;
|
||||
uint8_t cellCount;
|
||||
} batteryAdcToCellCountExpectation_t;
|
||||
|
||||
/* Test the cell count is correctly detected if we start at 0V */
|
||||
TEST(BatteryTest, CellCount)
|
||||
{
|
||||
// batteryInit() reads a bunch of fields including vbatscale, so set up the config with useful initial values:
|
||||
batteryConfig_t batteryConfig = {
|
||||
.vbatscale = VBAT_SCALE_DEFAULT,
|
||||
.vbatmaxcellvoltage = 43,
|
||||
.vbatmincellvoltage = 33,
|
||||
.vbatwarningcellvoltage = 35,
|
||||
.currentMeterScale = 400,
|
||||
.currentMeterOffset = 0,
|
||||
.currentMeterType = CURRENT_SENSOR_NONE,
|
||||
.multiwiiCurrentMeterOutput = 0,
|
||||
.batteryCapacity = 2200,
|
||||
};
|
||||
|
||||
batteryInit(&batteryConfig);
|
||||
|
||||
batteryAdcToCellCountExpectation_t batteryAdcToCellCountExpectations[] = {
|
||||
{0, 0, ELEVEN_TO_ONE_VOLTAGE_DIVIDER, 1},
|
||||
{1420, 126, ELEVEN_TO_ONE_VOLTAGE_DIVIDER, 3},
|
||||
};
|
||||
uint8_t testIterationCount = sizeof(batteryAdcToCellCountExpectations) / sizeof(batteryAdcToCellCountExpectation_t);
|
||||
|
||||
// expect
|
||||
for (uint8_t index = 0; index < testIterationCount; index ++) {
|
||||
batteryAdcToCellCountExpectation_t *batteryAdcToCellCountExpectation = &batteryAdcToCellCountExpectations[index];
|
||||
batteryConfig.vbatscale = batteryAdcToCellCountExpectation->scale;
|
||||
currentADCReading = batteryAdcToCellCountExpectation->adcReading;
|
||||
updateBattery( );
|
||||
EXPECT_EQ(batteryAdcToCellCountExpectation->cellCount, batteryCellCount);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// STUBS
|
||||
|
||||
extern "C" {
|
||||
|
@ -90,6 +195,7 @@ extern "C" {
|
|||
uint8_t armingFlags = 0;
|
||||
int16_t rcCommand[4] = {0,0,0,0};
|
||||
|
||||
|
||||
bool feature(uint32_t mask)
|
||||
{
|
||||
UNUSED(mask);
|
||||
|
@ -106,7 +212,7 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband
|
|||
uint16_t adcGetChannel(uint8_t channel)
|
||||
{
|
||||
UNUSED(channel);
|
||||
return 0;
|
||||
return currentADCReading;
|
||||
}
|
||||
|
||||
void delay(uint32_t ms)
|
||||
|
@ -114,4 +220,17 @@ void delay(uint32_t ms)
|
|||
UNUSED(ms);
|
||||
return;
|
||||
}
|
||||
|
||||
int32_t lowpassFixed(lowpass_t *filter, int32_t in, int16_t freq)
|
||||
{
|
||||
UNUSED(filter);
|
||||
UNUSED(freq);
|
||||
return in;
|
||||
}
|
||||
|
||||
void beeper(beeperMode_e mode)
|
||||
{
|
||||
UNUSED(mode);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
@ -355,7 +355,7 @@ int16_t rcCommand[4];
|
|||
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
uint32_t rcModeActivationMask;
|
||||
|
||||
batteryState_e calculateBatteryState(void) {
|
||||
batteryState_e getBatteryState(void) {
|
||||
return BATTERY_OK;
|
||||
}
|
||||
|
||||
|
|
|
@ -255,6 +255,18 @@ protected:
|
|||
rxConfig.mincheck = DEFAULT_MIN_CHECK;
|
||||
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
|
||||
rxConfig.midrc = 1500;
|
||||
|
||||
controlRateConfig.rcRate8 = 90;
|
||||
controlRateConfig.rcExpo8 = 0;
|
||||
controlRateConfig.thrMid8 = 0;
|
||||
controlRateConfig.thrExpo8 = 0;
|
||||
controlRateConfig.rcYawExpo8 = 0;
|
||||
controlRateConfig.rates[0] = 0;
|
||||
controlRateConfig.rates[1] = 0;
|
||||
controlRateConfig.rates[2] = 0;
|
||||
controlRateConfig.dynThrPID = 0;
|
||||
controlRateConfig.tpa_breakpoint = 0;
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
@ -163,7 +163,7 @@ int32_t GPS_coord[2];
|
|||
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
|
||||
uint8_t vbat;
|
||||
uint16_t vbat;
|
||||
int16_t GPS_directionToHome; // direction to home or hol point in degrees
|
||||
|
||||
int32_t amperage;
|
||||
|
@ -232,5 +232,9 @@ portSharing_e determinePortSharing(serialPortConfig_t *, serialPortFunction_e) {
|
|||
return PORTSHARING_NOT_SHARED;
|
||||
}
|
||||
|
||||
batteryState_e getBatteryState(void) {
|
||||
return BATTERY_OK;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue