rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1989 from mikeller/make_esc_feedback_more_resilient 

Made ESC feedback and vbat calculation more resilient.
This commit is contained in:
borisbstyle 2017-01-04 13:13:16 +01:00 committed by GitHub
parent c3d3ba8443 e3644ca507
commit eb87c1c41d
19 changed files with 219 additions and 221 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/fc/fc_msp.c
View File

@ -737,7 +737,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_ANALOG:
sbufWriteU8(dst, (uint8_t)constrain(vbat, 0, 255));
sbufWriteU8(dst, (uint8_t)constrain(getVbat(), 0, 255));
sbufWriteU16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
sbufWriteU16(dst, rssi);
if(batteryConfig()->multiwiiCurrentMeterOutput) {

2
src/main/fc/serial_cli.c
View File

@ -3164,7 +3164,7 @@ static void cliStatus(char *cmdline)
cliPrintf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery - %s), CPU:%d%%\r\n",
millis() / 1000,
vbat,
getVbat(),
batteryCellCount,
getBatteryStateString(),
constrain(averageSystemLoadPercent, 0, 100)

2
src/main/io/dashboard.c
View File

@ -458,7 +458,7 @@ void showBatteryPage(void)
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
if (feature(FEATURE_VBAT)) {
tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", vbat / 10, vbat % 10, batteryCellCount);
tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", getVbat() / 10, getVbat() % 10, batteryCellCount);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);

10
src/main/io/osd.c
View File

@ -161,7 +161,7 @@ static void osdDrawSingleElement(uint8_t item)
case OSD_MAIN_BATT_VOLTAGE:
{
buff[0] = SYM_BATT_5;
sprintf(buff + 1, "%d.%1dV", vbat / 10, vbat % 10);
sprintf(buff + 1, "%d.%1dV", getVbat() / 10, getVbat() % 10);
break;
}
@ -362,7 +362,7 @@ static void osdDrawSingleElement(uint8_t item)
case OSD_POWER:
{
sprintf(buff, "%dW", amperage * vbat / 1000);
sprintf(buff, "%dW", amperage * getVbat() / 1000);
break;
}
@ -500,7 +500,7 @@ void osdUpdateAlarms(void)
else
pOsdProfile->item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG;
if (vbat <= (batteryWarningVoltage - 1))
if (getVbat() <= (batteryWarningVoltage - 1))
pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] |= BLINK_FLAG;
else
pOsdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG;
@ -556,8 +556,8 @@ static void osdUpdateStats(void)
if (stats.max_speed < value)
stats.max_speed = value;
if (stats.min_voltage > vbat)
stats.min_voltage = vbat;
if (stats.min_voltage > getVbat())
stats.min_voltage = getVbat();
value = amperage / 100;
if (stats.max_current < value)

2
src/main/rx/jetiexbus.c
View File

