diff --git a/companion/src/simulation/simulatorimport.h b/companion/src/simulation/simulatorimport.h index 0a8d7b08f..5bf52b265 100644 --- a/companion/src/simulation/simulatorimport.h +++ b/companion/src/simulation/simulatorimport.h @@ -23,7 +23,7 @@ #ifdef INIT_IMPORT #undef INIT_IMPORT #ifdef FRSKY -frskyStreaming = 20; +telemetryStreaming = 20; #endif #endif diff --git a/radio/src/CMakeLists.txt b/radio/src/CMakeLists.txt index 4c7bddf22..31dfafc44 100644 --- a/radio/src/CMakeLists.txt +++ b/radio/src/CMakeLists.txt @@ -512,6 +512,8 @@ if(ARCH STREQUAL ARM) tasks_arm.cpp audio_arm.cpp telemetry/telemetry.cpp + telemetry/telemetry_holders.cpp + telemetry/telemetry_sensors.cpp telemetry/frsky.cpp telemetry/frsky_d_arm.cpp telemetry/frsky_sport.cpp @@ -637,7 +639,7 @@ else() option(GPS "GPS support" ON) option(VARIO "Vario support" ON) add_definitions(-DFRSKY) - set(SRC ${SRC} telemetry/frsky.cpp telemetry/frsky_d.cpp) + set(SRC ${SRC} telemetry/telemetry.cpp telemetry/telemetry_holders.cpp telemetry/frsky.cpp telemetry/frsky_d.cpp) set(FIRMWARE_SRC ${FIRMWARE_SRC} targets/common_avr/telemetry_driver.cpp) set(GUI_SRC ${GUI_SRC} view_telemetry.cpp) if(FRSKY_HUB) diff --git a/radio/src/gui/9x/menu_model_telemetry.cpp b/radio/src/gui/9x/menu_model_telemetry.cpp index 53aa9d29d..8d26418ed 100644 --- a/radio/src/gui/9x/menu_model_telemetry.cpp +++ b/radio/src/gui/9x/menu_model_telemetry.cpp @@ -609,7 +609,7 @@ void menuModelTelemetry(uint8_t event) case ITEM_TELEMETRY_A2_LABEL: lcd_putsLeft(y, STR_ACHANNEL); lcdDrawNumber(2*FW, y, ch+1, 0); - putsTelemetryChannelValue(TELEM_COL2+6*FW, y, dest, frskyData.analog[ch].value, LEFT); + putsTelemetryChannelValue(TELEM_COL2+6*FW, y, dest, telemetryData.analog[ch].value, LEFT); break; case ITEM_TELEMETRY_A1_RANGE: @@ -739,7 +739,7 @@ void menuModelTelemetry(uint8_t event) case ITEM_TELEMETRY_FAS_OFFSET: lcd_putsLeft(y, STR_FAS_OFFSET); lcdDrawNumber(TELEM_COL2, y, g_model.frsky.fasOffset, attr|LEFT|PREC1); - lcdDrawNumber(TELEM_COL2+6*FW, y, frskyData.hub.current, LEFT|PREC1); + lcdDrawNumber(TELEM_COL2+6*FW, y, telemetryData.hub.current, LEFT|PREC1); lcdDrawChar(TELEM_COL2+8*FW, y, 'A'); if (attr) g_model.frsky.fasOffset = checkIncDec(event, g_model.frsky.fasOffset, -120, 120, EE_MODEL); break; diff --git a/radio/src/gui/9x/view_telemetry.cpp b/radio/src/gui/9x/view_telemetry.cpp index 805813d72..816bc6d90 100644 --- a/radio/src/gui/9x/view_telemetry.cpp +++ b/radio/src/gui/9x/view_telemetry.cpp @@ -38,7 +38,7 @@ void displayRssiLine() lcdDrawSolidHorizontalLine(0, 55, 128, 0); // separator uint8_t rssi; #if !defined(CPUARM) - rssi = min((uint8_t)99, frskyData.rssi[1].value); + rssi = min((uint8_t)99, telemetryData.rssi[1].value); lcd_putsLeft(STATUS_BAR_Y, STR_TX); lcdDrawNumber(4*FW+1, STATUS_BAR_Y, rssi, LEADING0, 2); lcdDrawRect(BAR_LEFT+1, 57, 38, 7); lcdDrawFilledRect(BAR_LEFT+1, 58, 4*rssi/11, 5, (rssi < getRssiAlarmValue(0)) ? DOTTED : SOLID); @@ -59,17 +59,17 @@ void displayRssiLine() void displayGpsTime() { uint8_t att = (TELEMETRY_STREAMING() ? LEFT|LEADING0 : LEFT|LEADING0|BLINK); - lcdDrawNumber(CENTER_OFS+6*FW+7, STATUS_BAR_Y, frskyData.hub.hour, att, 2); + lcdDrawNumber(CENTER_OFS+6*FW+7, STATUS_BAR_Y, telemetryData.hub.hour, att, 2); lcdDrawChar(CENTER_OFS+8*FW+4, STATUS_BAR_Y, ':', att); - lcdDrawNumber(CENTER_OFS+9*FW+2, STATUS_BAR_Y, frskyData.hub.min, att, 2); + lcdDrawNumber(CENTER_OFS+9*FW+2, STATUS_BAR_Y, telemetryData.hub.min, att, 2); lcdDrawChar(CENTER_OFS+11*FW-1, STATUS_BAR_Y, ':', att); - lcdDrawNumber(CENTER_OFS+12*FW-3, STATUS_BAR_Y, frskyData.hub.sec, att, 2); + lcdDrawNumber(CENTER_OFS+12*FW-3, STATUS_BAR_Y, telemetryData.hub.sec, att, 2); lcdInvertLastLine(); } void displayGpsCoord(uint8_t y, char direction, int16_t bp, int16_t ap) { - if (frskyData.hub.gpsFix >= 0) { + if (telemetryData.hub.gpsFix >= 0) { if (!direction) direction = '-'; lcdDrawNumber(TELEM_2ND_COLUMN, y, bp / 100, LEFT); // ddd before '.' lcdDrawChar(lcdLastPos, y, '@'); @@ -107,9 +107,9 @@ void displayVoltageScreenLine(uint8_t y, uint8_t index) { drawStringWithIndex(0, y, STR_A, index+1, 0); if (TELEMETRY_STREAMING()) { - putsTelemetryChannelValue(3*FW+6*FW+4, y-FH, index+TELEM_A1-1, frskyData.analog[index].value, DBLSIZE); - lcdDrawChar(12*FW-1, y-FH, '<'); putsTelemetryChannelValue(17*FW, y-FH, index+TELEM_A1-1, frskyData.analog[index].min, NO_UNIT); - lcdDrawChar(12*FW, y, '>'); putsTelemetryChannelValue(17*FW, y, index+TELEM_A1-1, frskyData.analog[index].max, NO_UNIT); + putsTelemetryChannelValue(3*FW+6*FW+4, y-FH, index+TELEM_A1-1, telemetryData.analog[index].value, DBLSIZE); + lcdDrawChar(12*FW-1, y-FH, '<'); putsTelemetryChannelValue(17*FW, y-FH, index+TELEM_A1-1, telemetryData.analog[index].min, NO_UNIT); + lcdDrawChar(12*FW, y, '>'); putsTelemetryChannelValue(17*FW, y, index+TELEM_A1-1, telemetryData.analog[index].max, NO_UNIT); } } #endif @@ -142,10 +142,10 @@ void displayVoltagesScreen() break; #if defined(FRSKY_HUB) case FRSKY_VOLTS_SOURCE_FAS: - putsTelemetryChannelValue(3*FW+6*FW+4, FH, TELEM_VFAS-1, frskyData.hub.vfas, DBLSIZE); + putsTelemetryChannelValue(3*FW+6*FW+4, FH, TELEM_VFAS-1, telemetryData.hub.vfas, DBLSIZE); break; case FRSKY_VOLTS_SOURCE_CELLS: - putsTelemetryChannelValue(3*FW+6*FW+4, FH, TELEM_CELLS_SUM-1, frskyData.hub.cellsSum, DBLSIZE); + putsTelemetryChannelValue(3*FW+6*FW+4, FH, TELEM_CELLS_SUM-1, telemetryData.hub.cellsSum, DBLSIZE); break; #endif } @@ -159,13 +159,13 @@ void displayVoltagesScreen() break; #if defined(FRSKY_HUB) case FRSKY_CURRENT_SOURCE_FAS: - putsTelemetryChannelValue(3*FW+6*FW+4, 3*FH, TELEM_CURRENT-1, frskyData.hub.current, DBLSIZE); + putsTelemetryChannelValue(3*FW+6*FW+4, 3*FH, TELEM_CURRENT-1, telemetryData.hub.current, DBLSIZE); break; #endif } - putsTelemetryChannelValue(4, 5*FH, TELEM_POWER-1, frskyData.hub.power, LEFT|DBLSIZE); - putsTelemetryChannelValue(3*FW+4+4*FW+6*FW+FW, 5*FH, TELEM_CONSUMPTION-1, frskyData.hub.currentConsumption, DBLSIZE); + putsTelemetryChannelValue(4, 5*FH, TELEM_POWER-1, telemetryData.hub.power, LEFT|DBLSIZE); + putsTelemetryChannelValue(3*FW+4+4*FW+6*FW+FW, 5*FH, TELEM_CONSUMPTION-1, telemetryData.hub.currentConsumption, DBLSIZE); } else { displayVoltageScreenLine(analog > 0 ? 5*FH : 4*FH, analog ? 2-analog : 0); @@ -174,11 +174,11 @@ void displayVoltagesScreen() #if defined(FRSKY_HUB) // Cells voltage - if (frskyData.hub.cellsCount > 0) { + if (telemetryData.hub.cellsCount > 0) { uint8_t y = 1*FH; - for (uint8_t k=0; k frskyData.hub.pilotLatitude) ? lat - frskyData.hub.pilotLatitude : frskyData.hub.pilotLatitude - lat; + uint32_t angle = (lat > telemetryData.hub.pilotLatitude) ? lat - telemetryData.hub.pilotLatitude : telemetryData.hub.pilotLatitude - lat; uint32_t dist = EARTH_RADIUS * angle / 1000000; uint32_t result = dist*dist; - angle = (lng > frskyData.hub.pilotLongitude) ? lng - frskyData.hub.pilotLongitude : frskyData.hub.pilotLongitude - lng; - dist = frskyData.hub.distFromEarthAxis * angle / 1000000; + angle = (lng > telemetryData.hub.pilotLongitude) ? lng - telemetryData.hub.pilotLongitude : telemetryData.hub.pilotLongitude - lng; + dist = telemetryData.hub.distFromEarthAxis * angle / 1000000; result += dist*dist; dist = abs(TELEMETRY_BARO_ALT_AVAILABLE() ? TELEMETRY_RELATIVE_BARO_ALT_BP : TELEMETRY_RELATIVE_GPS_ALT_BP); result += dist*dist; - frskyData.hub.gpsDistance = isqrt32(result); - if (frskyData.hub.gpsDistance > frskyData.hub.maxGpsDistance) - frskyData.hub.maxGpsDistance = frskyData.hub.gpsDistance; + telemetryData.hub.gpsDistance = isqrt32(result); + if (telemetryData.hub.gpsDistance > telemetryData.hub.maxGpsDistance) + telemetryData.hub.maxGpsDistance = telemetryData.hub.gpsDistance; } #endif diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp index 0aa5b9f17..2fdb56b87 100644 --- a/radio/src/mixer.cpp +++ b/radio/src/mixer.cpp @@ -370,39 +370,39 @@ getvalue_t getValue(mixsrc_t i) } } #elif defined(FRSKY) - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_RSSI_TX) return frskyData.rssi[1].value; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_RSSI_RX) return frskyData.rssi[0].value; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_A1) return frskyData.analog[TELEM_ANA_A1].value; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_A2) return frskyData.analog[TELEM_ANA_A2].value; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_RSSI_TX) return telemetryData.rssi[1].value; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_RSSI_RX) return telemetryData.rssi[0].value; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_A1) return telemetryData.analog[TELEM_ANA_A1].value; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_A2) return telemetryData.analog[TELEM_ANA_A2].value; #if defined(FRSKY_SPORT) - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ALT) return frskyData.hub.baroAltitude; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ALT) return telemetryData.hub.baroAltitude; #elif defined(FRSKY_HUB) || defined(WS_HOW_HIGH) else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ALT) return TELEMETRY_RELATIVE_BARO_ALT_BP; #endif #if defined(FRSKY_HUB) - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_RPM) return frskyData.hub.rpm; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_FUEL) return frskyData.hub.fuelLevel; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_T1) return frskyData.hub.temperature1; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_T2) return frskyData.hub.temperature2; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_RPM) return telemetryData.hub.rpm; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_FUEL) return telemetryData.hub.fuelLevel; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_T1) return telemetryData.hub.temperature1; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_T2) return telemetryData.hub.temperature2; else if (i==MIXSRC_FIRST_TELEM-1+TELEM_SPEED) return TELEMETRY_GPS_SPEED_BP; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_DIST) return frskyData.hub.gpsDistance; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_DIST) return telemetryData.hub.gpsDistance; else if (i==MIXSRC_FIRST_TELEM-1+TELEM_GPSALT) return TELEMETRY_RELATIVE_GPS_ALT_BP; else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CELL) return (int16_t)TELEMETRY_MIN_CELL_VOLTAGE; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CELLS_SUM) return (int16_t)frskyData.hub.cellsSum; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_VFAS) return (int16_t)frskyData.hub.vfas; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CURRENT) return (int16_t)frskyData.hub.current; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CONSUMPTION) return frskyData.hub.currentConsumption; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_POWER) return frskyData.hub.power; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ACCx) return frskyData.hub.accelX; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ACCy) return frskyData.hub.accelY; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ACCz) return frskyData.hub.accelZ; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_HDG) return frskyData.hub.gpsCourse_bp; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_VSPEED) return frskyData.hub.varioSpeed; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ASPEED) return frskyData.hub.airSpeed; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_DTE) return frskyData.hub.dTE; - else if (i<=MIXSRC_FIRST_TELEM-1+TELEM_MIN_A1) return frskyData.analog[TELEM_ANA_A1].min; - else if (i==MIXSRC_FIRST_TELEM-1+TELEM_MIN_A2) return frskyData.analog[TELEM_ANA_A2].min; - else if (i<=MIXSRC_FIRST_TELEM-1+TELEM_CSW_MAX) return *(((int16_t*)(&frskyData.hub.minAltitude))+i-(MIXSRC_FIRST_TELEM-1+TELEM_MIN_ALT)); + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CELLS_SUM) return (int16_t)telemetryData.hub.cellsSum; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_VFAS) return (int16_t)telemetryData.hub.vfas; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CURRENT) return (int16_t)telemetryData.hub.current; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CONSUMPTION) return telemetryData.hub.currentConsumption; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_POWER) return telemetryData.hub.power; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ACCx) return telemetryData.hub.accelX; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ACCy) return telemetryData.hub.accelY; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ACCz) return telemetryData.hub.accelZ; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_HDG) return telemetryData.hub.gpsCourse_bp; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_VSPEED) return telemetryData.hub.varioSpeed; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ASPEED) return telemetryData.hub.airSpeed; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_DTE) return telemetryData.hub.dTE; + else if (i<=MIXSRC_FIRST_TELEM-1+TELEM_MIN_A1) return telemetryData.analog[TELEM_ANA_A1].min; + else if (i==MIXSRC_FIRST_TELEM-1+TELEM_MIN_A2) return telemetryData.analog[TELEM_ANA_A2].min; + else if (i<=MIXSRC_FIRST_TELEM-1+TELEM_CSW_MAX) return *(((int16_t*)(&telemetryData.hub.minAltitude))+i-(MIXSRC_FIRST_TELEM-1+TELEM_MIN_ALT)); #endif #endif else return 0; diff --git a/radio/src/opentx.h b/radio/src/opentx.h index cc7a82a65..55d3f157a 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -1301,26 +1301,7 @@ void evalFunctions(); extern volatile rotenc_t g_rotenc[1]; #endif -#if defined(CPUARM) - #include "telemetry/telemetry.h" -#endif - -#if defined (FRSKY) - // FrSky Telemetry - #include "telemetry/frsky.h" -#elif defined(JETI) - // Jeti-DUPLEX Telemetry - #include "telemetry/jeti.h" -#elif defined(ARDUPILOT) - // ArduPilot Telemetry - #include "telemetry/ardupilot.h" -#elif defined(NMEA) - // NMEA Telemetry - #include "telemetry/nmea.h" -#elif defined(MAVLINK) - // Mavlink Telemetry - #include "telemetry/mavlink.h" -#endif +#include "telemetry/telemetry.h" #if defined(CPUARM) uint16_t crc16(uint8_t * buf, uint32_t len); diff --git a/radio/src/simu.cpp b/radio/src/simu.cpp index 57f7c4201..c9adee146 100644 --- a/radio/src/simu.cpp +++ b/radio/src/simu.cpp @@ -524,7 +524,7 @@ int main(int argc,char **argv) #endif #if defined(FRSKY) && !defined(FRSKY_SPORT) - frskyStreaming = 1; + telemetryStreaming = 1; #endif printf("Model size = %d\n", (int)sizeof(g_model)); diff --git a/radio/src/targets/common_avr/telemetry_driver.cpp b/radio/src/targets/common_avr/telemetry_driver.cpp index 2388f3794..da3ebbdfc 100644 --- a/radio/src/targets/common_avr/telemetry_driver.cpp +++ b/radio/src/targets/common_avr/telemetry_driver.cpp @@ -33,7 +33,7 @@ void telemetryEnableRx(void) UCSRB_N(TLM_USART) |= (1 << RXCIE_N(TLM_USART)); // enable Interrupt } -void processSerialData(uint8_t data); +void processFrskyTelemetryData(uint8_t data); extern uint8_t frskyRxBufferCount; // TODO not driver, change name ISR(USART_RX_vect_N(TLM_USART)) @@ -82,7 +82,7 @@ ISR(USART_RX_vect_N(TLM_USART)) frskyRxBufferCount = 0; } else { - processSerialData(data); + processFrskyTelemetryData(data); } cli() ; diff --git a/radio/src/telemetry/frsky.cpp b/radio/src/telemetry/frsky.cpp index 934dc5437..345002f56 100644 --- a/radio/src/telemetry/frsky.cpp +++ b/radio/src/telemetry/frsky.cpp @@ -18,113 +18,14 @@ * GNU General Public License for more details. */ -#include "../opentx.h" +#include "opentx.h" -uint8_t frskyStreaming = 0; - -#if defined(WS_HOW_HIGH) -uint8_t frskyUsrStreaming = 0; -#endif - -uint8_t link_counter = 0; - -#define FRSKY_RX_PACKET_SIZE 19 uint8_t frskyRxBuffer[FRSKY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1) - -#if !defined(CPUARM) && !defined(PCBFLAMENCO) -uint8_t frskyTxBuffer[FRSKY_TX_PACKET_SIZE]; -#endif - -#if !defined(CPUARM) -uint8_t frskyTxBufferCount = 0; -#endif - -#if defined(CPUARM) -uint8_t telemetryState = TELEMETRY_INIT; -#endif - uint8_t frskyRxBufferCount = 0; -FrskyData frskyData; - -#if defined(CPUARM) -uint8_t telemetryProtocol = 255; -#if defined(REVX) -uint8_t serialInversion = 0; -#endif -#define IS_FRSKY_D_PROTOCOL() (telemetryProtocol == PROTOCOL_FRSKY_D) -#define IS_FRSKY_SPORT_PROTOCOL() (telemetryProtocol == PROTOCOL_FRSKY_SPORT) -#else -#define IS_FRSKY_D_PROTOCOL() (true) -#define IS_FRSKY_SPORT_PROTOCOL() (false) -#endif - -#if defined(CPUARM) -void FrskyValueWithMin::reset() -{ - memclear(this, sizeof(*this)); -} -#endif - -void FrskyValueWithMin::set(uint8_t value) -{ -#if defined(CPUARM) - if (this->value == 0) { - memset(values, value, TELEMETRY_AVERAGE_COUNT); - this->value = value; - } - else { - //calculate the average from values[] and value - //also shift readings in values [] array - unsigned int sum = values[0]; - for (int i=0; ivalue = sum/(TELEMETRY_AVERAGE_COUNT+1); - } -#else - if (this->value == 0) { - this->value = value; - } - else { - sum += value; - if (link_counter == 0) { - this->value = sum / (IS_FRSKY_D_PROTOCOL() ? FRSKY_D_AVERAGING : FRSKY_SPORT_AVERAGING); - sum = 0; - } - } -#endif - - if (!min || value < min) { - min = value; - } -} - -void FrskyValueWithMinMax::set(uint8_t value, uint8_t unit) -{ - FrskyValueWithMin::set(value); - if (unit != UNIT_VOLTS) { - this->value = value; - } - if (!max || value > max) { - max = value; - } -} - #if !defined(CPUARM) -uint16_t getChannelRatio(source_t channel) -{ - return (uint16_t)g_model.frsky.channels[channel].ratio << g_model.frsky.channels[channel].multiplier; -} - -lcdint_t applyChannelRatio(source_t channel, lcdint_t val) -{ - return ((int32_t)val+g_model.frsky.channels[channel].offset) * getChannelRatio(channel) * 2 / 51; -} +uint8_t frskyTxBuffer[FRSKY_TX_PACKET_SIZE]; +uint8_t frskyTxBufferCount = 0; #endif #if defined(TELEMETREZ) @@ -138,7 +39,7 @@ extern uint8_t TrotCount; extern uint8_t TezRotary; #endif -NOINLINE void processSerialData(uint8_t data) +NOINLINE void processFrskyTelemetryData(uint8_t data) { static uint8_t dataState = STATE_DATA_IDLE; @@ -166,8 +67,7 @@ NOINLINE void processSerialData(uint8_t data) } #endif - switch (dataState) - { + switch (dataState) { case STATE_DATA_START: if (data == START_STOP) { if (IS_FRSKY_SPORT_PROTOCOL()) { @@ -262,412 +162,23 @@ NOINLINE void processSerialData(uint8_t data) #endif } -void telemetryWakeup() -{ -#if defined(CPUARM) - uint8_t requiredTelemetryProtocol = MODEL_TELEMETRY_PROTOCOL(); -#if defined(REVX) - uint8_t requiredSerialInversion = g_model.moduleData[EXTERNAL_MODULE].invertedSerial; - if (telemetryProtocol != requiredTelemetryProtocol || serialInversion != requiredSerialInversion) { - serialInversion = requiredSerialInversion; -#else - if (telemetryProtocol != requiredTelemetryProtocol) { -#endif - telemetryInit(requiredTelemetryProtocol); - telemetryProtocol = requiredTelemetryProtocol; - } -#endif - -#if defined(CPUSTM32) - uint8_t data; -#if defined(LOG_TELEMETRY) && !defined(SIMU) - static tmr10ms_t lastTime = 0; - tmr10ms_t newTime = get_tmr10ms(); - struct gtm utm; - gettime(&utm); -#endif - while (telemetryFifo.pop(data)) { - processSerialData(data); -#if defined(LOG_TELEMETRY) && !defined(SIMU) - extern FIL g_telemetryFile; - if (lastTime != newTime) { - f_printf(&g_telemetryFile, "\r\n%4d-%02d-%02d,%02d:%02d:%02d.%02d0: %02X", utm.tm_year+1900, utm.tm_mon+1, utm.tm_mday, utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100, data); - lastTime = newTime; - } - else { - f_printf(&g_telemetryFile, " %02X", data); - } -#endif - } -#elif defined(PCBSKY9X) - if (telemetryProtocol == PROTOCOL_FRSKY_D_SECONDARY) { - uint8_t data; - while (telemetrySecondPortReceive(data)) { - processSerialData(data); - } - } - else { - // Receive serial data here - rxPdcUsart(processSerialData); - } -#endif - -#if !defined(CPUARM) - if (IS_FRSKY_D_PROTOCOL()) { - // Attempt to transmit any waiting Fr-Sky alarm set packets every 50ms (subject to packet buffer availability) - static uint8_t frskyTxDelay = 5; - if (frskyAlarmsSendState && (--frskyTxDelay == 0)) { - frskyTxDelay = 5; // 50ms -#if !defined(SIMU) - frskyDSendNextAlarm(); -#endif - } - } -#endif - -#if defined(CPUARM) - for (int i=0; i 0x33) -#else - #define FRSKY_BAD_ANTENNA() (frskyData.swr.value > 0x33) -#endif - -#if defined(CPUARM) - static tmr10ms_t alarmsCheckTime = 0; - #define SCHEDULE_NEXT_ALARMS_CHECK(seconds) alarmsCheckTime = get_tmr10ms() + (100*(seconds)) - if (int32_t(get_tmr10ms() - alarmsCheckTime) > 0) { - - SCHEDULE_NEXT_ALARMS_CHECK(1/*second*/); - - uint8_t now = TelemetryItem::now(); - bool sensor_lost = false; - for (int i=0; i TELEMETRY_VALUE_OLD_THRESHOLD) { - sensor_lost = true; - telemetryItems[i].lastReceived = TELEMETRY_VALUE_OLD; - TelemetrySensor * sensor = & g_model.telemetrySensors[i]; - if (sensor->unit == UNIT_DATETIME) { - telemetryItems[i].datetime.datestate = 0; - telemetryItems[i].datetime.timestate = 0; - } - } - } - } - if (sensor_lost && TELEMETRY_STREAMING()) { - audioEvent(AU_SENSOR_LOST); - } - -#if defined(PCBTARANIS) - if ((g_model.moduleData[INTERNAL_MODULE].rfProtocol != RF_PROTO_OFF || g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_XJT) && FRSKY_BAD_ANTENNA()) { - AUDIO_SWR_RED(); - POPUP_WARNING(STR_ANTENNAPROBLEM); - SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/); - } -#endif - - if (TELEMETRY_STREAMING()) { - if (getRssiAlarmValue(1) && TELEMETRY_RSSI() < getRssiAlarmValue(1)) { - AUDIO_RSSI_RED(); - SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/); - } - else if (getRssiAlarmValue(0) && TELEMETRY_RSSI() < getRssiAlarmValue(0)) { - AUDIO_RSSI_ORANGE(); - SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/); - } - } - } - - if (TELEMETRY_STREAMING()) { - if (telemetryState == TELEMETRY_KO) { - AUDIO_TELEMETRY_BACK(); - } - telemetryState = TELEMETRY_OK; - } - else if (telemetryState == TELEMETRY_OK) { - telemetryState = TELEMETRY_KO; - AUDIO_TELEMETRY_LOST(); - } -#endif -} - -void telemetryInterrupt10ms() -{ -#if defined(CPUARM) -#elif defined(FRSKY_HUB) - uint16_t voltage = 0; /* unit: 1/10 volts */ - for (uint8_t i=0; i>1) * (voltage>>1)) / 25; - - frskyData.hub.currentPrescale += current; - if (frskyData.hub.currentPrescale >= 3600) { - frskyData.hub.currentConsumption += 1; - frskyData.hub.currentPrescale -= 3600; - } -#endif - } - -#if !defined(CPUARM) - if (frskyData.hub.power > frskyData.hub.maxPower) { - frskyData.hub.maxPower = frskyData.hub.power; - } -#endif - } - -#if defined(WS_HOW_HIGH) - if (frskyUsrStreaming > 0) { - frskyUsrStreaming--; - } -#endif - - if (frskyStreaming > 0) { - frskyStreaming--; - } - else { -#if !defined(SIMU) -#if defined(CPUARM) - frskyData.rssi.reset(); -#else - frskyData.rssi[0].set(0); - frskyData.rssi[1].set(0); -#endif -#endif - } -} - -void telemetryReset() -{ - memclear(&frskyData, sizeof(frskyData)); - -#if defined(CPUARM) - for (int index=0; index Cell number + Cell voltage - uint8_t battnumber = ((frskyData.hub.volts & 0x00F0) >> 4); + uint8_t battnumber = ((telemetryData.hub.volts & 0x00F0) >> 4); if (battnumber < 12) { - if (frskyData.hub.cellsCount < battnumber+1) { - frskyData.hub.cellsCount = battnumber+1; + if (telemetryData.hub.cellsCount < battnumber+1) { + telemetryData.hub.cellsCount = battnumber+1; } - uint8_t cellVolts = (uint8_t)(((((frskyData.hub.volts & 0xFF00) >> 8) + ((frskyData.hub.volts & 0x000F) << 8))) / 10); - frskyData.hub.cellVolts[battnumber] = cellVolts; - if (!frskyData.hub.minCellVolts || cellVolts> 8) + ((telemetryData.hub.volts & 0x000F) << 8))) / 10); + telemetryData.hub.cellVolts[battnumber] = cellVolts; + if (!telemetryData.hub.minCellVolts || cellVolts * luaInputTelemetryFifo = NULL; -Fifo * luaOutputTelemetryFifo = NULL; -#endif diff --git a/radio/src/telemetry/frsky.h b/radio/src/telemetry/frsky.h index a5187bbfe..2ede667a1 100644 --- a/radio/src/telemetry/frsky.h +++ b/radio/src/telemetry/frsky.h @@ -21,7 +21,7 @@ #ifndef _FRSKY_H_ #define _FRSKY_H_ -#include +#include "telemetry_holders.h" #define FRSKY_SPORT_BAUDRATE 57600 #define FRSKY_D_BAUDRATE 9600 @@ -161,59 +161,9 @@ #define DATA_ID_SP2UH 0x45 // 5 #define DATA_ID_SP2UR 0xC6 // 6 - -// Global Fr-Sky telemetry data variables -extern uint8_t frskyStreaming; // >0 (true) == data is streaming in. 0 = nodata detected for some time - -#if defined(WS_HOW_HIGH) -extern uint8_t frskyUsrStreaming; -#endif - -extern uint8_t link_counter; - -#if defined(CPUARM) -enum TelemetryStates { - TELEMETRY_INIT, - TELEMETRY_OK, - TELEMETRY_KO -}; - -extern uint8_t telemetryState; -#endif - -#if defined(CPUARM) -#define TELEMETRY_AVERAGE_COUNT 3 // we actually average one more reading! -#define RAW_FRSKY_MINMAX(v) v.values[TELEMETRY_AVERAGE_COUNT-1] -class FrskyValueWithMin { - public: - uint8_t value; // fitered value (average of last TELEMETRY_AVERAGE_COUNT+1 values) - uint8_t min; - uint8_t values[TELEMETRY_AVERAGE_COUNT]; - void set(uint8_t value); - void reset(); -}; -#else -#define RAW_FRSKY_MINMAX(v) v.value -class FrskyValueWithMin { - public: - uint8_t value; - uint8_t min; - uint16_t sum; - void set(uint8_t value); -}; -#endif - -class FrskyValueWithMinMax: public FrskyValueWithMin { - public: - uint8_t max; - void set(uint8_t value, uint8_t unit); -}; - -#if defined(CPUARM) - - -#elif defined(FRSKY_HUB) -PACK(struct FrskySerialData { +#if !defined(CPUARM) +#if defined(FRSKY_HUB) +PACK(struct FrskyTelemetryData { int16_t baroAltitudeOffset; // spare reused int16_t gpsAltitude_bp; // 0x01 before punct int16_t temperature1; // 0x02 -20 .. 250 deg. celcius @@ -291,7 +241,7 @@ PACK(struct FrskySerialData { int16_t dTE; }); #elif defined(WS_HOW_HIGH) -PACK(struct FrskySerialData { +PACK(struct FrskyTelemetryData { int16_t baroAltitude_bp; // 0..9,999 meters int16_t baroAltitudeOffset; int16_t minAltitude; @@ -305,7 +255,7 @@ PACK(struct FrskySerialData { #endif }); #elif defined(VARIO) -PACK(struct FrskySerialData { +PACK(struct FrskyTelemetryData { int16_t varioSpeed; // Vertical speed in cm/s uint16_t currentConsumption; uint16_t currentPrescale; @@ -313,41 +263,17 @@ PACK(struct FrskySerialData { uint16_t maxPower; }); #else -PACK(struct FrskySerialData { +PACK(struct FrskyTelemetryData { uint16_t currentConsumption; uint16_t currentPrescale; uint16_t power; uint16_t maxPower; }); #endif - -enum TelemAnas { - TELEM_ANA_A1, - TELEM_ANA_A2, -#if defined(CPUARM) - TELEM_ANA_A3, - TELEM_ANA_A4, -#endif - TELEM_ANA_COUNT -}; - -#if defined(CPUARM) -struct FrskyData { - FrskyValueWithMin swr; // TODO Min not needed - FrskyValueWithMin rssi; // TODO Min not needed - uint16_t xjtVersion; - bool varioHighPrecision; -}; -#else -struct FrskyData { - FrskyValueWithMinMax analog[TELEM_ANA_COUNT]; - FrskyValueWithMin rssi[2]; - FrskySerialData hub; -}; #endif #if defined(PCBTARANIS) && defined(REVPLUS) - #define IS_VALID_XJT_VERSION() (frskyData.xjtVersion != 0 && frskyData.xjtVersion != 0xff) + #define IS_VALID_XJT_VERSION() (telemetryData.xjtVersion != 0 && telemetryData.xjtVersion != 0xff) #else #define IS_VALID_XJT_VERSION() (1) #endif @@ -364,101 +290,99 @@ enum AlarmLevel { #define ALARM_LEVEL(channel, alarm) ((g_model.frsky.channels[channel].alarms_level >> (2*alarm)) & 3) #if defined(CPUARM) - #define TELEMETRY_STREAMING() (frskyData.rssi.value > 0) - #define TELEMETRY_RSSI() (frskyData.rssi.value) - #define TELEMETRY_RSSI_MIN() (frskyData.rssi.min) + #define TELEMETRY_STREAMING() (telemetryData.rssi.value > 0) + #define TELEMETRY_RSSI() (telemetryData.rssi.value) + #define TELEMETRY_RSSI_MIN() (telemetryData.rssi.min) #define TELEMETRY_CELL_VOLTAGE_MUTLIPLIER 1 - #define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp - #define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap + #define TELEMETRY_GPS_SPEED_BP telemetryData.hub.gpsSpeed_bp + #define TELEMETRY_GPS_SPEED_AP telemetryData.hub.gpsSpeed_ap - #define TELEMETRY_ABSOLUTE_GPS_ALT (frskyData.hub.gpsAltitude) - #define TELEMETRY_RELATIVE_GPS_ALT (frskyData.hub.gpsAltitude + frskyData.hub.gpsAltitudeOffset) + #define TELEMETRY_ABSOLUTE_GPS_ALT (telemetryData.hub.gpsAltitude) + #define TELEMETRY_RELATIVE_GPS_ALT (telemetryData.hub.gpsAltitude + telemetryData.hub.gpsAltitudeOffset) #define TELEMETRY_RELATIVE_GPS_ALT_BP (TELEMETRY_RELATIVE_GPS_ALT / 100) - #define TELEMETRY_RELATIVE_BARO_ALT_BP (frskyData.hub.baroAltitude / 100) - #define TELEMETRY_RELATIVE_BARO_ALT_AP (frskyData.hub.baroAltitude % 100) + #define TELEMETRY_RELATIVE_BARO_ALT_BP (telemetryData.hub.baroAltitude / 100) + #define TELEMETRY_RELATIVE_BARO_ALT_AP (telemetryData.hub.baroAltitude % 100) - #define TELEMETRY_BARO_ALT_PREPARE() div_t baroAltitudeDivision = div(getConvertedTelemetryValue(frskyData.hub.baroAltitude, UNIT_DIST), 100) + #define TELEMETRY_BARO_ALT_PREPARE() div_t baroAltitudeDivision = div(getConvertedTelemetryValue(telemetryData.hub.baroAltitude, UNIT_DIST), 100) #define TELEMETRY_BARO_ALT_FORMAT "%c%d.%02d," - #define TELEMETRY_BARO_ALT_ARGS frskyData.hub.baroAltitude < 0 ? '-' : ' ', abs(baroAltitudeDivision.quot), abs(baroAltitudeDivision.rem), + #define TELEMETRY_BARO_ALT_ARGS telemetryData.hub.baroAltitude < 0 ? '-' : ' ', abs(baroAltitudeDivision.quot), abs(baroAltitudeDivision.rem), #define TELEMETRY_GPS_ALT_FORMAT "%c%d.%02d," - #define TELEMETRY_GPS_ALT_ARGS frskyData.hub.gpsAltitude < 0 ? '-' : ' ', abs(frskyData.hub.gpsAltitude / 100), abs(frskyData.hub.gpsAltitude % 100), + #define TELEMETRY_GPS_ALT_ARGS telemetryData.hub.gpsAltitude < 0 ? '-' : ' ', abs(telemetryData.hub.gpsAltitude / 100), abs(telemetryData.hub.gpsAltitude % 100), #define TELEMETRY_SPEED_UNIT (IS_IMPERIAL_ENABLE() ? SPEED_UNIT_IMP : SPEED_UNIT_METR) #define TELEMETRY_GPS_SPEED_FORMAT "%d," - #define TELEMETRY_GPS_SPEED_ARGS frskyData.hub.gpsSpeed_bp, + #define TELEMETRY_GPS_SPEED_ARGS telemetryData.hub.gpsSpeed_bp, #if defined(CPUARM) - #define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, TELEMETRY_CELL_VOLTAGE(0)/100, TELEMETRY_CELL_VOLTAGE(0)%100, TELEMETRY_CELL_VOLTAGE(1)/100, TELEMETRY_CELL_VOLTAGE(1)%100, TELEMETRY_CELL_VOLTAGE(2)/100, TELEMETRY_CELL_VOLTAGE(2)%100, TELEMETRY_CELL_VOLTAGE(3)/100, TELEMETRY_CELL_VOLTAGE(3)%100, TELEMETRY_CELL_VOLTAGE(4)/100, TELEMETRY_CELL_VOLTAGE(4)%100, TELEMETRY_CELL_VOLTAGE(5)/100, TELEMETRY_CELL_VOLTAGE(5)%100, TELEMETRY_CELL_VOLTAGE(6)/100, TELEMETRY_CELL_VOLTAGE(6)%100, TELEMETRY_CELL_VOLTAGE(7)/100, TELEMETRY_CELL_VOLTAGE(7)%100, TELEMETRY_CELL_VOLTAGE(8)/100, TELEMETRY_CELL_VOLTAGE(8)%100, TELEMETRY_CELL_VOLTAGE(9)/100, TELEMETRY_CELL_VOLTAGE(9)%100, TELEMETRY_CELL_VOLTAGE(10)/100, TELEMETRY_CELL_VOLTAGE(10)%100, TELEMETRY_CELL_VOLTAGE(11)/100, TELEMETRY_CELL_VOLTAGE(11)%100, + #define TELEMETRY_CELLS_ARGS telemetryData.hub.cellsSum / 10, telemetryData.hub.cellsSum % 10, TELEMETRY_CELL_VOLTAGE(0)/100, TELEMETRY_CELL_VOLTAGE(0)%100, TELEMETRY_CELL_VOLTAGE(1)/100, TELEMETRY_CELL_VOLTAGE(1)%100, TELEMETRY_CELL_VOLTAGE(2)/100, TELEMETRY_CELL_VOLTAGE(2)%100, TELEMETRY_CELL_VOLTAGE(3)/100, TELEMETRY_CELL_VOLTAGE(3)%100, TELEMETRY_CELL_VOLTAGE(4)/100, TELEMETRY_CELL_VOLTAGE(4)%100, TELEMETRY_CELL_VOLTAGE(5)/100, TELEMETRY_CELL_VOLTAGE(5)%100, TELEMETRY_CELL_VOLTAGE(6)/100, TELEMETRY_CELL_VOLTAGE(6)%100, TELEMETRY_CELL_VOLTAGE(7)/100, TELEMETRY_CELL_VOLTAGE(7)%100, TELEMETRY_CELL_VOLTAGE(8)/100, TELEMETRY_CELL_VOLTAGE(8)%100, TELEMETRY_CELL_VOLTAGE(9)/100, TELEMETRY_CELL_VOLTAGE(9)%100, TELEMETRY_CELL_VOLTAGE(10)/100, TELEMETRY_CELL_VOLTAGE(10)%100, TELEMETRY_CELL_VOLTAGE(11)/100, TELEMETRY_CELL_VOLTAGE(11)%100, #define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d," #define TELEMETRY_CELLS_LABEL "Cell volts,Cell 1,Cell 2,Cell 3,Cell 4,Cell 5,Cell 6,Cell 7,Cell 8,Cell 9,Cell 10,Cell 11,Cell 12," #else - #define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, TELEMETRY_CELL_VOLTAGE(0)/100, TELEMETRY_CELL_VOLTAGE(0)%100, TELEMETRY_CELL_VOLTAGE(1)/100, TELEMETRY_CELL_VOLTAGE(1)%100, TELEMETRY_CELL_VOLTAGE(2)/100, TELEMETRY_CELL_VOLTAGE(2)%100, TELEMETRY_CELL_VOLTAGE(3)/100, TELEMETRY_CELL_VOLTAGE(3)%100, TELEMETRY_CELL_VOLTAGE(4)/100, TELEMETRY_CELL_VOLTAGE(4)%100, TELEMETRY_CELL_VOLTAGE(5)/100, TELEMETRY_CELL_VOLTAGE(5)%100, + #define TELEMETRY_CELLS_ARGS telemetryData.hub.cellsSum / 10, telemetryData.hub.cellsSum % 10, TELEMETRY_CELL_VOLTAGE(0)/100, TELEMETRY_CELL_VOLTAGE(0)%100, TELEMETRY_CELL_VOLTAGE(1)/100, TELEMETRY_CELL_VOLTAGE(1)%100, TELEMETRY_CELL_VOLTAGE(2)/100, TELEMETRY_CELL_VOLTAGE(2)%100, TELEMETRY_CELL_VOLTAGE(3)/100, TELEMETRY_CELL_VOLTAGE(3)%100, TELEMETRY_CELL_VOLTAGE(4)/100, TELEMETRY_CELL_VOLTAGE(4)%100, TELEMETRY_CELL_VOLTAGE(5)/100, TELEMETRY_CELL_VOLTAGE(5)%100, #define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d," #define TELEMETRY_CELLS_LABEL "Cell volts,Cell 1,Cell 2,Cell 3,Cell 4,Cell 5,Cell 6," #endif #define TELEMETRY_CURRENT_FORMAT "%d.%d," - #define TELEMETRY_CURRENT_ARGS frskyData.hub.current / 10, frskyData.hub.current % 10, + #define TELEMETRY_CURRENT_ARGS telemetryData.hub.current / 10, telemetryData.hub.current % 10, #define TELEMETRY_VFAS_FORMAT "%d.%d," - #define TELEMETRY_VFAS_ARGS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10, + #define TELEMETRY_VFAS_ARGS telemetryData.hub.vfas / 10, telemetryData.hub.vfas % 10, #define TELEMETRY_VSPEED_FORMAT "%c%d.%02d," - #define TELEMETRY_VSPEED_ARGS frskyData.hub.varioSpeed < 0 ? '-' : ' ', abs(frskyData.hub.varioSpeed / 100), abs(frskyData.hub.varioSpeed % 100), + #define TELEMETRY_VSPEED_ARGS telemetryData.hub.varioSpeed < 0 ? '-' : ' ', abs(telemetryData.hub.varioSpeed / 100), abs(telemetryData.hub.varioSpeed % 100), #define TELEMETRY_ASPEED_FORMAT "%d.%d," - #define TELEMETRY_ASPEED_ARGS frskyData.hub.airSpeed / 10, frskyData.hub.airSpeed % 10, + #define TELEMETRY_ASPEED_ARGS telemetryData.hub.airSpeed / 10, telemetryData.hub.airSpeed % 10, #define TELEMETRY_OPENXSENSOR() (0) #else - #define TELEMETRY_STREAMING() (frskyStreaming > 0) - #define TELEMETRY_RSSI() (frskyData.rssi[0].value) - #define TELEMETRY_RSSI_MIN() (frskyData.rssi[0].min) + #define TELEMETRY_STREAMING() (telemetryStreaming > 0) + #define TELEMETRY_RSSI() (telemetryData.rssi[0].value) + #define TELEMETRY_RSSI_MIN() (telemetryData.rssi[0].min) #define TELEMETRY_CELL_VOLTAGE_MUTLIPLIER 2 - #define TELEMETRY_BARO_ALT_AVAILABLE() (frskyData.hub.baroAltitudeOffset) + #define TELEMETRY_BARO_ALT_AVAILABLE() (telemetryData.hub.baroAltitudeOffset) #define TELEMETRY_BARO_ALT_UNIT (IS_IMPERIAL_ENABLE() ? LENGTH_UNIT_IMP : LENGTH_UNIT_METR) - #define TELEMETRY_RELATIVE_BARO_ALT_BP frskyData.hub.baroAltitude_bp - #define TELEMETRY_RELATIVE_BARO_ALT_AP frskyData.hub.baroAltitude_ap - #define TELEMETRY_RELATIVE_GPS_ALT_BP frskyData.hub.gpsAltitude_bp - #define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp - #define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap + #define TELEMETRY_RELATIVE_BARO_ALT_BP telemetryData.hub.baroAltitude_bp + #define TELEMETRY_RELATIVE_BARO_ALT_AP telemetryData.hub.baroAltitude_ap + #define TELEMETRY_RELATIVE_GPS_ALT_BP telemetryData.hub.gpsAltitude_bp + #define TELEMETRY_GPS_SPEED_BP telemetryData.hub.gpsSpeed_bp + #define TELEMETRY_GPS_SPEED_AP telemetryData.hub.gpsSpeed_ap #define TELEMETRY_BARO_ALT_PREPARE() #define TELEMETRY_BARO_ALT_FORMAT "%d," - #define TELEMETRY_BARO_ALT_ARGS frskyData.hub.baroAltitude_bp, + #define TELEMETRY_BARO_ALT_ARGS telemetryData.hub.baroAltitude_bp, #define TELEMETRY_GPS_ALT_FORMAT "%d," - #define TELEMETRY_GPS_ALT_ARGS frskyData.hub.gpsAltitude_bp, + #define TELEMETRY_GPS_ALT_ARGS telemetryData.hub.gpsAltitude_bp, #define TELEMETRY_SPEED_UNIT (IS_IMPERIAL_ENABLE() ? SPEED_UNIT_IMP : SPEED_UNIT_METR) #define TELEMETRY_GPS_SPEED_FORMAT "%d," - #define TELEMETRY_GPS_SPEED_ARGS frskyData.hub.gpsSpeed_bp, + #define TELEMETRY_GPS_SPEED_ARGS telemetryData.hub.gpsSpeed_bp, #if defined(CPUARM) - #define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, frskyData.hub.cellVolts[0]*2/100, frskyData.hub.cellVolts[0]*2%100, frskyData.hub.cellVolts[1]*2/100, frskyData.hub.cellVolts[1]*2%100, frskyData.hub.cellVolts[2]*2/100, frskyData.hub.cellVolts[2]*2%100, frskyData.hub.cellVolts[3]*2/100, frskyData.hub.cellVolts[3]*2%100, frskyData.hub.cellVolts[4]*2/100, frskyData.hub.cellVolts[4]*2%100, frskyData.hub.cellVolts[5]*2/100, frskyData.hub.cellVolts[5]*2%100, frskyData.hub.cellVolts[6]*2/100, frskyData.hub.cellVolts[6]*2%100, frskyData.hub.cellVolts[7]*2/100, frskyData.hub.cellVolts[7]*2%100, frskyData.hub.cellVolts[8]*2/100, frskyData.hub.cellVolts[8]*2%100, frskyData.hub.cellVolts[9]*2/100, frskyData.hub.cellVolts[9]*2%100, frskyData.hub.cellVolts[10]*2/100, frskyData.hub.cellVolts[10]*2%100, frskyData.hub.cellVolts[11]*2/100, frskyData.hub.cellVolts[11]*2%100, + #define TELEMETRY_CELLS_ARGS telemetryData.hub.cellsSum / 10, telemetryData.hub.cellsSum % 10, telemetryData.hub.cellVolts[0]*2/100, telemetryData.hub.cellVolts[0]*2%100, telemetryData.hub.cellVolts[1]*2/100, telemetryData.hub.cellVolts[1]*2%100, telemetryData.hub.cellVolts[2]*2/100, telemetryData.hub.cellVolts[2]*2%100, telemetryData.hub.cellVolts[3]*2/100, telemetryData.hub.cellVolts[3]*2%100, telemetryData.hub.cellVolts[4]*2/100, telemetryData.hub.cellVolts[4]*2%100, telemetryData.hub.cellVolts[5]*2/100, telemetryData.hub.cellVolts[5]*2%100, telemetryData.hub.cellVolts[6]*2/100, telemetryData.hub.cellVolts[6]*2%100, telemetryData.hub.cellVolts[7]*2/100, telemetryData.hub.cellVolts[7]*2%100, telemetryData.hub.cellVolts[8]*2/100, telemetryData.hub.cellVolts[8]*2%100, telemetryData.hub.cellVolts[9]*2/100, telemetryData.hub.cellVolts[9]*2%100, telemetryData.hub.cellVolts[10]*2/100, telemetryData.hub.cellVolts[10]*2%100, telemetryData.hub.cellVolts[11]*2/100, telemetryData.hub.cellVolts[11]*2%100, #define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d," #define TELEMETRY_CELLS_LABEL "Cell volts,Cell 1,Cell 2,Cell 3,Cell 4,Cell 5,Cell 6,Cell 7,Cell 8,Cell 9,Cell 10,Cell 11,Cell 12," #else - #define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, frskyData.hub.cellVolts[0]*2/100, frskyData.hub.cellVolts[0]*2%100, frskyData.hub.cellVolts[1]*2/100, frskyData.hub.cellVolts[1]*2%100, frskyData.hub.cellVolts[2]*2/100, frskyData.hub.cellVolts[2]*2%100, frskyData.hub.cellVolts[3]*2/100, frskyData.hub.cellVolts[3]*2%100, frskyData.hub.cellVolts[4]*2/100, frskyData.hub.cellVolts[4]*2%100, frskyData.hub.cellVolts[5]*2/100, frskyData.hub.cellVolts[5]*2%100, + #define TELEMETRY_CELLS_ARGS telemetryData.hub.cellsSum / 10, telemetryData.hub.cellsSum % 10, telemetryData.hub.cellVolts[0]*2/100, telemetryData.hub.cellVolts[0]*2%100, telemetryData.hub.cellVolts[1]*2/100, telemetryData.hub.cellVolts[1]*2%100, telemetryData.hub.cellVolts[2]*2/100, telemetryData.hub.cellVolts[2]*2%100, telemetryData.hub.cellVolts[3]*2/100, telemetryData.hub.cellVolts[3]*2%100, telemetryData.hub.cellVolts[4]*2/100, telemetryData.hub.cellVolts[4]*2%100, telemetryData.hub.cellVolts[5]*2/100, telemetryData.hub.cellVolts[5]*2%100, #define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d," #define TELEMETRY_CELLS_LABEL "Cell volts,Cell 1,Cell 2,Cell 3,Cell 4,Cell 5,Cell 6," #endif #define TELEMETRY_CURRENT_FORMAT "%d.%02d," - #define TELEMETRY_CURRENT_ARGS frskyData.hub.current / 100, frskyData.hub.current % 100, + #define TELEMETRY_CURRENT_ARGS telemetryData.hub.current / 100, telemetryData.hub.current % 100, #define TELEMETRY_VFAS_FORMAT "%d.%d," - #define TELEMETRY_VFAS_ARGS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10, + #define TELEMETRY_VFAS_ARGS telemetryData.hub.vfas / 10, telemetryData.hub.vfas % 10, #define TELEMETRY_VSPEED_FORMAT "%c%d.%02d," - #define TELEMETRY_VSPEED_ARGS frskyData.hub.varioSpeed < 0 ? '-' : ' ', frskyData.hub.varioSpeed / 100, frskyData.hub.varioSpeed % 100, + #define TELEMETRY_VSPEED_ARGS telemetryData.hub.varioSpeed < 0 ? '-' : ' ', telemetryData.hub.varioSpeed / 100, telemetryData.hub.varioSpeed % 100, #define TELEMETRY_ASPEED_FORMAT "%d.%d," - #define TELEMETRY_ASPEED_ARGS frskyData.hub.airSpeed / 10, frskyData.hub.airSpeed % 10, + #define TELEMETRY_ASPEED_ARGS telemetryData.hub.airSpeed / 10, telemetryData.hub.airSpeed % 10, #if defined(FRSKY_HUB) - #define TELEMETRY_OPENXSENSOR() (frskyData.hub.openXsensor) + #define TELEMETRY_OPENXSENSOR() (telemetryData.hub.openXsensor) #else #define TELEMETRY_OPENXSENSOR() (0) #endif #endif -#define TELEMETRY_CELL_VOLTAGE(k) (frskyData.hub.cellVolts[k] * TELEMETRY_CELL_VOLTAGE_MUTLIPLIER) -#define TELEMETRY_MIN_CELL_VOLTAGE (frskyData.hub.minCellVolts * TELEMETRY_CELL_VOLTAGE_MUTLIPLIER) - -extern FrskyData frskyData; +#define TELEMETRY_CELL_VOLTAGE(k) (telemetryData.hub.cellVolts[k] * TELEMETRY_CELL_VOLTAGE_MUTLIPLIER) +#define TELEMETRY_MIN_CELL_VOLTAGE (telemetryData.hub.minCellVolts * TELEMETRY_CELL_VOLTAGE_MUTLIPLIER) #define START_STOP 0x7e #define BYTESTUFF 0x7d @@ -476,6 +400,8 @@ enum FrSkyDataState { #endif }; +#define FRSKY_RX_PACKET_SIZE 19 + #if defined(CPUARM) #define frskySendAlarms() #else @@ -523,6 +449,39 @@ void telemetryInit(void); void telemetryInterrupt10ms(); +enum TelemetryProtocol +{ + TELEM_PROTO_FRSKY_D, + TELEM_PROTO_FRSKY_SPORT, +}; + +enum TelemAnas { + TELEM_ANA_A1, + TELEM_ANA_A2, +#if defined(CPUARM) + TELEM_ANA_A3, + TELEM_ANA_A4, +#endif + TELEM_ANA_COUNT +}; + +#if defined(CPUARM) +struct TelemetryData { + TelemetryValueWithMin swr; // TODO Min not needed + TelemetryValueWithMin rssi; // TODO Min not needed + uint16_t xjtVersion; + bool varioHighPrecision; +}; +#else +struct TelemetryData { + TelemetryValueWithMinMax analog[TELEM_ANA_COUNT]; + TelemetryValueWithMin rssi[2]; + FrskyTelemetryData hub; +}; +#endif + +extern TelemetryData telemetryData; + #if defined(CPUARM) typedef uint16_t frskyCellVoltage_t; #elif defined(FRSKY_HUB) @@ -535,7 +494,7 @@ void frskySetCellVoltage(uint8_t battnumber, frskyCellVoltage_t cellVolts); void frskyUpdateCells(); #endif -void processSerialData(uint8_t data); +void processFrskyTelemetryData(uint8_t data); #if defined(PCBTARANIS) inline uint8_t modelTelemetryProtocol() diff --git a/radio/src/telemetry/frsky_d.cpp b/radio/src/telemetry/frsky_d.cpp index a16b6ff7d..98adf0a3d 100644 --- a/radio/src/telemetry/frsky_d.cpp +++ b/radio/src/telemetry/frsky_d.cpp @@ -23,10 +23,10 @@ #if (defined(FRSKY_HUB) || defined(WS_HOW_HIGH)) void checkMinMaxAltitude() { - if (TELEMETRY_RELATIVE_BARO_ALT_BP > frskyData.hub.maxAltitude) - frskyData.hub.maxAltitude = TELEMETRY_RELATIVE_BARO_ALT_BP; - if (TELEMETRY_RELATIVE_BARO_ALT_BP < frskyData.hub.minAltitude) - frskyData.hub.minAltitude = TELEMETRY_RELATIVE_BARO_ALT_BP; + if (TELEMETRY_RELATIVE_BARO_ALT_BP > telemetryData.hub.maxAltitude) + telemetryData.hub.maxAltitude = TELEMETRY_RELATIVE_BARO_ALT_BP; + if (TELEMETRY_RELATIVE_BARO_ALT_BP < telemetryData.hub.minAltitude) + telemetryData.hub.minAltitude = TELEMETRY_RELATIVE_BARO_ALT_BP; } #endif @@ -71,148 +71,148 @@ void parseTelemHubByte(uint8_t byte) state = TS_IDLE; #if defined(GPS) - if ((uint8_t)structPos == offsetof(FrskySerialData, gpsLatitude_bp)) { + if ((uint8_t)structPos == offsetof(FrskyTelemetryData, gpsLatitude_bp)) { if (lowByte || byte) - frskyData.hub.gpsFix = 1; - else if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLatitude_bp > 1) - frskyData.hub.gpsFix = 0; + telemetryData.hub.gpsFix = 1; + else if (telemetryData.hub.gpsFix > 0 && telemetryData.hub.gpsLatitude_bp > 1) + telemetryData.hub.gpsFix = 0; } - else if ((uint8_t)structPos == offsetof(FrskySerialData, gpsLongitude_bp)) { + else if ((uint8_t)structPos == offsetof(FrskyTelemetryData, gpsLongitude_bp)) { if (lowByte || byte) - frskyData.hub.gpsFix = 1; - else if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLongitude_bp > 1) - frskyData.hub.gpsFix = 0; + telemetryData.hub.gpsFix = 1; + else if (telemetryData.hub.gpsFix > 0 && telemetryData.hub.gpsLongitude_bp > 1) + telemetryData.hub.gpsFix = 0; } - if ((uint8_t)structPos == offsetof(FrskySerialData, gpsAltitude_bp) || - ((uint8_t)structPos >= offsetof(FrskySerialData, gpsAltitude_ap) && (uint8_t)structPos <= offsetof(FrskySerialData, gpsLatitudeNS) && (uint8_t)structPos != offsetof(FrskySerialData, baroAltitude_bp) && (uint8_t)structPos != offsetof(FrskySerialData, baroAltitude_ap))) { + if ((uint8_t)structPos == offsetof(FrskyTelemetryData, gpsAltitude_bp) || + ((uint8_t)structPos >= offsetof(FrskyTelemetryData, gpsAltitude_ap) && (uint8_t)structPos <= offsetof(FrskyTelemetryData, gpsLatitudeNS) && (uint8_t)structPos != offsetof(FrskyTelemetryData, baroAltitude_bp) && (uint8_t)structPos != offsetof(FrskyTelemetryData, baroAltitude_ap))) { // If we don't have a fix, we may discard the value - if (frskyData.hub.gpsFix <= 0) + if (telemetryData.hub.gpsFix <= 0) return; } #endif // #if defined(GPS) - ((uint8_t*)&frskyData.hub)[structPos] = lowByte; - ((uint8_t*)&frskyData.hub)[structPos+1] = byte; + ((uint8_t*)&telemetryData.hub)[structPos] = lowByte; + ((uint8_t*)&telemetryData.hub)[structPos+1] = byte; switch ((uint8_t)structPos) { - case offsetof(FrskySerialData, rpm): - frskyData.hub.rpm *= (uint8_t)60/(g_model.frsky.blades+2); - if (frskyData.hub.rpm > frskyData.hub.maxRpm) - frskyData.hub.maxRpm = frskyData.hub.rpm; + case offsetof(FrskyTelemetryData, rpm): + telemetryData.hub.rpm *= (uint8_t)60/(g_model.frsky.blades+2); + if (telemetryData.hub.rpm > telemetryData.hub.maxRpm) + telemetryData.hub.maxRpm = telemetryData.hub.rpm; break; - case offsetof(FrskySerialData, temperature1): - if (frskyData.hub.temperature1 > frskyData.hub.maxTemperature1) - frskyData.hub.maxTemperature1 = frskyData.hub.temperature1; + case offsetof(FrskyTelemetryData, temperature1): + if (telemetryData.hub.temperature1 > telemetryData.hub.maxTemperature1) + telemetryData.hub.maxTemperature1 = telemetryData.hub.temperature1; break; - case offsetof(FrskySerialData, temperature2): - if (frskyData.hub.temperature2 > frskyData.hub.maxTemperature2) - frskyData.hub.maxTemperature2 = frskyData.hub.temperature2; + case offsetof(FrskyTelemetryData, temperature2): + if (telemetryData.hub.temperature2 > telemetryData.hub.maxTemperature2) + telemetryData.hub.maxTemperature2 = telemetryData.hub.temperature2; break; - case offsetof(FrskySerialData, current): + case offsetof(FrskyTelemetryData, current): #if defined(FAS_OFFSET) || !defined(CPUM64) - if ((int16_t)frskyData.hub.current > 0 && ((int16_t)frskyData.hub.current + g_model.frsky.fasOffset) > 0) - frskyData.hub.current += g_model.frsky.fasOffset; + if ((int16_t)telemetryData.hub.current > 0 && ((int16_t)telemetryData.hub.current + g_model.frsky.fasOffset) > 0) + telemetryData.hub.current += g_model.frsky.fasOffset; else - frskyData.hub.current = 0; + telemetryData.hub.current = 0; #endif - if (frskyData.hub.current > frskyData.hub.maxCurrent) - frskyData.hub.maxCurrent = frskyData.hub.current; + if (telemetryData.hub.current > telemetryData.hub.maxCurrent) + telemetryData.hub.maxCurrent = telemetryData.hub.current; break; - case offsetof(FrskySerialData, currentConsumption): + case offsetof(FrskyTelemetryData, currentConsumption): // we receive data from openXsensor. stops the calculation of consumption and power - frskyData.hub.openXsensor = 1; + telemetryData.hub.openXsensor = 1; break; - case offsetof(FrskySerialData, volts_ap): + case offsetof(FrskyTelemetryData, volts_ap): #if defined(FAS_BSS) - frskyData.hub.vfas = (frskyData.hub.volts_bp * 10 + frskyData.hub.volts_ap); + telemetryData.hub.vfas = (telemetryData.hub.volts_bp * 10 + telemetryData.hub.volts_ap); #else - frskyData.hub.vfas = ((frskyData.hub.volts_bp * 100 + frskyData.hub.volts_ap * 10) * 21) / 110; + telemetryData.hub.vfas = ((telemetryData.hub.volts_bp * 100 + telemetryData.hub.volts_ap * 10) * 21) / 110; #endif - /* TODO later if (!frskyData.hub.minVfas || frskyData.hub.minVfas > frskyData.hub.vfas) - frskyData.hub.minVfas = frskyData.hub.vfas; */ + /* TODO later if (!telemetryData.hub.minVfas || telemetryData.hub.minVfas > telemetryData.hub.vfas) + telemetryData.hub.minVfas = telemetryData.hub.vfas; */ break; - case offsetof(FrskySerialData, baroAltitude_bp): + case offsetof(FrskyTelemetryData, baroAltitude_bp): // First received barometer altitude => Altitude offset - if (!frskyData.hub.baroAltitudeOffset) - frskyData.hub.baroAltitudeOffset = -frskyData.hub.baroAltitude_bp; - frskyData.hub.baroAltitude_bp += frskyData.hub.baroAltitudeOffset; + if (!telemetryData.hub.baroAltitudeOffset) + telemetryData.hub.baroAltitudeOffset = -telemetryData.hub.baroAltitude_bp; + telemetryData.hub.baroAltitude_bp += telemetryData.hub.baroAltitudeOffset; checkMinMaxAltitude(); break; #if defined(GPS) - case offsetof(FrskySerialData, gpsAltitude_ap): - if (!frskyData.hub.gpsAltitudeOffset) - frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude_bp; - frskyData.hub.gpsAltitude_bp += frskyData.hub.gpsAltitudeOffset; - if (!frskyData.hub.baroAltitudeOffset) { - if (frskyData.hub.gpsAltitude_bp > frskyData.hub.maxAltitude) - frskyData.hub.maxAltitude = frskyData.hub.gpsAltitude_bp; - if (frskyData.hub.gpsAltitude_bp < frskyData.hub.minAltitude) - frskyData.hub.minAltitude = frskyData.hub.gpsAltitude_bp; + case offsetof(FrskyTelemetryData, gpsAltitude_ap): + if (!telemetryData.hub.gpsAltitudeOffset) + telemetryData.hub.gpsAltitudeOffset = -telemetryData.hub.gpsAltitude_bp; + telemetryData.hub.gpsAltitude_bp += telemetryData.hub.gpsAltitudeOffset; + if (!telemetryData.hub.baroAltitudeOffset) { + if (telemetryData.hub.gpsAltitude_bp > telemetryData.hub.maxAltitude) + telemetryData.hub.maxAltitude = telemetryData.hub.gpsAltitude_bp; + if (telemetryData.hub.gpsAltitude_bp < telemetryData.hub.minAltitude) + telemetryData.hub.minAltitude = telemetryData.hub.gpsAltitude_bp; } - if (!frskyData.hub.pilotLatitude && !frskyData.hub.pilotLongitude) { + if (!telemetryData.hub.pilotLatitude && !telemetryData.hub.pilotLongitude) { // First received GPS position => Pilot GPS position getGpsPilotPosition(); } - else if (frskyData.hub.gpsDistNeeded || menuHandlers[menuLevel] == menuTelemetryFrsky) { + else if (telemetryData.hub.gpsDistNeeded || menuHandlers[menuLevel] == menuTelemetryFrsky) { getGpsDistance(); } break; - case offsetof(FrskySerialData, gpsSpeed_bp): + case offsetof(FrskyTelemetryData, gpsSpeed_bp): // Speed => Max speed - if (frskyData.hub.gpsSpeed_bp > frskyData.hub.maxGpsSpeed) - frskyData.hub.maxGpsSpeed = frskyData.hub.gpsSpeed_bp; + if (telemetryData.hub.gpsSpeed_bp > telemetryData.hub.maxGpsSpeed) + telemetryData.hub.maxGpsSpeed = telemetryData.hub.gpsSpeed_bp; break; #endif - case offsetof(FrskySerialData, volts): + case offsetof(FrskyTelemetryData, volts): frskyUpdateCells(); break; #if defined(GPS) - case offsetof(FrskySerialData, hour): - frskyData.hub.hour = ((uint8_t)(frskyData.hub.hour + g_eeGeneral.timezone + 24)) % 24; + case offsetof(FrskyTelemetryData, hour): + telemetryData.hub.hour = ((uint8_t)(telemetryData.hub.hour + g_eeGeneral.timezone + 24)) % 24; break; #endif - case offsetof(FrskySerialData, accelX): - case offsetof(FrskySerialData, accelY): - case offsetof(FrskySerialData, accelZ): - *(int16_t*)(&((uint8_t*)&frskyData.hub)[structPos]) /= 10; + case offsetof(FrskyTelemetryData, accelX): + case offsetof(FrskyTelemetryData, accelY): + case offsetof(FrskyTelemetryData, accelZ): + *(int16_t*)(&((uint8_t*)&telemetryData.hub)[structPos]) /= 10; break; #if 0 - case offsetof(FrskySerialData, gpsAltitude_bp): - case offsetof(FrskySerialData, fuelLevel): - case offsetof(FrskySerialData, gpsLongitude_bp): - case offsetof(FrskySerialData, gpsLatitude_bp): - case offsetof(FrskySerialData, gpsCourse_bp): - case offsetof(FrskySerialData, day): - case offsetof(FrskySerialData, year): - case offsetof(FrskySerialData, sec): - case offsetof(FrskySerialData, gpsSpeed_ap): - case offsetof(FrskySerialData, gpsLongitude_ap): - case offsetof(FrskySerialData, gpsLatitude_ap): - case offsetof(FrskySerialData, gpsCourse_ap): - case offsetof(FrskySerialData, gpsLongitudeEW): - case offsetof(FrskySerialData, gpsLatitudeNS): - case offsetof(FrskySerialData, varioSpeed): - case offsetof(FrskySerialData, power): /* because sent by openXsensor */ - case offsetof(FrskySerialData, vfas): - case offsetof(FrskySerialData, volts_bp): + case offsetof(FrskyTelemetryData, gpsAltitude_bp): + case offsetof(FrskyTelemetryData, fuelLevel): + case offsetof(FrskyTelemetryData, gpsLongitude_bp): + case offsetof(FrskyTelemetryData, gpsLatitude_bp): + case offsetof(FrskyTelemetryData, gpsCourse_bp): + case offsetof(FrskyTelemetryData, day): + case offsetof(FrskyTelemetryData, year): + case offsetof(FrskyTelemetryData, sec): + case offsetof(FrskyTelemetryData, gpsSpeed_ap): + case offsetof(FrskyTelemetryData, gpsLongitude_ap): + case offsetof(FrskyTelemetryData, gpsLatitude_ap): + case offsetof(FrskyTelemetryData, gpsCourse_ap): + case offsetof(FrskyTelemetryData, gpsLongitudeEW): + case offsetof(FrskyTelemetryData, gpsLatitudeNS): + case offsetof(FrskyTelemetryData, varioSpeed): + case offsetof(FrskyTelemetryData, power): /* because sent by openXsensor */ + case offsetof(FrskyTelemetryData, vfas): + case offsetof(FrskyTelemetryData, volts_bp): break; default: - *((uint16_t *)(((uint8_t*)&frskyData.hub) + structPos)) = previousValue; + *((uint16_t *)(((uint8_t*)&telemetryData.hub) + structPos)) = previousValue; break; #endif } @@ -222,16 +222,16 @@ void parseTelemHubByte(uint8_t byte) #if defined(WS_HOW_HIGH) void parseTelemWSHowHighByte(uint8_t byte) { - if (frskyUsrStreaming < (WSHH_TIMEOUT10ms - 10)) { - ((uint8_t*)&frskyData.hub)[offsetof(FrskySerialData, baroAltitude_bp)] = byte; + if (wshhStreaming < (WSHH_TIMEOUT10ms - 10)) { + ((uint8_t*)&telemetryData.hub)[offsetof(FrskyTelemetryData, baroAltitude_bp)] = byte; checkMinMaxAltitude(); } else { // At least 100mS passed since last data received - ((uint8_t*)&frskyData.hub)[offsetof(FrskySerialData, baroAltitude_bp)+1] = byte; + ((uint8_t*)&telemetryData.hub)[offsetof(FrskyTelemetryData, baroAltitude_bp)+1] = byte; } // baroAltitude_bp unit here is feet! - frskyUsrStreaming = WSHH_TIMEOUT10ms; // reset counter + wshhStreaming = WSHH_TIMEOUT10ms; // reset counter } #endif @@ -242,16 +242,16 @@ void frskyDProcessPacket(uint8_t *packet) { case LINKPKT: // A1/A2/RSSI values { - frskyData.analog[TELEM_ANA_A1].set(packet[1], g_model.frsky.channels[TELEM_ANA_A1].type); - frskyData.analog[TELEM_ANA_A2].set(packet[2], g_model.frsky.channels[TELEM_ANA_A2].type); - frskyData.rssi[0].set(packet[3]); - frskyData.rssi[1].set(packet[4] / 2); - frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected + telemetryData.analog[TELEM_ANA_A1].set(packet[1], g_model.frsky.channels[TELEM_ANA_A1].type); + telemetryData.analog[TELEM_ANA_A2].set(packet[2], g_model.frsky.channels[TELEM_ANA_A2].type); + telemetryData.rssi[0].set(packet[3]); + telemetryData.rssi[1].set(packet[4] / 2); + telemetryStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected link_counter += 256 / FRSKY_D_AVERAGING; #if defined(VARIO) uint8_t varioSource = g_model.frsky.varioSource - VARIO_SOURCE_A1; if (varioSource < 2) { - frskyData.hub.varioSpeed = applyChannelRatio(varioSource, frskyData.analog[varioSource].value); + telemetryData.hub.varioSpeed = applyChannelRatio(varioSource, telemetryData.analog[varioSource].value); } #endif break; diff --git a/radio/src/telemetry/frsky_d_arm.cpp b/radio/src/telemetry/frsky_d_arm.cpp index d2172da01..1e476b7b0 100644 --- a/radio/src/telemetry/frsky_d_arm.cpp +++ b/radio/src/telemetry/frsky_d_arm.cpp @@ -73,8 +73,8 @@ void frskyDProcessPacket(uint8_t *packet) setTelemetryValue(TELEM_PROTO_FRSKY_D, D_A1_ID, 0, 0, packet[1], UNIT_VOLTS, 0); setTelemetryValue(TELEM_PROTO_FRSKY_D, D_A2_ID, 0, 0, packet[2], UNIT_VOLTS, 0); setTelemetryValue(TELEM_PROTO_FRSKY_D, D_RSSI_ID, 0, 0, packet[3], UNIT_RAW, 0); - frskyData.rssi.set(packet[3]); - frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected + telemetryData.rssi.set(packet[3]); + telemetryStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected break; } @@ -178,8 +178,8 @@ void processHubPacket(uint8_t id, int16_t value) } else if (id == BARO_ALT_AP_ID) { if (lastId == BARO_ALT_BP_ID) { - if (data > 9 || frskyData.varioHighPrecision) { - frskyData.varioHighPrecision = true; + if (data > 9 || telemetryData.varioHighPrecision) { + telemetryData.varioHighPrecision = true; data /= 10; // map hi precision vario into low precision. Altitude is stored in 0.1m anyways } data = (int16_t)lastValue * 10 + (((int16_t)lastValue < 0) ? -data : data); diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp index ba77828bf..a9cd0d17c 100644 --- a/radio/src/telemetry/frsky_sport.cpp +++ b/radio/src/telemetry/frsky_sport.cpp @@ -212,32 +212,32 @@ void processSportPacket(uint8_t * packet) uint32_t data = SPORT_DATA_S32(packet); if (id == RSSI_ID) { - frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected + telemetryStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected data = SPORT_DATA_U8(packet); if (data == 0) - frskyData.rssi.reset(); + telemetryData.rssi.reset(); else - frskyData.rssi.set(data); + telemetryData.rssi.set(data); } #if defined(PCBTARANIS) && defined(REVPLUS) else if (id == XJT_VERSION_ID) { - frskyData.xjtVersion = HUB_DATA_U16(packet); + telemetryData.xjtVersion = HUB_DATA_U16(packet); if (!IS_VALID_XJT_VERSION()) { - frskyData.swr.set(0x00); + telemetryData.swr.set(0x00); } } else if (id == SWR_ID) { if (IS_VALID_XJT_VERSION()) - frskyData.swr.set(SPORT_DATA_U8(packet)); + telemetryData.swr.set(SPORT_DATA_U8(packet)); else - frskyData.swr.set(0x00); + telemetryData.swr.set(0x00); } #else else if (id == XJT_VERSION_ID) { - frskyData.xjtVersion = HUB_DATA_U16(packet); + telemetryData.xjtVersion = HUB_DATA_U16(packet); } else if (id == SWR_ID) { - frskyData.swr.set(SPORT_DATA_U8(packet)); + telemetryData.swr.set(SPORT_DATA_U8(packet)); } #endif @@ -339,7 +339,7 @@ bool sportWaitState(SportUpdateState state, int timeout) for (int i=timeout/2; i>=0; i--) { uint8_t byte ; while (telemetryGetByte(&byte)) { - processSerialData(byte); + processFrskyTelemetryData(byte); } if (sportUpdateState == state) { return true; diff --git a/radio/src/telemetry/mavlink.h b/radio/src/telemetry/mavlink.h index 8c80b3bf7..fb2281259 100644 --- a/radio/src/telemetry/mavlink.h +++ b/radio/src/telemetry/mavlink.h @@ -1,29 +1,29 @@ -/* - * Copyright (C) OpenTX - * - * Based on code named - * th9x - http://code.google.com/p/th9x - * er9x - http://code.google.com/p/er9x - * gruvin9x - http://code.google.com/p/gruvin9x - * - * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 2 as - * published by the Free Software Foundation. - * - * This program 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. - */ - +/* + * Copyright (C) OpenTX + * + * Based on code named + * th9x - http://code.google.com/p/th9x + * er9x - http://code.google.com/p/er9x + * gruvin9x - http://code.google.com/p/gruvin9x + * + * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program 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. + */ + /*! \file mavlink.h * Mavlink include file */ -#ifndef _MAVLINK_H_ -#define _MAVLINK_H_ +#ifndef _MAVLINK_H_ +#define _MAVLINK_H_ #define MAVLINK_USE_CONVENIENCE_FUNCTIONS #define MAVLINK_COMM_NUM_BUFFERS 1 @@ -32,11 +32,11 @@ #include "targets/common_avr/serial_driver.h" #include "opentx.h" -extern int8_t mav_heartbeat; +extern int8_t mav_heartbeat; extern mavlink_system_t mavlink_system; -#define LEN_STATUSTEXT 20 -extern char mav_statustext[LEN_STATUSTEXT]; - +#define LEN_STATUSTEXT 20 +extern char mav_statustext[LEN_STATUSTEXT]; + extern void SERIAL_start_uart_send(); extern void SERIAL_end_uart_send(); extern void SERIAL_send_uart_bytes(const uint8_t * buf, uint16_t len); @@ -321,5 +321,5 @@ inline uint8_t getPrecisMavlinParamsValue(uint8_t idx) { void lcd_outdezFloat(uint8_t x, uint8_t y, float val, uint8_t precis, uint8_t mode); #endif -#endif // _MAVLINK_H_ +#endif // _MAVLINK_H_ diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index 5b723fcfd..70e58474f 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -2,7 +2,7 @@ * Copyright (C) OpenTX * * Based on code named - * th9x - http://code.google.com/p/th9x + * th9x - http://code.google.com/p/th9x * er9x - http://code.google.com/p/er9x * gruvin9x - http://code.google.com/p/gruvin9x * @@ -18,676 +18,435 @@ * GNU General Public License for more details. */ -#include "../opentx.h" +#include "opentx.h" -TelemetryItem telemetryItems[MAX_SENSORS]; -uint8_t allowNewSensors; +uint8_t telemetryStreaming = 0; -void TelemetryItem::gpsReceived() +#if defined(WS_HOW_HIGH) +uint8_t wshhStreaming = 0; +#endif + +uint8_t link_counter = 0; + +#if defined(CPUARM) +uint8_t telemetryState = TELEMETRY_INIT; +#endif + +TelemetryData telemetryData; + +#if defined(CPUARM) +uint8_t telemetryProtocol = 255; +#if defined(REVX) +uint8_t serialInversion = 0; +#endif +#endif + +#if defined(PCBSKY9X) && defined(REVX) +uint8_t serialInversion = 0; +#endif + +#if !defined(CPUARM) +uint16_t getChannelRatio(source_t channel) { - if (!distFromEarthAxis) { - gps.extractLatitudeLongitude(&pilotLatitude, &pilotLongitude); - uint32_t lat = pilotLatitude / 10000; - uint32_t angle2 = (lat*lat) / 10000; - uint32_t angle4 = angle2 * angle2; - distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500); - } - lastReceived = now(); + return (uint16_t)g_model.frsky.channels[channel].ratio << g_model.frsky.channels[channel].multiplier; } -void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32_t unit, uint32_t prec) +lcdint_t applyChannelRatio(source_t channel, lcdint_t val) { - int32_t newVal = val; + return ((int32_t)val+g_model.frsky.channels[channel].offset) * getChannelRatio(channel) * 2 / 51; +} +#endif - if (unit == UNIT_CELLS) { - uint32_t data = uint32_t(newVal); - uint8_t cellsCount = (data >> 24); - uint8_t cellIndex = ((data >> 16) & 0x0F); - uint16_t cellValue = (data & 0xFFFF); - if (cellsCount == 0) { - cellsCount = (cellIndex >= cells.count ? cellIndex + 1 : cells.count); - if (cellsCount != cells.count) { - clear(); - cells.count = cellsCount; - // we skip this round as we are not sure we received all cells values - return; - } - } - else if (cellsCount != cells.count) { - clear(); - cells.count = cellsCount; - } - cells.values[cellIndex].set(cellValue); - if (cellIndex+1 == cells.count) { - newVal = 0; - for (int i=0; i> 24); - datetime.month = (uint8_t) ((data & 0x00ff0000) >> 16); - datetime.day = (uint8_t) ((data & 0x0000ff00) >> 8); - if (datetime.year != 0) { - datetime.datestate = 1; - } -#if defined(RTCLOCK) - if (g_eeGeneral.adjustRTC && (datetime.datestate == 1)) { - struct gtm t; - gettime(&t); - t.tm_year = datetime.year+4; - t.tm_mon = datetime.month-1; - t.tm_mday = datetime.day; - rtcSetTime(&t); - } #endif - } - else { - datetime.hour = ((uint8_t) ((data & 0xff000000) >> 24) + g_eeGeneral.timezone + 24) % 24; - datetime.min = (uint8_t) ((data & 0x00ff0000) >> 16); - datetime.sec = (uint16_t) ((data & 0x0000ff00) >> 8); - if (datetime.datestate == 1) { - datetime.timestate = 1; - } -#if defined(RTCLOCK) - if (g_eeGeneral.adjustRTC && datetime.datestate == 1) { - struct gtm t; - gettime(&t); - if (abs((t.tm_hour-datetime.hour)*3600 + (t.tm_min-datetime.min)*60 + (t.tm_sec-datetime.sec)) > 20) { - // we adjust RTC only if difference is > 20 seconds - t.tm_hour = datetime.hour; - t.tm_min = datetime.min; - t.tm_sec = datetime.sec; - g_rtcTime = gmktime(&t); // update local timestamp and get wday calculated - rtcSetTime(&t); - } - } -#endif - } - if (datetime.year == 0) { - return; - } - newVal = 0; } - else if (unit == UNIT_GPS) { - uint32_t gps_long_lati_data = uint32_t(newVal); - uint32_t gps_long_lati_b1w, gps_long_lati_a1w; - gps_long_lati_b1w = (gps_long_lati_data & 0x3fffffff) / 10000; - gps_long_lati_a1w = (gps_long_lati_data & 0x3fffffff) % 10000; - switch ((gps_long_lati_data & 0xc0000000) >> 30) { - case 0: - gps.latitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60); - gps.latitude_ap = gps_long_lati_a1w; - gps.latitudeNS = 'N'; - break; - case 1: - gps.latitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60); - gps.latitude_ap = gps_long_lati_a1w; - gps.latitudeNS = 'S'; - break; - case 2: - gps.longitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60); - gps.longitude_ap = gps_long_lati_a1w; - gps.longitudeEW = 'E'; - break; - case 3: - gps.longitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60); - gps.longitude_ap = gps_long_lati_a1w; - gps.longitudeEW = 'W'; - break; - } - if (gps.longitudeEW && gps.latitudeNS) { - gpsReceived(); - } - return; - } - else if (unit >= UNIT_GPS_LONGITUDE && unit <= UNIT_GPS_LATITUDE_NS) { - uint32_t data = uint32_t(newVal); - switch (unit) { - case UNIT_GPS_LONGITUDE: - gps.longitude_bp = data >> 16; - gps.longitude_ap = data & 0xFFFF; - break; - case UNIT_GPS_LATITUDE: - gps.latitude_bp = data >> 16; - gps.latitude_ap = data & 0xFFFF; - break; - case UNIT_GPS_LONGITUDE_EW: - gps.longitudeEW = data; - break; - case UNIT_GPS_LATITUDE_NS: - gps.latitudeNS = data; - break; - } - if (gps.longitudeEW && gps.latitudeNS && gps.longitude_ap && gps.latitude_ap) { - gpsReceived(); - } - return; - } - else if (unit == UNIT_DATETIME_YEAR) { - datetime.year = newVal; - return; - } - else if (unit == UNIT_DATETIME_DAY_MONTH) { - uint32_t data = uint32_t(newVal); - datetime.month = data >> 8; - datetime.day = data & 0xFF; - datetime.datestate = 1; - return; - } - else if (unit == UNIT_DATETIME_HOUR_MIN) { - uint32_t data = uint32_t(newVal); - datetime.hour = ((data & 0xFF) + g_eeGeneral.timezone + 24) % 24; - datetime.min = data >> 8; - } - else if (unit == UNIT_DATETIME_SEC) { - datetime.sec = newVal & 0xFF; - datetime.timestate = 1; - newVal = 0; - } - else if (unit == UNIT_RPMS) { - if (sensor.custom.ratio != 0) { - newVal = (newVal * sensor.custom.offset) / sensor.custom.ratio; +#elif defined(PCBSKY9X) + if (telemetryProtocol == PROTOCOL_FRSKY_D_SECONDARY) { + uint8_t data; + while (telemetrySecondPortReceive(data)) { + processFrskyTelemetryData(data); } } else { - newVal = sensor.getValue(newVal, unit, prec); - if (sensor.autoOffset) { - if (!isAvailable()) { - std.offsetAuto = -newVal; - } - newVal += std.offsetAuto; - } - else if (sensor.filter) { - if (!isAvailable()) { - for (int i=0; i valueMax) { - valueMax = newVal; - if (sensor.unit == UNIT_VOLTS) { - valueMin = newVal; // the batt was changed +#if !defined(CPUARM) + if (IS_FRSKY_D_PROTOCOL()) { + // Attempt to transmit any waiting Fr-Sky alarm set packets every 50ms (subject to packet buffer availability) + static uint8_t frskyTxDelay = 5; + if (frskyAlarmsSendState && (--frskyTxDelay == 0)) { + frskyTxDelay = 5; // 50ms +#if !defined(SIMU) + frskyDSendNextAlarm(); +#endif } } +#endif +#if defined(CPUARM) for (int i=0; i 0x33) +#else + #define FRSKY_BAD_ANTENNA() (telemetryData.swr.value > 0x33) +#endif + +#if defined(CPUARM) + static tmr10ms_t alarmsCheckTime = 0; + #define SCHEDULE_NEXT_ALARMS_CHECK(seconds) alarmsCheckTime = get_tmr10ms() + (100*(seconds)) + if (int32_t(get_tmr10ms() - alarmsCheckTime) > 0) { + + SCHEDULE_NEXT_ALARMS_CHECK(1/*second*/); + + uint8_t now = TelemetryItem::now(); + bool sensor_lost = false; + for (int i=0; i TELEMETRY_VALUE_OLD_THRESHOLD) { + sensor_lost = true; + telemetryItems[i].lastReceived = TELEMETRY_VALUE_OLD; + TelemetrySensor * sensor = & g_model.telemetrySensors[i]; + if (sensor->unit == UNIT_DATETIME) { + telemetryItems[i].datetime.datestate = 0; + telemetryItems[i].datetime.timestate = 0; + } + } + } + } + if (sensor_lost && TELEMETRY_STREAMING()) { + audioEvent(AU_SENSOR_LOST); + } + +#if defined(PCBTARANIS) + if ((g_model.moduleData[INTERNAL_MODULE].rfProtocol != RF_PROTO_OFF || g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_XJT) && FRSKY_BAD_ANTENNA()) { + AUDIO_SWR_RED(); + POPUP_WARNING(STR_ANTENNAPROBLEM); + SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/); + } +#endif + + if (TELEMETRY_STREAMING()) { + if (getRssiAlarmValue(1) && TELEMETRY_RSSI() < getRssiAlarmValue(1)) { + AUDIO_RSSI_RED(); + SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/); + } + else if (getRssiAlarmValue(0) && TELEMETRY_RSSI() < getRssiAlarmValue(0)) { + AUDIO_RSSI_ORANGE(); + SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/); + } } } - value = newVal; - lastReceived = now(); + if (TELEMETRY_STREAMING()) { + if (telemetryState == TELEMETRY_KO) { + AUDIO_TELEMETRY_BACK(); + } + telemetryState = TELEMETRY_OK; + } + else if (telemetryState == TELEMETRY_OK) { + telemetryState = TELEMETRY_KO; + AUDIO_TELEMETRY_LOST(); + } +#endif } -bool TelemetryItem::isAvailable() +void telemetryInterrupt10ms() { - return (lastReceived != TELEMETRY_VALUE_UNAVAILABLE); -} +#if defined(FRSKY_HUB) && !defined(CPUARM) + uint16_t voltage = 0; /* unit: 1/10 volts */ + for (uint8_t i=0; i= 3600) { - currentItem.consumption.prescale -= 3600; - setValue(sensor, value+1, sensor.unit, sensor.prec); - } - lastReceived = now(); } - break; +#else + // power calculation + uint8_t channel = g_model.frsky.voltsSource; + if (channel <= FRSKY_VOLTS_SOURCE_A2) { + voltage = applyChannelRatio(channel, telemetryData.analog[channel].value) / 10; + } - default: - break; +#if defined(FRSKY_HUB) + else if (channel == FRSKY_VOLTS_SOURCE_FAS) { + voltage = telemetryData.hub.vfas; + } +#endif + +#if defined(FRSKY_HUB) + uint16_t current = telemetryData.hub.current; /* unit: 1/10 amps */ +#else + uint16_t current = 0; +#endif + + channel = g_model.frsky.currentSource - FRSKY_CURRENT_SOURCE_A1; + if (channel < MAX_FRSKY_A_CHANNELS) { + current = applyChannelRatio(channel, telemetryData.analog[channel].value) / 10; + } + + telemetryData.hub.power = ((current>>1) * (voltage>>1)) / 25; + + telemetryData.hub.currentPrescale += current; + if (telemetryData.hub.currentPrescale >= 3600) { + telemetryData.hub.currentConsumption += 1; + telemetryData.hub.currentPrescale -= 3600; + } +#endif + } + +#if !defined(CPUARM) + if (telemetryData.hub.power > telemetryData.hub.maxPower) { + telemetryData.hub.maxPower = telemetryData.hub.power; + } +#endif + } + +#if defined(WS_HOW_HIGH) + if (wshhStreaming > 0) { + wshhStreaming--; + } +#endif + + if (telemetryStreaming > 0) { + telemetryStreaming--; + } + else { +#if !defined(SIMU) +#if defined(CPUARM) + telemetryData.rssi.reset(); +#else + telemetryData.rssi[0].set(0); + telemetryData.rssi[1].set(0); +#endif +#endif } } -void TelemetryItem::eval(const TelemetrySensor & sensor) +void telemetryReset() { - switch (sensor.formula) { - case TELEM_FORMULA_CELL: - if (sensor.cell.source) { - TelemetryItem & cellsItem = telemetryItems[sensor.cell.source-1]; - if (cellsItem.isOld()) { - lastReceived = TELEMETRY_VALUE_OLD; - } - else { - unsigned int index = sensor.cell.index; - if (index == TELEM_CELL_INDEX_LOWEST || index == TELEM_CELL_INDEX_HIGHEST || index == TELEM_CELL_INDEX_DELTA) { - unsigned int lowest=0, highest=0; - for (int i=0; i cellsItem.cells.values[highest-1].value) - highest = i+1; - } - else { - lowest = highest = 0; - } - } - if (lowest) { - switch (index) { - case TELEM_CELL_INDEX_LOWEST: - setValue(sensor, cellsItem.cells.values[lowest-1].value, UNIT_VOLTS, 2); - break; - case TELEM_CELL_INDEX_HIGHEST: - setValue(sensor, cellsItem.cells.values[highest-1].value, UNIT_VOLTS, 2); - break; - case TELEM_CELL_INDEX_DELTA: - setValue(sensor, cellsItem.cells.values[highest-1].value - cellsItem.cells.values[lowest-1].value, UNIT_VOLTS, 2); - break; - } - } - } - else { - index -= 1; - if (index < cellsItem.cells.count && cellsItem.cells.values[index].state) { - setValue(sensor, cellsItem.cells.values[index].value, UNIT_VOLTS, 2); - } - } - } - } - break; + memclear(&telemetryData, sizeof(telemetryData)); - case TELEM_FORMULA_DIST: - if (sensor.dist.gps) { - TelemetryItem gpsItem = telemetryItems[sensor.dist.gps-1]; - TelemetryItem * altItem = NULL; - if (!gpsItem.isAvailable()) { - return; - } - else if (gpsItem.isOld()) { - lastReceived = TELEMETRY_VALUE_OLD; - return; - } - if (sensor.dist.alt) { - altItem = &telemetryItems[sensor.dist.alt-1]; - if (!altItem->isAvailable()) { - return; - } - else if (altItem->isOld()) { - lastReceived = TELEMETRY_VALUE_OLD; - return; - } - } - uint32_t latitude, longitude; - gpsItem.gps.extractLatitudeLongitude(&latitude, &longitude); +#if defined(CPUARM) + for (int index=0; index gpsItem.pilotLatitude) ? latitude - gpsItem.pilotLatitude : gpsItem.pilotLatitude - latitude; - uint32_t dist = EARTH_RADIUS * angle / 1000000; - uint32_t result = dist*dist; + telemetryStreaming = 0; // reset counter only if valid frsky packets are being detected + link_counter = 0; - angle = (longitude > gpsItem.pilotLongitude) ? longitude - gpsItem.pilotLongitude : gpsItem.pilotLongitude - longitude; - dist = gpsItem.distFromEarthAxis * angle / 1000000; - result += dist*dist; +#if defined(CPUARM) + telemetryState = TELEMETRY_INIT; +#endif - if (altItem) { - dist = abs(altItem->value) / g_model.telemetrySensors[sensor.dist.alt-1].getPrecDivisor(); - result += dist*dist; - } +#if defined(FRSKY_HUB) && !defined(CPUARM) + telemetryData.hub.gpsLatitude_bp = 2; + telemetryData.hub.gpsLongitude_bp = 2; + telemetryData.hub.gpsFix = -1; +#endif - setValue(sensor, isqrt32(result), UNIT_METERS); - } - break; +#if defined(SIMU) - case TELEM_FORMULA_ADD: - case TELEM_FORMULA_AVERAGE: - case TELEM_FORMULA_MIN: - case TELEM_FORMULA_MAX: - case TELEM_FORMULA_MULTIPLY: +#if defined(CPUARM) + telemetryData.swr.value = 30; + telemetryData.rssi.value = 75; +#else + telemetryData.rssi[0].value = 75; + telemetryData.rssi[1].value = 75; + telemetryData.analog[TELEM_ANA_A1].set(120, UNIT_VOLTS); + telemetryData.analog[TELEM_ANA_A2].set(240, UNIT_VOLTS); +#endif + +#if !defined(CPUARM) + telemetryData.hub.fuelLevel = 75; + telemetryData.hub.rpm = 12000; + telemetryData.hub.vfas = 100; + telemetryData.hub.minVfas = 90; + +#if defined(GPS) + telemetryData.hub.gpsFix = 1; + telemetryData.hub.gpsLatitude_bp = 4401; + telemetryData.hub.gpsLatitude_ap = 7710; + telemetryData.hub.gpsLongitude_bp = 1006; + telemetryData.hub.gpsLongitude_ap = 8872; + telemetryData.hub.gpsSpeed_bp = 200; //in knots + telemetryData.hub.gpsSpeed_ap = 0; + getGpsPilotPosition(); + + telemetryData.hub.gpsLatitude_bp = 4401; + telemetryData.hub.gpsLatitude_ap = 7455; + telemetryData.hub.gpsLongitude_bp = 1006; + telemetryData.hub.gpsLongitude_ap = 9533; + getGpsDistance(); +#endif + + telemetryData.hub.airSpeed = 1000; // 185.1 km/h + + telemetryData.hub.cellsCount = 6; + telemetryData.hub.cellVolts[0] = 410/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER; + telemetryData.hub.cellVolts[1] = 420/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER; + telemetryData.hub.cellVolts[2] = 430/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER; + telemetryData.hub.cellVolts[3] = 440/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER; + telemetryData.hub.cellVolts[4] = 450/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER; + telemetryData.hub.cellVolts[5] = 460/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER; + telemetryData.hub.minCellVolts = 250/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER; + telemetryData.hub.minCell = 300; //unit 10mV + telemetryData.hub.minCells = 220; //unit 100mV + //telemetryData.hub.cellsSum = 261; //calculated from cellVolts[] + + telemetryData.hub.gpsAltitude_bp = 50; + telemetryData.hub.baroAltitude_bp = 50; + telemetryData.hub.minAltitude = 10; + telemetryData.hub.maxAltitude = 500; + + telemetryData.hub.accelY = 100; + telemetryData.hub.temperature1 = -30; + telemetryData.hub.maxTemperature1 = 100; + + telemetryData.hub.current = 55; + telemetryData.hub.maxCurrent = 65; +#endif +#endif + +/*Add some default sensor values to the simulator*/ +#if defined(CPUARM) && defined(SIMU) + for (int i=0; i(value, sensorValue)); - else if (sensor.formula == TELEM_FORMULA_MAX) - value = (count==1 ? sensorValue : max(value, sensorValue)); - else - value += sensorValue; - } - } - } - if (sensor.formula == TELEM_FORMULA_AVERAGE) { - if (count == 0) { - if (available) - lastReceived = TELEMETRY_VALUE_OLD; - return; - } - else { - value = (value + count/2) / count; - } - } - else if (sensor.formula == TELEM_FORMULA_MULTIPLY) { - if (count == 0) - return; - value = convertTelemetryValue(value, sensor.unit, mulprec, sensor.unit, sensor.prec); - } - setValue(sensor, value, sensor.unit, sensor.prec); - break; - } - - default: - break; - } -} - -void delTelemetryIndex(uint8_t index) -{ - memclear(&g_model.telemetrySensors[index], sizeof(TelemetrySensor)); - telemetryItems[index].clear(); - storageDirty(EE_MODEL); -} - -int availableTelemetryIndex() -{ - for (int index=0; index=0; index--) { - TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index]; - if (telemetrySensor.isAvailable()) { - return index; - } - } - return -1; -} - -void setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec) -{ - bool available = false; - - for (int index=0; index= 0) { - switch (protocol) { -#if defined(FRSKY_SPORT) - case TELEM_PROTO_FRSKY_SPORT: - frskySportSetDefault(index, id, subId, instance); + case RSSI_ID: + setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, RSSI_ID, 0, sensor.instance , 75, UNIT_RAW, 0); break; + case ADC1_ID: + setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, ADC1_ID, 0, sensor.instance, 100, UNIT_RAW, 0); + break; + case ADC2_ID: + setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, ADC2_ID, 0, sensor.instance, 245, UNIT_RAW, 0); + break; + case SWR_ID: + setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, SWR_ID, 0, sensor.instance, 30, UNIT_RAW, 0); + break; + case BATT_ID: + setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, BATT_ID, 0, sensor.instance, 100, UNIT_RAW, 0); + break; + } + } #endif -#if defined(FRSKY) - case TELEM_PROTO_FRSKY_D: - frskyDSetDefault(index, id); - break; +} + +#if defined(CPUARM) +// we don't reset the telemetry here as we would also reset the consumption after model load +void telemetryInit(uint8_t protocol) +{ + if (protocol == PROTOCOL_FRSKY_D) { + telemetryPortInit(FRSKY_D_BAUDRATE); + } +#if defined(CROSSFIRE) + else if (protocol == PROTOCOL_PULSES_CROSSFIRE) { + telemetryPortInit(CROSSFIRE_BAUDRATE); + telemetryPortSetDirectionOutput(); + } #endif - default: - return; - } - telemetryItems[index].setValue(g_model.telemetrySensors[index], value, unit, prec); + else if (protocol == PROTOCOL_FRSKY_D_SECONDARY) { + telemetryPortInit(0); + serial2TelemetryInit(PROTOCOL_FRSKY_D_SECONDARY); } else { - POPUP_WARNING(STR_TELEMETRYFULL); + telemetryPortInit(FRSKY_SPORT_BAUDRATE); } -} -void TelemetrySensor::init(const char * label, uint8_t unit, uint8_t prec) -{ - memclear(this->label, TELEM_LABEL_LEN); - strncpy(this->label, label, TELEM_LABEL_LEN); - this->unit = unit; - if (prec > 1 && (IS_DISTANCE_UNIT(unit) || IS_SPEED_UNIT(unit))) { - // 2 digits precision is not needed here - prec = 1; - } - this->prec = prec; -} - -void TelemetrySensor::init(uint16_t id) -{ - char label[4]; - label[0] = hex2zchar((id & 0xf000) >> 12); - label[1] = hex2zchar((id & 0x0f00) >> 8); - label[2] = hex2zchar((id & 0x00f0) >> 4); - label[3] = hex2zchar((id & 0x000f) >> 0); - init(label); -} - -bool TelemetrySensor::isAvailable() const -{ - return ZLEN(label) > 0; -} - -PACK(typedef struct { - uint8_t unitFrom; - uint8_t unitTo; - int16_t multiplier; - int16_t divisor; -}) UnitConversionRule; - -const UnitConversionRule unitConversionTable[] = { - /* unitFrom unitTo multiplier divisor */ - { UNIT_METERS, UNIT_FEET, 105, 32}, - { UNIT_METERS_PER_SECOND, UNIT_FEET_PER_SECOND, 105, 32}, - - { UNIT_KTS, UNIT_KMH, 1852, 1000}, // 1 knot = 1.85200 kilometers per hour - { UNIT_KTS, UNIT_MPH, 1151, 1000}, // 1 knot = 1.15077945 miles per hour - { UNIT_KTS, UNIT_METERS_PER_SECOND, 1000, 1944}, // 1 knot = 0.514444444 meters / second (divide with 1.94384449) - { UNIT_KTS, UNIT_FEET_PER_SECOND, 1688, 1000}, // 1 knot = 1.68780986 feet per second - - { UNIT_KMH, UNIT_KTS, 1000, 1852}, // 1 km/h = 0.539956803 knots (divide with 1.85200) - { UNIT_KMH, UNIT_MPH, 1000, 1609}, // 1 km/h = 0.621371192 miles per hour (divide with 1.60934400) - { UNIT_KMH, UNIT_METERS_PER_SECOND, 10, 36}, // 1 km/h = 0.277777778 meters / second (divide with 3.6) - { UNIT_KMH, UNIT_FEET_PER_SECOND, 911, 1000}, // 1 km/h = 0.911344415 feet per second - - { UNIT_MILLILITERS, UNIT_FLOZ, 100, 2957}, - { 0, 0, 0, 0} // termination -}; - -int32_t convertTelemetryValue(int32_t value, uint8_t unit, uint8_t prec, uint8_t destUnit, uint8_t destPrec) -{ - for (int i=prec; idivisor) { - if (p->unitFrom == unit && p->unitTo == destUnit) { - value = (value * (int32_t)p->multiplier) / (int32_t)p->divisor; - break; - } - ++p; - } + clearMFP(); } - - for (int i=destPrec; iprec == 2) { - value *= 10; - prec = 2; - } - else { - prec = 1; - } - value = (custom.ratio * value + 122) / 255; - } - - value = convertTelemetryValue(value, unit, prec, this->unit, this->prec); - - if (type == TELEM_TYPE_CUSTOM) { - value += custom.offset; - if (value < 0 && onlyPositive) { - value = 0; - } - } - - return value; + telemetryPortInit(); } +#endif -bool TelemetrySensor::isConfigurable() const +NOINLINE uint8_t getRssiAlarmValue(uint8_t alarm) { - if (type == TELEM_TYPE_CALCULATED) { - if (formula >= TELEM_FORMULA_CELL) { - return false; - } - } - else { - if (unit >= UNIT_FIRST_VIRTUAL) { - return false; - } - } - return true; + return (45 - 3*alarm + g_model.frsky.rssiAlarms[alarm].value); } -bool TelemetrySensor::isPrecConfigurable() const -{ - if (isConfigurable()) { - return true; - } - else if (unit == UNIT_CELLS) { - return true; - } - else { - return false; - } -} - -int32_t TelemetrySensor::getPrecMultiplier() const -{ - /* - Important: the return type must be signed, otherwise - mathematic operations with a negative telemetry value won't work - */ - if (prec == 2) return 1; - if (prec == 1) return 10; - return 100; -} - -int32_t TelemetrySensor::getPrecDivisor() const -{ - if (prec == 2) return 100; - if (prec == 1) return 10; - return 1; -} +#if defined(LUA) +Fifo * luaInputTelemetryFifo = NULL; +Fifo * luaOutputTelemetryFifo = NULL; +#endif diff --git a/radio/src/telemetry/telemetry.h b/radio/src/telemetry/telemetry.h index 36b3f695d..7ceb1e218 100644 --- a/radio/src/telemetry/telemetry.h +++ b/radio/src/telemetry/telemetry.h @@ -21,12 +21,41 @@ #ifndef _TELEMETRY_H_ #define _TELEMETRY_H_ -enum TelemetryProtocol -{ - TELEM_PROTO_FRSKY_D, - TELEM_PROTO_FRSKY_SPORT, -}; +#if defined (FRSKY) + // FrSky Telemetry + #include "frsky.h" +#elif defined(JETI) + // Jeti-DUPLEX Telemetry + #include "jeti.h" +#elif defined(ARDUPILOT) + // ArduPilot Telemetry + #include "ardupilot.h" +#elif defined(NMEA) + // NMEA Telemetry + #include "nmea.h" +#elif defined(MAVLINK) + // Mavlink Telemetry + #include "mavlink.h" +#endif +extern uint8_t telemetryStreaming; // >0 (true) == data is streaming in. 0 = no data detected for some time + +#if defined(WS_HOW_HIGH) +extern uint8_t wshhStreaming; +#endif + +extern uint8_t link_counter; + +#if defined(CPUARM) +enum TelemetryStates { + TELEMETRY_INIT, + TELEMETRY_OK, + TELEMETRY_KO +}; +extern uint8_t telemetryState; +#endif + +#if defined(CPUARM) #define TELEMETRY_VALUE_TIMER_CYCLE 200 /*20 seconds*/ #define TELEMETRY_VALUE_OLD_THRESHOLD 150 /*15 seconds*/ #define TELEMETRY_VALUE_UNAVAILABLE 255 @@ -60,108 +89,32 @@ PACK(struct CellValue } }); -class TelemetryItem -{ - public: - union { - int32_t value; // value, stored as uint32_t but interpreted accordingly to type - uint32_t distFromEarthAxis; - }; - - union { - int32_t valueMin; // min store - uint32_t pilotLongitude; - }; - - union { - int32_t valueMax; // max store - uint32_t pilotLatitude; - }; - - uint8_t lastReceived; // for detection of sensor loss - - union { - struct { - int32_t offsetAuto; - int32_t filterValues[TELEMETRY_AVERAGE_COUNT]; - } std; - struct { - uint16_t prescale; - } consumption; - struct { - uint8_t count; - CellValue values[6]; - } cells; - struct { - uint8_t datestate; - uint16_t year; - uint8_t month; - uint8_t day; - uint8_t timestate; - uint8_t hour; - uint8_t min; - uint8_t sec; - } datetime; - struct { - uint16_t longitude_bp; - uint16_t longitude_ap; - char longitudeEW; - uint16_t latitude_bp; - uint16_t latitude_ap; - char latitudeNS; - // pilot longitude is stored in min - // pilot latitude is stored in max - // distFromEarthAxis is stored in value - void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude) - { - div_t qr = div(latitude_bp, 100); - *latitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + latitude_ap) * 5) / 3; - qr = div(longitude_bp, 100); - *longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + longitude_ap) * 5) / 3; - } - } gps; - }; - - static uint8_t now() - { - return (get_tmr10ms() / 10) % TELEMETRY_VALUE_TIMER_CYCLE; - } - - TelemetryItem() - { - clear(); - } - - void clear() - { - memset(this, 0, sizeof(*this)); - lastReceived = TELEMETRY_VALUE_UNAVAILABLE; - } - - void eval(const TelemetrySensor & sensor); - void per10ms(const TelemetrySensor & sensor); - - void setValue(const TelemetrySensor & sensor, int32_t newVal, uint32_t unit, uint32_t prec=0); - bool isAvailable(); - bool isFresh(); - bool isOld(); - void gpsReceived(); -}; - -extern TelemetryItem telemetryItems[MAX_SENSORS]; -extern uint8_t allowNewSensors; - void setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec); void delTelemetryIndex(uint8_t index); int availableTelemetryIndex(); int lastUsedTelemetryIndex(); + int32_t getTelemetryValue(uint8_t index, uint8_t & prec); int32_t convertTelemetryValue(int32_t value, uint8_t unit, uint8_t prec, uint8_t destUnit, uint8_t destPrec); void frskySportSetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance); void frskyDSetDefault(int index, uint16_t id); +#endif #define IS_DISTANCE_UNIT(unit) ((unit) == UNIT_METERS || (unit) == UNIT_FEET) #define IS_SPEED_UNIT(unit) ((unit) >= UNIT_KTS && (unit) <= UNIT_MPH) +#if defined(CPUARM) +extern uint8_t telemetryProtocol; +#define IS_FRSKY_D_PROTOCOL() (telemetryProtocol == PROTOCOL_FRSKY_D) +#define IS_FRSKY_SPORT_PROTOCOL() (telemetryProtocol == PROTOCOL_FRSKY_SPORT) +#else +#define IS_FRSKY_D_PROTOCOL() (true) +#define IS_FRSKY_SPORT_PROTOCOL() (false) +#endif + +#if defined(CPUARM) + #include "telemetry_sensors.h" +#endif + #endif // _TELEMETRY_H_ diff --git a/radio/src/telemetry/telemetry_holders.cpp b/radio/src/telemetry/telemetry_holders.cpp new file mode 100644 index 000000000..edcbc14d2 --- /dev/null +++ b/radio/src/telemetry/telemetry_holders.cpp @@ -0,0 +1,77 @@ +/* + * Copyright (C) OpenTX + * + * Based on code named + * th9x - http://code.google.com/p/th9x + * er9x - http://code.google.com/p/er9x + * gruvin9x - http://code.google.com/p/gruvin9x + * + * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program 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. + */ + +#include "opentx.h" + +#if defined(CPUARM) +void TelemetryValueWithMin::reset() +{ + memclear(this, sizeof(*this)); +} +#endif + +void TelemetryValueWithMin::set(uint8_t value) +{ +#if defined(CPUARM) + if (this->value == 0) { + memset(values, value, TELEMETRY_AVERAGE_COUNT); + this->value = value; + } + else { + //calculate the average from values[] and value + //also shift readings in values [] array + unsigned int sum = values[0]; + for (int i=0; ivalue = sum/(TELEMETRY_AVERAGE_COUNT+1); + } +#else + if (this->value == 0) { + this->value = value; + } + else { + sum += value; + if (link_counter == 0) { + this->value = sum / (IS_FRSKY_D_PROTOCOL() ? FRSKY_D_AVERAGING : FRSKY_SPORT_AVERAGING); + sum = 0; + } + } +#endif + + if (!min || value < min) { + min = value; + } +} + +void TelemetryValueWithMinMax::set(uint8_t value, uint8_t unit) +{ + TelemetryValueWithMin::set(value); + if (unit != UNIT_VOLTS) { + this->value = value; + } + if (!max || value > max) { + max = value; + } +} diff --git a/radio/src/telemetry/telemetry_holders.h b/radio/src/telemetry/telemetry_holders.h new file mode 100644 index 000000000..1068686d9 --- /dev/null +++ b/radio/src/telemetry/telemetry_holders.h @@ -0,0 +1,54 @@ +/* + * Copyright (C) OpenTX + * + * Based on code named + * th9x - http://code.google.com/p/th9x + * er9x - http://code.google.com/p/er9x + * gruvin9x - http://code.google.com/p/gruvin9x + * + * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program 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. + */ + +#ifndef _TELEMETRY_HOLDERS_H_ +#define _TELEMETRY_HOLDERS_H_ + +#include + +#if defined(CPUARM) +#define TELEMETRY_AVERAGE_COUNT 3 // we actually average one more reading! +#define RAW_FRSKY_MINMAX(v) v.values[TELEMETRY_AVERAGE_COUNT-1] +class TelemetryValueWithMin { + public: + uint8_t value; // fitered value (average of last TELEMETRY_AVERAGE_COUNT+1 values) + uint8_t min; + uint8_t values[TELEMETRY_AVERAGE_COUNT]; + void set(uint8_t value); + void reset(); +}; +#else +#define RAW_FRSKY_MINMAX(v) v.value +class TelemetryValueWithMin { + public: + uint8_t value; + uint8_t min; + uint16_t sum; + void set(uint8_t value); +}; +#endif + +class TelemetryValueWithMinMax: public TelemetryValueWithMin { + public: + uint8_t max; + void set(uint8_t value, uint8_t unit); +}; + +#endif // _TELEMETRY_HOLDERS_H_ \ No newline at end of file diff --git a/radio/src/telemetry/telemetry_sensors.cpp b/radio/src/telemetry/telemetry_sensors.cpp new file mode 100644 index 000000000..294c2a951 --- /dev/null +++ b/radio/src/telemetry/telemetry_sensors.cpp @@ -0,0 +1,693 @@ +/* + * Copyright (C) OpenTX + * + * Based on code named + * th9x - http://code.google.com/p/th9x + * er9x - http://code.google.com/p/er9x + * gruvin9x - http://code.google.com/p/gruvin9x + * + * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program 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. + */ + +#include "opentx.h" + +TelemetryItem telemetryItems[MAX_SENSORS]; +uint8_t allowNewSensors; + +void TelemetryItem::gpsReceived() +{ + if (!distFromEarthAxis) { + gps.extractLatitudeLongitude(&pilotLatitude, &pilotLongitude); + uint32_t lat = pilotLatitude / 10000; + uint32_t angle2 = (lat*lat) / 10000; + uint32_t angle4 = angle2 * angle2; + distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500); + } + lastReceived = now(); +} + +void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32_t unit, uint32_t prec) +{ + int32_t newVal = val; + + if (unit == UNIT_CELLS) { + uint32_t data = uint32_t(newVal); + uint8_t cellsCount = (data >> 24); + uint8_t cellIndex = ((data >> 16) & 0x0F); + uint16_t cellValue = (data & 0xFFFF); + if (cellsCount == 0) { + cellsCount = (cellIndex >= cells.count ? cellIndex + 1 : cells.count); + if (cellsCount != cells.count) { + clear(); + cells.count = cellsCount; + // we skip this round as we are not sure we received all cells values + return; + } + } + else if (cellsCount != cells.count) { + clear(); + cells.count = cellsCount; + } + cells.values[cellIndex].set(cellValue); + if (cellIndex+1 == cells.count) { + newVal = 0; + for (int i=0; i> 24); + datetime.month = (uint8_t) ((data & 0x00ff0000) >> 16); + datetime.day = (uint8_t) ((data & 0x0000ff00) >> 8); + if (datetime.year != 0) { + datetime.datestate = 1; + } +#if defined(RTCLOCK) + if (g_eeGeneral.adjustRTC && (datetime.datestate == 1)) { + struct gtm t; + gettime(&t); + t.tm_year = datetime.year+4; + t.tm_mon = datetime.month-1; + t.tm_mday = datetime.day; + rtcSetTime(&t); + } +#endif + } + else { + datetime.hour = ((uint8_t) ((data & 0xff000000) >> 24) + g_eeGeneral.timezone + 24) % 24; + datetime.min = (uint8_t) ((data & 0x00ff0000) >> 16); + datetime.sec = (uint16_t) ((data & 0x0000ff00) >> 8); + if (datetime.datestate == 1) { + datetime.timestate = 1; + } +#if defined(RTCLOCK) + if (g_eeGeneral.adjustRTC && datetime.datestate == 1) { + struct gtm t; + gettime(&t); + if (abs((t.tm_hour-datetime.hour)*3600 + (t.tm_min-datetime.min)*60 + (t.tm_sec-datetime.sec)) > 20) { + // we adjust RTC only if difference is > 20 seconds + t.tm_hour = datetime.hour; + t.tm_min = datetime.min; + t.tm_sec = datetime.sec; + g_rtcTime = gmktime(&t); // update local timestamp and get wday calculated + rtcSetTime(&t); + } + } +#endif + } + if (datetime.year == 0) { + return; + } + newVal = 0; + } + else if (unit == UNIT_GPS) { + uint32_t gps_long_lati_data = uint32_t(newVal); + uint32_t gps_long_lati_b1w, gps_long_lati_a1w; + gps_long_lati_b1w = (gps_long_lati_data & 0x3fffffff) / 10000; + gps_long_lati_a1w = (gps_long_lati_data & 0x3fffffff) % 10000; + switch ((gps_long_lati_data & 0xc0000000) >> 30) { + case 0: + gps.latitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60); + gps.latitude_ap = gps_long_lati_a1w; + gps.latitudeNS = 'N'; + break; + case 1: + gps.latitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60); + gps.latitude_ap = gps_long_lati_a1w; + gps.latitudeNS = 'S'; + break; + case 2: + gps.longitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60); + gps.longitude_ap = gps_long_lati_a1w; + gps.longitudeEW = 'E'; + break; + case 3: + gps.longitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60); + gps.longitude_ap = gps_long_lati_a1w; + gps.longitudeEW = 'W'; + break; + } + if (gps.longitudeEW && gps.latitudeNS) { + gpsReceived(); + } + return; + } + else if (unit >= UNIT_GPS_LONGITUDE && unit <= UNIT_GPS_LATITUDE_NS) { + uint32_t data = uint32_t(newVal); + switch (unit) { + case UNIT_GPS_LONGITUDE: + gps.longitude_bp = data >> 16; + gps.longitude_ap = data & 0xFFFF; + break; + case UNIT_GPS_LATITUDE: + gps.latitude_bp = data >> 16; + gps.latitude_ap = data & 0xFFFF; + break; + case UNIT_GPS_LONGITUDE_EW: + gps.longitudeEW = data; + break; + case UNIT_GPS_LATITUDE_NS: + gps.latitudeNS = data; + break; + } + if (gps.longitudeEW && gps.latitudeNS && gps.longitude_ap && gps.latitude_ap) { + gpsReceived(); + } + return; + } + else if (unit == UNIT_DATETIME_YEAR) { + datetime.year = newVal; + return; + } + else if (unit == UNIT_DATETIME_DAY_MONTH) { + uint32_t data = uint32_t(newVal); + datetime.month = data >> 8; + datetime.day = data & 0xFF; + datetime.datestate = 1; + return; + } + else if (unit == UNIT_DATETIME_HOUR_MIN) { + uint32_t data = uint32_t(newVal); + datetime.hour = ((data & 0xFF) + g_eeGeneral.timezone + 24) % 24; + datetime.min = data >> 8; + } + else if (unit == UNIT_DATETIME_SEC) { + datetime.sec = newVal & 0xFF; + datetime.timestate = 1; + newVal = 0; + } + else if (unit == UNIT_RPMS) { + if (sensor.custom.ratio != 0) { + newVal = (newVal * sensor.custom.offset) / sensor.custom.ratio; + } + } + else { + newVal = sensor.getValue(newVal, unit, prec); + if (sensor.autoOffset) { + if (!isAvailable()) { + std.offsetAuto = -newVal; + } + newVal += std.offsetAuto; + } + else if (sensor.filter) { + if (!isAvailable()) { + for (int i=0; i valueMax) { + valueMax = newVal; + if (sensor.unit == UNIT_VOLTS) { + valueMin = newVal; // the batt was changed + } + } + + for (int i=0; i= 3600) { + currentItem.consumption.prescale -= 3600; + setValue(sensor, value+1, sensor.unit, sensor.prec); + } + lastReceived = now(); + } + break; + + default: + break; + } +} + +void TelemetryItem::eval(const TelemetrySensor & sensor) +{ + switch (sensor.formula) { + case TELEM_FORMULA_CELL: + if (sensor.cell.source) { + TelemetryItem & cellsItem = telemetryItems[sensor.cell.source-1]; + if (cellsItem.isOld()) { + lastReceived = TELEMETRY_VALUE_OLD; + } + else { + unsigned int index = sensor.cell.index; + if (index == TELEM_CELL_INDEX_LOWEST || index == TELEM_CELL_INDEX_HIGHEST || index == TELEM_CELL_INDEX_DELTA) { + unsigned int lowest=0, highest=0; + for (int i=0; i cellsItem.cells.values[highest-1].value) + highest = i+1; + } + else { + lowest = highest = 0; + } + } + if (lowest) { + switch (index) { + case TELEM_CELL_INDEX_LOWEST: + setValue(sensor, cellsItem.cells.values[lowest-1].value, UNIT_VOLTS, 2); + break; + case TELEM_CELL_INDEX_HIGHEST: + setValue(sensor, cellsItem.cells.values[highest-1].value, UNIT_VOLTS, 2); + break; + case TELEM_CELL_INDEX_DELTA: + setValue(sensor, cellsItem.cells.values[highest-1].value - cellsItem.cells.values[lowest-1].value, UNIT_VOLTS, 2); + break; + } + } + } + else { + index -= 1; + if (index < cellsItem.cells.count && cellsItem.cells.values[index].state) { + setValue(sensor, cellsItem.cells.values[index].value, UNIT_VOLTS, 2); + } + } + } + } + break; + + case TELEM_FORMULA_DIST: + if (sensor.dist.gps) { + TelemetryItem gpsItem = telemetryItems[sensor.dist.gps-1]; + TelemetryItem * altItem = NULL; + if (!gpsItem.isAvailable()) { + return; + } + else if (gpsItem.isOld()) { + lastReceived = TELEMETRY_VALUE_OLD; + return; + } + if (sensor.dist.alt) { + altItem = &telemetryItems[sensor.dist.alt-1]; + if (!altItem->isAvailable()) { + return; + } + else if (altItem->isOld()) { + lastReceived = TELEMETRY_VALUE_OLD; + return; + } + } + uint32_t latitude, longitude; + gpsItem.gps.extractLatitudeLongitude(&latitude, &longitude); + + uint32_t angle = (latitude > gpsItem.pilotLatitude) ? latitude - gpsItem.pilotLatitude : gpsItem.pilotLatitude - latitude; + uint32_t dist = EARTH_RADIUS * angle / 1000000; + uint32_t result = dist*dist; + + angle = (longitude > gpsItem.pilotLongitude) ? longitude - gpsItem.pilotLongitude : gpsItem.pilotLongitude - longitude; + dist = gpsItem.distFromEarthAxis * angle / 1000000; + result += dist*dist; + + if (altItem) { + dist = abs(altItem->value) / g_model.telemetrySensors[sensor.dist.alt-1].getPrecDivisor(); + result += dist*dist; + } + + setValue(sensor, isqrt32(result), UNIT_METERS); + } + break; + + case TELEM_FORMULA_ADD: + case TELEM_FORMULA_AVERAGE: + case TELEM_FORMULA_MIN: + case TELEM_FORMULA_MAX: + case TELEM_FORMULA_MULTIPLY: + { + int32_t value=0, count=0, available=0, maxitems=4, mulprec=0; + if (sensor.formula == TELEM_FORMULA_MULTIPLY) { + maxitems = 2; + value = 1; + } + for (int i=0; i(value, sensorValue)); + else if (sensor.formula == TELEM_FORMULA_MAX) + value = (count==1 ? sensorValue : max(value, sensorValue)); + else + value += sensorValue; + } + } + } + if (sensor.formula == TELEM_FORMULA_AVERAGE) { + if (count == 0) { + if (available) + lastReceived = TELEMETRY_VALUE_OLD; + return; + } + else { + value = (value + count/2) / count; + } + } + else if (sensor.formula == TELEM_FORMULA_MULTIPLY) { + if (count == 0) + return; + value = convertTelemetryValue(value, sensor.unit, mulprec, sensor.unit, sensor.prec); + } + setValue(sensor, value, sensor.unit, sensor.prec); + break; + } + + default: + break; + } +} + +void delTelemetryIndex(uint8_t index) +{ + memclear(&g_model.telemetrySensors[index], sizeof(TelemetrySensor)); + telemetryItems[index].clear(); + storageDirty(EE_MODEL); +} + +int availableTelemetryIndex() +{ + for (int index=0; index=0; index--) { + TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index]; + if (telemetrySensor.isAvailable()) { + return index; + } + } + return -1; +} + +void setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec) +{ + bool available = false; + + for (int index=0; index= 0) { + switch (protocol) { +#if defined(FRSKY_SPORT) + case TELEM_PROTO_FRSKY_SPORT: + frskySportSetDefault(index, id, subId, instance); + break; +#endif +#if defined(FRSKY) + case TELEM_PROTO_FRSKY_D: + frskyDSetDefault(index, id); + break; +#endif + default: + return; + } + telemetryItems[index].setValue(g_model.telemetrySensors[index], value, unit, prec); + } + else { + POPUP_WARNING(STR_TELEMETRYFULL); + } +} + +void TelemetrySensor::init(const char * label, uint8_t unit, uint8_t prec) +{ + memclear(this->label, TELEM_LABEL_LEN); + strncpy(this->label, label, TELEM_LABEL_LEN); + this->unit = unit; + if (prec > 1 && (IS_DISTANCE_UNIT(unit) || IS_SPEED_UNIT(unit))) { + // 2 digits precision is not needed here + prec = 1; + } + this->prec = prec; +} + +void TelemetrySensor::init(uint16_t id) +{ + char label[4]; + label[0] = hex2zchar((id & 0xf000) >> 12); + label[1] = hex2zchar((id & 0x0f00) >> 8); + label[2] = hex2zchar((id & 0x00f0) >> 4); + label[3] = hex2zchar((id & 0x000f) >> 0); + init(label); +} + +bool TelemetrySensor::isAvailable() const +{ + return ZLEN(label) > 0; +} + +PACK(typedef struct { + uint8_t unitFrom; + uint8_t unitTo; + int16_t multiplier; + int16_t divisor; +}) UnitConversionRule; + +const UnitConversionRule unitConversionTable[] = { + /* unitFrom unitTo multiplier divisor */ + { UNIT_METERS, UNIT_FEET, 105, 32}, + { UNIT_METERS_PER_SECOND, UNIT_FEET_PER_SECOND, 105, 32}, + + { UNIT_KTS, UNIT_KMH, 1852, 1000}, // 1 knot = 1.85200 kilometers per hour + { UNIT_KTS, UNIT_MPH, 1151, 1000}, // 1 knot = 1.15077945 miles per hour + { UNIT_KTS, UNIT_METERS_PER_SECOND, 1000, 1944}, // 1 knot = 0.514444444 meters / second (divide with 1.94384449) + { UNIT_KTS, UNIT_FEET_PER_SECOND, 1688, 1000}, // 1 knot = 1.68780986 feet per second + + { UNIT_KMH, UNIT_KTS, 1000, 1852}, // 1 km/h = 0.539956803 knots (divide with 1.85200) + { UNIT_KMH, UNIT_MPH, 1000, 1609}, // 1 km/h = 0.621371192 miles per hour (divide with 1.60934400) + { UNIT_KMH, UNIT_METERS_PER_SECOND, 10, 36}, // 1 km/h = 0.277777778 meters / second (divide with 3.6) + { UNIT_KMH, UNIT_FEET_PER_SECOND, 911, 1000}, // 1 km/h = 0.911344415 feet per second + + { UNIT_MILLILITERS, UNIT_FLOZ, 100, 2957}, + { 0, 0, 0, 0} // termination +}; + +int32_t convertTelemetryValue(int32_t value, uint8_t unit, uint8_t prec, uint8_t destUnit, uint8_t destPrec) +{ + for (int i=prec; idivisor) { + if (p->unitFrom == unit && p->unitTo == destUnit) { + value = (value * (int32_t)p->multiplier) / (int32_t)p->divisor; + break; + } + ++p; + } + } + + for (int i=destPrec; iprec == 2) { + value *= 10; + prec = 2; + } + else { + prec = 1; + } + value = (custom.ratio * value + 122) / 255; + } + + value = convertTelemetryValue(value, unit, prec, this->unit, this->prec); + + if (type == TELEM_TYPE_CUSTOM) { + value += custom.offset; + if (value < 0 && onlyPositive) { + value = 0; + } + } + + return value; +} + +bool TelemetrySensor::isConfigurable() const +{ + if (type == TELEM_TYPE_CALCULATED) { + if (formula >= TELEM_FORMULA_CELL) { + return false; + } + } + else { + if (unit >= UNIT_FIRST_VIRTUAL) { + return false; + } + } + return true; +} + +bool TelemetrySensor::isPrecConfigurable() const +{ + if (isConfigurable()) { + return true; + } + else if (unit == UNIT_CELLS) { + return true; + } + else { + return false; + } +} + +int32_t TelemetrySensor::getPrecMultiplier() const +{ + /* + Important: the return type must be signed, otherwise + mathematic operations with a negative telemetry value won't work + */ + if (prec == 2) return 1; + if (prec == 1) return 10; + return 100; +} + +int32_t TelemetrySensor::getPrecDivisor() const +{ + if (prec == 2) return 100; + if (prec == 1) return 10; + return 1; +} diff --git a/radio/src/telemetry/telemetry_sensors.h b/radio/src/telemetry/telemetry_sensors.h new file mode 100644 index 000000000..26747076b --- /dev/null +++ b/radio/src/telemetry/telemetry_sensors.h @@ -0,0 +1,115 @@ +/* + * Copyright (C) OpenTX + * + * Based on code named + * th9x - http://code.google.com/p/th9x + * er9x - http://code.google.com/p/er9x + * gruvin9x - http://code.google.com/p/gruvin9x + * + * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program 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. + */ + +#ifndef _TELEMETRY_SENSORS_H_ +#define _TELEMETRY_SENSORS_H_ + +class TelemetryItem +{ + public: + union { + int32_t value; // value, stored as uint32_t but interpreted accordingly to type + uint32_t distFromEarthAxis; + }; + + union { + int32_t valueMin; // min store + uint32_t pilotLongitude; + }; + + union { + int32_t valueMax; // max store + uint32_t pilotLatitude; + }; + + uint8_t lastReceived; // for detection of sensor loss + + union { + struct { + int32_t offsetAuto; + int32_t filterValues[TELEMETRY_AVERAGE_COUNT]; + } std; + struct { + uint16_t prescale; + } consumption; + struct { + uint8_t count; + CellValue values[6]; + } cells; + struct { + uint8_t datestate; + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t timestate; + uint8_t hour; + uint8_t min; + uint8_t sec; + } datetime; + struct { + uint16_t longitude_bp; + uint16_t longitude_ap; + char longitudeEW; + uint16_t latitude_bp; + uint16_t latitude_ap; + char latitudeNS; + // pilot longitude is stored in min + // pilot latitude is stored in max + // distFromEarthAxis is stored in value + void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude) + { + div_t qr = div(latitude_bp, 100); + *latitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + latitude_ap) * 5) / 3; + qr = div(longitude_bp, 100); + *longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + longitude_ap) * 5) / 3; + } + } gps; + }; + + static uint8_t now() + { + return (get_tmr10ms() / 10) % TELEMETRY_VALUE_TIMER_CYCLE; + } + + TelemetryItem() + { + clear(); + } + + void clear() + { + memset(this, 0, sizeof(*this)); + lastReceived = TELEMETRY_VALUE_UNAVAILABLE; + } + + void eval(const TelemetrySensor & sensor); + void per10ms(const TelemetrySensor & sensor); + + void setValue(const TelemetrySensor & sensor, int32_t newVal, uint32_t unit, uint32_t prec=0); + bool isAvailable(); + bool isFresh(); + bool isOld(); + void gpsReceived(); +}; + +extern TelemetryItem telemetryItems[MAX_SENSORS]; +extern uint8_t allowNewSensors; + +#endif // _TELEMETRY_SENSORS_H_ diff --git a/radio/src/tests/frsky.cpp b/radio/src/tests/frsky.cpp index 189f35bb6..3872e0ad6 100644 --- a/radio/src/tests/frsky.cpp +++ b/radio/src/tests/frsky.cpp @@ -34,7 +34,7 @@ void displayVoltagesScreen(); TEST(FrSky, gpsNfuel) { g_model.frsky.usrProto = 1; - frskyData.hub.gpsFix = 1; + telemetryData.hub.gpsFix = 1; uint8_t pkt1[] = { 0xfd, 0x07, 0x00, 0x5e, 0x14, 0x2c, 0x00, 0x5e, 0x1c, 0x03 }; uint8_t pkt2[] = { 0xfd, 0x07, 0x00, 0x00, 0x5e, 0x13, 0x38, 0x0c, 0x5e, 0x1b }; @@ -50,14 +50,14 @@ TEST(FrSky, gpsNfuel) frskyDProcessPacket(pkt5); frskyDProcessPacket(pkt6); frskyDProcessPacket(pkt7); - EXPECT_EQ(frskyData.hub.gpsCourse_bp, 44); - EXPECT_EQ(frskyData.hub.gpsCourse_ap, 03); - EXPECT_EQ(frskyData.hub.gpsLongitude_bp / 100, 120); - EXPECT_EQ(frskyData.hub.gpsLongitude_bp % 100, 15); - EXPECT_EQ(frskyData.hub.gpsLongitude_ap, 0x2698); - EXPECT_EQ(frskyData.hub.gpsLatitudeNS, 'N'); - EXPECT_EQ(frskyData.hub.gpsLongitudeEW, 'E'); - EXPECT_EQ(frskyData.hub.fuelLevel, 100); + EXPECT_EQ(telemetryData.hub.gpsCourse_bp, 44); + EXPECT_EQ(telemetryData.hub.gpsCourse_ap, 03); + EXPECT_EQ(telemetryData.hub.gpsLongitude_bp / 100, 120); + EXPECT_EQ(telemetryData.hub.gpsLongitude_bp % 100, 15); + EXPECT_EQ(telemetryData.hub.gpsLongitude_ap, 0x2698); + EXPECT_EQ(telemetryData.hub.gpsLatitudeNS, 'N'); + EXPECT_EQ(telemetryData.hub.gpsLongitudeEW, 'E'); + EXPECT_EQ(telemetryData.hub.fuelLevel, 100); } TEST(FrSky, dateNtime) @@ -68,17 +68,17 @@ TEST(FrSky, dateNtime) frskyDProcessPacket(pkt1); frskyDProcessPacket(pkt2); frskyDProcessPacket(pkt3); - EXPECT_EQ(frskyData.hub.day, 15); - EXPECT_EQ(frskyData.hub.month, 07); - EXPECT_EQ(frskyData.hub.year, 11); - EXPECT_EQ(frskyData.hub.hour, 06); - EXPECT_EQ(frskyData.hub.min, 18); - EXPECT_EQ(frskyData.hub.sec, 50); + EXPECT_EQ(telemetryData.hub.day, 15); + EXPECT_EQ(telemetryData.hub.month, 07); + EXPECT_EQ(telemetryData.hub.year, 11); + EXPECT_EQ(telemetryData.hub.hour, 06); + EXPECT_EQ(telemetryData.hub.min, 18); + EXPECT_EQ(telemetryData.hub.sec, 50); } #endif #if defined(FRSKY) && defined(CPUARM) -TEST(FrSky, FrskyValueWithMinAveraging) +TEST(FrSky, TelemetryValueWithMinAveraging) { /* The following expected[] array is filled @@ -90,7 +90,7 @@ TEST(FrSky, FrskyValueWithMinAveraging) uint8_t expected[] = { 10, 12, 17, 25, 35, 45, 55, 65, 75, 85, 92, 97, 100, 100, 100, 100, 100}; int testPos = 0; //test of averaging - FrskyValueWithMin testVal; + TelemetryValueWithMin testVal; testVal.value = 0; testVal.set(10); EXPECT_EQ(RAW_FRSKY_MINMAX(testVal), 10); diff --git a/radio/src/tests/gtests.h b/radio/src/tests/gtests.h index 5dd7db858..07e7a14e8 100644 --- a/radio/src/tests/gtests.h +++ b/radio/src/tests/gtests.h @@ -79,7 +79,7 @@ inline void MIXER_RESET() inline void TELEMETRY_RESET() { #if defined(FRSKY) - memclear(&frskyData, sizeof(frskyData)); + memclear(&telemetryData, sizeof(telemetryData)); TELEMETRY_RSSI() = 100; #endif #if defined(CPUARM) && defined(FRSKY) diff --git a/radio/src/vario.cpp b/radio/src/vario.cpp index 67f195e4e..2d81568e9 100644 --- a/radio/src/vario.cpp +++ b/radio/src/vario.cpp @@ -80,7 +80,7 @@ void varioWakeup() if (isFunctionActive(FUNCTION_VARIO)) { #if defined(AUDIO) cli(); - int16_t verticalSpeed = frskyData.hub.varioSpeed; + int16_t verticalSpeed = telemetryData.hub.varioSpeed; sei(); #if defined(PCBSTD) @@ -126,7 +126,7 @@ void varioWakeup() #elif defined(BUZZER) // && !defined(AUDIO) - int8_t verticalSpeed = limit((int16_t)-100, (int16_t)(frskyData.hub.varioSpeed/10), (int16_t)+100); + int8_t verticalSpeed = limit((int16_t)-100, (int16_t)(telemetryData.hub.varioSpeed/10), (int16_t)+100); uint16_t interval; if (verticalSpeed == 0) {