diff --git a/src/frsky.cpp b/src/frsky.cpp index 1643578ef..d0f046c8e 100644 --- a/src/frsky.cpp +++ b/src/frsky.cpp @@ -254,14 +254,18 @@ void parseTelemHubByte(uint8_t byte) case offsetof(FrskyHubData, baroAltitude_ap): { int16_t actVario = frskyHubData.baroAltitude_bp - frskyHubData.lastBaroAltitude_bp; - actVario *= 100; - actVario += frskyHubData.baroAltitude_ap - frskyHubData.lastBaroAltitude_ap; - frskyHubData.varioSpeed = frskyHubData.varioSpeed - frskyHubData.varioQueue[frskyHubData.queuePointer] + actVario; + frskyHubData.varioAcc2 = frskyHubData.varioAcc2 - frskyHubData.varioQueue[frskyHubData.queuePointer]; frskyHubData.varioQueue[frskyHubData.queuePointer] = actVario; - if (++frskyHubData.queuePointer > 4) + uint8_t tmp = frskyHubData.queuePointer + 5; + if (tmp >= 10) + tmp -= 10; + tmp = (uint8_t)frskyHubData.varioQueue[tmp]; + frskyHubData.varioAcc2 = frskyHubData.varioAcc2 + (int8_t)tmp; + frskyHubData.varioAcc1 = frskyHubData.varioAcc1 + actVario - (int8_t)tmp; + frskyHubData.varioSpeed = frskyHubData.varioAcc2 - frskyHubData.varioAcc1; + if (++frskyHubData.queuePointer >= 10) frskyHubData.queuePointer=0; frskyHubData.lastBaroAltitude_bp = frskyHubData.baroAltitude_bp; - frskyHubData.lastBaroAltitude_ap = frskyHubData.baroAltitude_ap; break; } diff --git a/src/frsky.h b/src/frsky.h index c17dff0fd..499fc772a 100644 --- a/src/frsky.h +++ b/src/frsky.h @@ -105,15 +105,16 @@ PACK(struct FrskyHubData { int16_t accelZ; // 0x26 1/256th gram (-8g ~ +8g) uint8_t gpsDistNeeded:1; // 1bits out of 16bits spare reused int8_t gpsFix:2; // 2bits out of 16bits spare reused: -1=never fixed, 0=not fixed now, 1=fixed - uint8_t spare:1; // + uint8_t spare1:1; // uint8_t cellsCount:4; // 4bits out of 16bits spare reused uint8_t minCellVolts; // 8bits out of 16bits spare reused uint16_t current; // 0x28 Current - int16_t varioQueue[5]; // circular-buffer + int8_t varioQueue[10]; // circular-buffer uint8_t queuePointer; // circular-buffer pointer - int8_t lastBaroAltitude_ap; + int8_t spare2; int16_t lastBaroAltitude_bp; int16_t varioSpeed; + /* next fields must keep this order! */ int16_t minAltitude; int16_t maxAltitude; uint16_t maxRpm; @@ -121,12 +122,16 @@ PACK(struct FrskyHubData { int16_t maxTemperature2; uint16_t maxGpsSpeed; uint16_t maxGpsDistance; - uint16_t gpsDistance; - int16_t gpsAltitudeOffset; + /* end */ + int16_t varioAcc1; + int16_t varioAcc2; uint16_t volts_bp; // 0x3A uint16_t volts_ap; // 0x3B // end of FrSky Hub data + uint16_t gpsDistance; + int16_t gpsAltitudeOffset; uint8_t minCellMinVolts; + }); #elif defined(WS_HOW_HIGH)