@ -526,7 +526,7 @@ void handleJetiExBusTelemetry(void)
}
if((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (calcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) {
jetiExSensors[EX_VOLTAGE].value = vbat;
jetiExSensors[EX_VOLTAGE].value = getVbat();
jetiExSensors[EX_CURRENT].value = amperage;
jetiExSensors[EX_ALTITUDE].value = baro.BaroAlt;
jetiExSensors[EX_CAPACITY].value = mAhDrawn;

19
src/main/sensors/battery.c
View File

@ -50,6 +50,8 @@
#define ADCVREF 3300 // in mV
#define MAX_ESC_BATTERY_AGE 10
// Battery monitoring stuff
uint8_t batteryCellCount;
uint16_t batteryWarningVoltage;
@ -85,8 +87,8 @@ static void updateBatteryVoltage(void)
#ifdef USE_ESC_SENSOR
if (feature(FEATURE_ESC_SENSOR) && batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) {
escSensorData_t escData = getEscSensorData(ESC_SENSOR_COMBINED);
vbatLatest = escData.stale ? 0 : escData.voltage / 10;
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;
}
@ -294,10 +296,10 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea
case CURRENT_SENSOR_ESC:
#ifdef USE_ESC_SENSOR
if (feature(FEATURE_ESC_SENSOR)) {
escSensorData_t escData = getEscSensorData(ESC_SENSOR_COMBINED);
if (!escData.stale) {
amperageLatest = escData.current;
mAhDrawn = escData.consumption;
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;
@ -340,3 +342,8 @@ uint8_t calculateBatteryPercentage(void)
return batteryPercentage;
}
uint16_t getVbat(void)
{
return vbat;
}

2
src/main/sensors/battery.h
View File

@ -70,7 +70,6 @@ typedef enum {
BATTERY_NOT_PRESENT
} batteryState_e;
extern uint16_t vbat;
extern uint16_t vbatRaw;
extern uint16_t vbatLatest;
extern uint8_t batteryCellCount;
@ -91,3 +90,4 @@ int32_t currentMeterToCentiamps(uint16_t src);
float calculateVbatPidCompensation(void);
uint8_t calculateBatteryPercentage(void);
uint16_t getVbat(void);

339
src/main/sensors/esc_sensor.c
View File

@ -73,19 +73,20 @@ set debug_mode = DEBUG_ESC_TELEMETRY in cli
enum {
DEBUG_ESC_MOTOR_INDEX = 0,
DEBUG_ESC_NUM_TIMEOUTS = 1,
DEBUG_ESC_TEMPERATURE = 2,
DEBUG_ESC_RPM = 3
DEBUG_ESC_NUM_CRC_ERRORS = 2,
DEBUG_ESC_DATA_AGE = 3,
};
typedef enum {
ESC_SENSOR_FRAME_PENDING = 1 << 0, // 1
ESC_SENSOR_FRAME_COMPLETE = 1 << 1 // 2
ESC_SENSOR_FRAME_PENDING = 0,
ESC_SENSOR_FRAME_COMPLETE = 1,
ESC_SENSOR_FRAME_FAILED = 2
} escTlmFrameState_t;
typedef enum {
ESC_SENSOR_TRIGGER_WAIT = 0,
ESC_SENSOR_TRIGGER_READY = 1 << 0, // 1
ESC_SENSOR_TRIGGER_PENDING = 1 << 1, // 2
ESC_SENSOR_TRIGGER_STARTUP = 0,
ESC_SENSOR_TRIGGER_READY = 1,
ESC_SENSOR_TRIGGER_PENDING = 2
} escSensorTriggerState_t;
#define ESC_SENSOR_BAUDRATE 115200
@ -96,71 +97,79 @@ typedef enum {
static bool tlmFrameDone = false;
static uint8_t tlm[ESC_SENSOR_BUFFSIZE] = { 0, };
static uint8_t tlmFramePosition = 0;
static serialPort_t *escSensorPort = NULL;
static escSensorData_t escSensorData[MAX_SUPPORTED_MOTORS];
static uint32_t escTriggerTimestamp = -1;
static uint32_t escLastResponseTimestamp;
static uint8_t timeoutRetryCount = 0;
static uint8_t totalRetryCount = 0;
static escSensorTriggerState_t escSensorTriggerState = ESC_SENSOR_TRIGGER_STARTUP;
static uint32_t escTriggerTimestamp;
static uint8_t escSensorMotor = 0; // motor index
static bool escSensorEnabled = false;
static escSensorTriggerState_t escSensorTriggerState = ESC_SENSOR_TRIGGER_WAIT;
static void escSensorDataReceive(uint16_t c);
static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed);
static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen);
static void selectNextMotor(void);
static escSensorData_t combinedEscSensorData;
static bool combinedDataNeedsUpdate = true;
static uint16_t totalTimeoutCount = 0;
static uint16_t totalCrcErrorCount = 0;
bool isEscSensorActive(void)
{
return escSensorEnabled;
return escSensorPort != NULL;
}
escSensorData_t getEscSensorData(uint8_t motorNumber)
escSensorData_t *getEscSensorData(uint8_t motorNumber)
{
if (motorNumber < getMotorCount()) {
return escSensorData[motorNumber];
}
return &escSensorData[motorNumber];
} else if (motorNumber == ESC_SENSOR_COMBINED) {
if (combinedDataNeedsUpdate) {
combinedEscSensorData.dataAge = 0;
combinedEscSensorData.temperature = 0;
combinedEscSensorData.voltage = 0;
combinedEscSensorData.current = 0;
combinedEscSensorData.consumption = 0;
combinedEscSensorData.rpm = 0;
escSensorData_t combinedEscSensorData = {
.stale = true,
.temperature = 0,
.voltage = 0,
.current = 0,
.consumption = 0,
.rpm = 0
};
if (motorNumber == ESC_SENSOR_COMBINED) {
unsigned int activeSensors = 0;
for (int i = 0; i < getMotorCount(); i = i + 1) {
if (!escSensorData[i].stale) {
combinedEscSensorData.dataAge = MAX(combinedEscSensorData.dataAge, escSensorData[i].dataAge);
combinedEscSensorData.temperature = MAX(combinedEscSensorData.temperature, escSensorData[i].temperature);
combinedEscSensorData.voltage += escSensorData[i].voltage;
combinedEscSensorData.current += escSensorData[i].current;
combinedEscSensorData.consumption += escSensorData[i].consumption;
combinedEscSensorData.rpm += escSensorData[i].rpm;
activeSensors = activeSensors + 1;
}
}
if (activeSensors > 0) {
combinedEscSensorData.stale = false;
combinedEscSensorData.voltage = combinedEscSensorData.voltage / activeSensors;
combinedEscSensorData.rpm = combinedEscSensorData.rpm / activeSensors;
combinedEscSensorData.voltage = combinedEscSensorData.voltage / getMotorCount();
combinedEscSensorData.rpm = combinedEscSensorData.rpm / getMotorCount();
DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_TEMPERATURE, combinedEscSensorData.temperature);
DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_RPM, combinedEscSensorData.rpm);
}
combinedDataNeedsUpdate = false;
DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_DATA_AGE, combinedEscSensorData.dataAge);
}
return combinedEscSensorData;
return &combinedEscSensorData;
} else {
return NULL;
}
}
static void resetEscSensorData(void)
// Receive ISR callback
static void escSensorDataReceive(uint16_t c)
{
for (int i; i < MAX_SUPPORTED_MOTORS; i = i + 1) {
escSensorData[i].stale = true;
// KISS ESC sends some data during startup, ignore this for now (maybe future use)
// startup data could be firmware version and serialnumber
if (escSensorTriggerState == ESC_SENSOR_TRIGGER_STARTUP || tlmFrameDone) {
return;
}
tlm[tlmFramePosition] = (uint8_t)c;
if (tlmFramePosition == ESC_SENSOR_BUFFSIZE - 1) {
tlmFrameDone = true;
tlmFramePosition = 0;
} else {
tlmFramePosition++;
}
}
@ -176,143 +185,13 @@ bool escSensorInit(void)
// Initialize serial port
escSensorPort = openSerialPort(portConfig->identifier, FUNCTION_ESC_SENSOR, escSensorDataReceive, ESC_SENSOR_BAUDRATE, MODE_RX, options);
if (escSensorPort) {
escSensorEnabled = true;
for (int i; i < MAX_SUPPORTED_MOTORS; i = i + 1) {
escSensorData[i].dataAge = ESC_DATA_INVALID;
}
resetEscSensorData();
return escSensorPort != NULL;
}
static void freeEscSensorPort(void)
{
closeSerialPort(escSensorPort);
escSensorPort = NULL;
escSensorEnabled = false;
}
// Receive ISR callback
static void escSensorDataReceive(uint16_t c)
{
// KISS ESC sends some data during startup, ignore this for now (maybe future use)
// startup data could be firmware version and serialnumber
if (escSensorTriggerState == ESC_SENSOR_TRIGGER_WAIT) return;
tlm[tlmFramePosition] = (uint8_t)c;
if (tlmFramePosition == ESC_SENSOR_BUFFSIZE - 1) {
tlmFrameDone = true;
tlmFramePosition = 0;
} else {
tlmFramePosition++;
}
}
static uint8_t escSensorFrameStatus(void)
{
uint8_t frameStatus = ESC_SENSOR_FRAME_PENDING;
uint16_t chksum, tlmsum;
if (!tlmFrameDone) {
return frameStatus;
}
tlmFrameDone = false;
// Get CRC8 checksum
chksum = get_crc8(tlm, ESC_SENSOR_BUFFSIZE - 1);
tlmsum = tlm[ESC_SENSOR_BUFFSIZE - 1]; // last byte contains CRC value
if (chksum == tlmsum) {
escSensorData[escSensorMotor].stale = false;
escSensorData[escSensorMotor].temperature = tlm[0];
escSensorData[escSensorMotor].voltage = tlm[1] << 8 | tlm[2];
escSensorData[escSensorMotor].current = tlm[3] << 8 | tlm[4];
escSensorData[escSensorMotor].consumption = tlm[5] << 8 | tlm[6];
escSensorData[escSensorMotor].rpm = tlm[7] << 8 | tlm[8];
frameStatus = ESC_SENSOR_FRAME_COMPLETE;
}
return frameStatus;
}
void escSensorProcess(timeUs_t currentTimeUs)
{
const timeMs_t currentTimeMs = currentTimeUs / 1000;
if (!escSensorEnabled) {
return;
}
// Wait period of time before requesting telemetry (let the system boot first)
if (currentTimeMs < ESC_BOOTTIME) {
return;
}
else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_WAIT) {
// Ready for starting requesting telemetry
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
escSensorMotor = 0;
escTriggerTimestamp = currentTimeMs;
escLastResponseTimestamp = escTriggerTimestamp;
}
else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_READY) {
DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_MOTOR_INDEX, escSensorMotor + 1);
motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor);
motor->requestTelemetry = true;
escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING;
}
else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_PENDING) {
if (escTriggerTimestamp + ESC_REQUEST_TIMEOUT < currentTimeMs) {
// ESC did not repond in time, retry
timeoutRetryCount++;
escTriggerTimestamp = currentTimeMs;
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
if (timeoutRetryCount == 4) {
// Not responding after 3 times, skip motor
escSensorData[escSensorMotor].stale = true;
selectNextMotor();
}
DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_NUM_TIMEOUTS, ++totalRetryCount);
}
// Get received frame status
uint8_t state = escSensorFrameStatus();
if (state == ESC_SENSOR_FRAME_COMPLETE) {
selectNextMotor();
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
escLastResponseTimestamp = currentTimeMs;
}
}
if (escLastResponseTimestamp + 10000 < currentTimeMs) {
// ESCs did not respond for 10 seconds
// Disable ESC telemetry and reset voltage and current to let the use know something is wrong
freeEscSensorPort();
resetEscSensorData();
}
}
static void selectNextMotor(void)
{
escSensorMotor++;
if (escSensorMotor == getMotorCount()) {
escSensorMotor = 0;
}
timeoutRetryCount = 0;
escTriggerTimestamp = millis();
}
//-- CRC
static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed)
{
uint8_t crc_u = crc;
@ -332,4 +211,110 @@ static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen)
return (crc);
}
static uint8_t decodeEscFrame(void)
{
if (!tlmFrameDone) {
return ESC_SENSOR_FRAME_PENDING;
}
// Get CRC8 checksum
uint16_t chksum = get_crc8(tlm, ESC_SENSOR_BUFFSIZE - 1);
uint16_t tlmsum = tlm[ESC_SENSOR_BUFFSIZE - 1]; // last byte contains CRC value
uint8_t frameStatus;
if (chksum == tlmsum) {
escSensorData[escSensorMotor].dataAge = 0;
escSensorData[escSensorMotor].temperature = tlm[0];
escSensorData[escSensorMotor].voltage = tlm[1] << 8 | tlm[2];
escSensorData[escSensorMotor].current = tlm[3] << 8 | tlm[4];
escSensorData[escSensorMotor].consumption = tlm[5] << 8 | tlm[6];
escSensorData[escSensorMotor].rpm = tlm[7] << 8 | tlm[8];
combinedDataNeedsUpdate = true;
frameStatus = ESC_SENSOR_FRAME_COMPLETE;
} else {
frameStatus = ESC_SENSOR_FRAME_FAILED;
}
tlmFrameDone = false;
return frameStatus;
}
static void increaseDataAge(void)
{
if (escSensorData[escSensorMotor].dataAge < ESC_DATA_INVALID) {
escSensorData[escSensorMotor].dataAge++;
combinedDataNeedsUpdate = true;
}
}
static void selectNextMotor(void)
{
escSensorMotor++;
if (escSensorMotor == getMotorCount()) {
escSensorMotor = 0;
}
}
void escSensorProcess(timeUs_t currentTimeUs)
{
const timeMs_t currentTimeMs = currentTimeUs / 1000;
if (!escSensorPort) {
return;
}
switch (escSensorTriggerState) {
case ESC_SENSOR_TRIGGER_STARTUP:
// Wait period of time before requesting telemetry (let the system boot first)
if (currentTimeMs >= ESC_BOOTTIME) {
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
}
break;
case ESC_SENSOR_TRIGGER_READY:
escTriggerTimestamp = currentTimeMs;
motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor);
motor->requestTelemetry = true;
escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING;
DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_MOTOR_INDEX, escSensorMotor + 1);
break;
case ESC_SENSOR_TRIGGER_PENDING:
if (currentTimeMs < escTriggerTimestamp + ESC_REQUEST_TIMEOUT) {
uint8_t state = decodeEscFrame();
switch (state) {
case ESC_SENSOR_FRAME_COMPLETE:
selectNextMotor();
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
break;
case ESC_SENSOR_FRAME_FAILED:
increaseDataAge();
selectNextMotor();
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_NUM_CRC_ERRORS, ++totalCrcErrorCount);
break;
case ESC_SENSOR_FRAME_PENDING:
break;
}
} else {
// Move on to next ESC, we'll come back to this one
increaseDataAge();
selectNextMotor();
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_NUM_TIMEOUTS, ++totalTimeoutCount);
}
break;
}
}
#endif

6
src/main/sensors/esc_sensor.h
View File

@ -20,7 +20,7 @@
#include "common/time.h"
typedef struct {
bool stale;
uint8_t dataAge;
int8_t temperature;
int16_t voltage;
int16_t current;
@ -28,10 +28,12 @@ typedef struct {
int16_t rpm;
} escSensorData_t;
#define ESC_DATA_INVALID 255
bool escSensorInit(void);
void escSensorProcess(timeUs_t currentTime);
#define ESC_SENSOR_COMBINED 255
escSensorData_t getEscSensorData(uint8_t motorNumber);
escSensorData_t *getEscSensorData(uint8_t motorNumber);

2
src/main/target/COLIBRI_RACE/i2c_bst.c
View File

@ -677,7 +677,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
#endif
break;
case BST_ANALOG:
bstWrite8((uint8_t)constrain(vbat, 0, 255));
bstWrite8((uint8_t)constrain(getVbat(), 0, 255));
bstWrite16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
bstWrite16(rssi);
if(batteryConfig()->multiwiiCurrentMeterOutput) {

2
src/main/telemetry/crsf.c
View File

@ -175,7 +175,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, vbat); // vbat is in units of 0.1V
crsfSerialize16(dst, getVbat()); // 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

14
src/main/telemetry/frsky.c
View File

@ -205,8 +205,8 @@ static void sendThrottleOrBatterySizeAsRpm(rxConfig_t *rxConfig, uint16_t deadba
UNUSED(rxConfig);
UNUSED(deadband3d_throttle);
escSensorData_t escData = getEscSensorData(ESC_SENSOR_COMBINED);
serialize16(escData.stale ? 0 : escData.rpm);
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
serialize16(escData->dataAge < ESC_DATA_INVALID ? escData->rpm : 0);
#else
if (ARMING_FLAG(ARMED)) {
throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle);
@ -224,8 +224,8 @@ static void sendTemperature1(void)
{
sendDataHead(ID_TEMPRATURE1);
#if defined(USE_ESC_SENSOR)
escSensorData_t escData = getEscSensorData(ESC_SENSOR_COMBINED);
serialize16(escData.stale ? 0 : escData.temperature);
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
serialize16(escData->dataAge < ESC_DATA_INVALID ? escData->temperature : 0);
#elif defined(BARO)
serialize16((baro.baroTemperature + 50)/ 100); //Airmamaf
#else
@ -388,7 +388,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)vbat * 100 + batteryCellCount) / (batteryCellCount * 2);
cellVoltage = ((uint32_t)getVbat() * 100 + batteryCellCount) / (batteryCellCount * 2);
// Cell number is at bit 9-12
payload = (currentCell << 4);
@ -416,9 +416,9 @@ static void sendVoltageAmp(void)
* Use new ID 0x39 to send voltage directly in 0.1 volts resolution
*/
sendDataHead(ID_VOLTAGE_AMP);
serialize16(vbat);
serialize16(getVbat());
} else {
uint16_t voltage = (vbat * 110) / 21;
uint16_t voltage = (getVbat() * 110) / 21;
uint16_t vfasVoltage;
if (telemetryConfig->frsky_vfas_cell_voltage) {
vfasVoltage = voltage / batteryCellCount;

8
src/main/telemetry/hott.c
View File

@ -242,10 +242,10 @@ static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage)
static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage)
{
hottEAMMessage->main_voltage_L = vbat & 0xFF;
hottEAMMessage->main_voltage_H = vbat >> 8;
hottEAMMessage->batt1_voltage_L = vbat & 0xFF;
hottEAMMessage->batt1_voltage_H = vbat >> 8;
hottEAMMessage->main_voltage_L = getVbat() & 0xFF;
hottEAMMessage->main_voltage_H = getVbat() >> 8;
hottEAMMessage->batt1_voltage_L = getVbat() & 0xFF;
hottEAMMessage->batt1_voltage_H = getVbat() >> 8;
updateAlarmBatteryStatus(hottEAMMessage);
}

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(vbat * 100); //vbat converted to mv
ltm_serialise_16(getVbat() * 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

2
src/main/telemetry/mavlink.c
View File

@ -223,7 +223,7 @@ 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) ? vbat * 100 : 0,
feature(FEATURE_VBAT) ? getVbat() * 100 : 0,
// current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
feature(FEATURE_VBAT) ? amperage : -1,
// battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery

6
src/main/telemetry/smartport.c
View File

@ -639,9 +639,9 @@ void handleSmartPortTelemetry(void)
if (feature(FEATURE_VBAT) && batteryCellCount > 0) {
uint16_t vfasVoltage;
if (telemetryConfig->frsky_vfas_cell_voltage) {
vfasVoltage = vbat / batteryCellCount;
vfasVoltage = getVbat() / batteryCellCount;
} else {
vfasVoltage = vbat;
vfasVoltage = getVbat();
}
smartPortSendPackage(id, vfasVoltage * 10); // given in 0.1V, convert to volts
smartPortHasRequest = 0;
@ -810,7 +810,7 @@ void handleSmartPortTelemetry(void)
#endif
case FSSP_DATAID_A4 :
if (feature(FEATURE_VBAT) && batteryCellCount > 0) {
smartPortSendPackage(id, vbat * 10 / batteryCellCount ); // given in 0.1V, convert to volts
smartPortSendPackage(id, getVbat() * 10 / batteryCellCount ); // given in 0.1V, convert to volts
smartPortHasRequest = 0;
}
break;

6
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, vbat * 10); // vbat is in units of 0.1V
srxlSerialize16(dst, getVbat() * 10); // vbat is in units of 0.1V
srxlSerialize16(dst, 0x7FFF); // temperature
srxlSerialize8(dst, 0xFF); // dbmA
srxlSerialize8(dst, 0xFF); // dbmB
@ -200,8 +200,8 @@ void srxlFramePowerBox(sbuf_t *dst)
{
srxlSerialize8(dst, SRXL_FRAMETYPE_POWERBOX);
srxlSerialize8(dst, SRXL_FRAMETYPE_SID);
srxlSerialize16(dst, vbat * 10); // vbat is in units of 0.1V - vbat1
srxlSerialize16(dst, vbat * 10); // vbat is in units of 0.1V - vbat2
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, 0xFFFF);

4
src/test/unit/telemetry_crsf_unittest.cc
View File

@ -54,6 +54,7 @@ extern "C" {
#include "flight/gps_conversion.h"
bool airMode;
uint16_t vbat;
serialPort_t *telemetrySharedPort;
}
@ -282,7 +283,6 @@ uint16_t GPS_distanceToHome; // distance to home point in meters
uint16_t GPS_altitude; // altitude in m
uint16_t GPS_speed; // speed in 0.1m/s
uint16_t GPS_ground_course = 0; // degrees * 10
uint16_t vbat;
int32_t amperage;
int32_t mAhDrawn;
@ -314,6 +314,6 @@ 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; }
}

6
src/test/unit/telemetry_hott_unittest.cc
View File

@ -165,8 +165,8 @@ 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
uint16_t vbat;
int16_t GPS_directionToHome; // direction to home or hol point in degrees
uint16_t vbat;
int32_t amperage;
int32_t mAhDrawn;
@ -256,5 +256,9 @@ batteryState_e getBatteryState(void)
return BATTERY_OK;
}
uint16_t getVbat(void)
{
return vbat;
}
}