diff --git a/radio/src/gui/128x64/radio_diaganas.cpp b/radio/src/gui/128x64/radio_diaganas.cpp index d7c3275d0..46b3a1bbf 100644 --- a/radio/src/gui/128x64/radio_diaganas.cpp +++ b/radio/src/gui/128x64/radio_diaganas.cpp @@ -67,26 +67,4 @@ void menuRadioDiagAnalogs(event_t event) lcdDrawChar(lcdNextPos, y, '@'); lcdDrawNumber(x+10*FW-1, y, gyro.scaledY(), RIGHT); #endif - - // RAS -#if defined(PCBTARANIS) - if (isModuleXJT2(INTERNAL_MODULE) && IS_INTERNAL_MODULE_ON()) { - y += FH; - lcdDrawText(1, y, "RAS:"); - lcdDrawNumber(1 + 4*FW, y, telemetryData.swr.value, LEFT); - } - else if (isModuleXJT(EXTERNAL_MODULE) && !IS_INTERNAL_MODULE_ON()) { - y += FH; - lcdDrawText(1, y, "RAS:"); - lcdDrawNumber(1 + 4*FW, y, telemetryData.swr.value, LEFT); - } -#else - if (isModuleXJT(EXTERNAL_MODULE)) { - y += FH; - uint8_t x = ((NUM_STICKS+NUM_POTS+NUM_SLIDERS) & 1) ? (LCD_W/2)+FW : 0; - - lcdDrawText(x, y, "RAS:"); - lcdDrawNumber(x + 4*FW, y, telemetryData.swr.value, LEFT); - } -#endif } diff --git a/radio/src/gui/128x64/radio_hardware.cpp b/radio/src/gui/128x64/radio_hardware.cpp index 9203b40ea..753607fcd 100644 --- a/radio/src/gui/128x64/radio_hardware.cpp +++ b/radio/src/gui/128x64/radio_hardware.cpp @@ -138,6 +138,7 @@ enum MenuRadioHardwareItems { ITEM_RADIO_HARDWARE_BLUETOOTH_NAME, #endif ITEM_RADIO_HARDWARE_JITTER_FILTER, + ITEM_RADIO_HARDWARE_RAS, ITEM_RADIO_HARDWARE_DEBUG, ITEM_RADIO_HARDWARE_MAX }; @@ -207,6 +208,7 @@ void menuRadioHardware(event_t event) #endif BLUETOOTH_ROWS 0 /* jitter filter */, + READONLY_ROW, 1 /* debugs */, }); @@ -397,6 +399,19 @@ void menuRadioHardware(event_t event) g_eeGeneral.jitterFilter = 1 - editCheckBox(1 - g_eeGeneral.jitterFilter, HW_SETTINGS_COLUMN2, y, STR_JITTER_FILTER, attr, event); break; + case ITEM_RADIO_HARDWARE_RAS: + lcdDrawTextAlignedLeft(y, "RAS"); + if (telemetryData.swrInternal.value) + lcdDrawNumber(HW_SETTINGS_COLUMN2, y, telemetryData.swrInternal.value); + else + lcdDrawText(HW_SETTINGS_COLUMN2, y, "---"); + lcdDrawText(lcdNextPos, y, "/"); + if (telemetryData.swrInternal.value) + lcdDrawNumber(lcdNextPos, y, telemetryData.swrInternal.value); + else + lcdDrawText(lcdNextPos, y, "---"); + break; + case ITEM_RADIO_HARDWARE_DEBUG: lcdDrawTextAlignedLeft(y, STR_DEBUG); lcdDrawText(HW_SETTINGS_COLUMN2, y, STR_ANALOGS_BTN, menuHorizontalPosition == 0 ? attr : 0); diff --git a/radio/src/lua/api_general.cpp b/radio/src/lua/api_general.cpp index 31bfa86b5..97ec2467a 100644 --- a/radio/src/lua/api_general.cpp +++ b/radio/src/lua/api_general.cpp @@ -672,8 +672,8 @@ This is just a hardware pass/fail measure and does not represent the quality of */ static int luaGetRAS(lua_State * L) { - if (IS_RAS_VALUE_VALID()) { - lua_pushinteger(L, telemetryData.swr.value); + if (!IS_RAS_VALUE_VALID(telemetryData.xjtVersion)) { + lua_pushinteger(L, telemetryData.swrInternal.value); } else { lua_pushnil(L); diff --git a/radio/src/targets/common/arm/CMakeLists.txt b/radio/src/targets/common/arm/CMakeLists.txt index da513bb66..ef4276798 100644 --- a/radio/src/targets/common/arm/CMakeLists.txt +++ b/radio/src/targets/common/arm/CMakeLists.txt @@ -141,7 +141,6 @@ set(SRC tasks.cpp audio.cpp telemetry/telemetry.cpp - telemetry/telemetry_holders.cpp telemetry/telemetry_sensors.cpp telemetry/frsky.cpp telemetry/frsky_d.cpp diff --git a/radio/src/telemetry/frsky.h b/radio/src/telemetry/frsky.h index da6bc98c4..2f21ea88c 100644 --- a/radio/src/telemetry/frsky.h +++ b/radio/src/telemetry/frsky.h @@ -205,69 +205,57 @@ enum FrSkyDataState { #define DATA_ID_SP2UH 0x45 // 5 #define DATA_ID_SP2UR 0xC6 // 6 -#if defined(NO_RAS) - #define IS_RAS_VALUE_VALID() (false) -#elif defined(PCBX10) - #define IS_RAS_VALUE_VALID() (false) -#elif defined(PCBX9DP) || defined(PCBX9E) - #define IS_RAS_VALUE_VALID() (telemetryData.xjtVersion != 0 && telemetryData.xjtVersion != 0xff) -#else - #define IS_RAS_VALUE_VALID() (true) -#endif - #define IS_HIDDEN_TELEMETRY_VALUE(id) ((id == SP2UART_A_ID) || (id == SP2UART_B_ID) || (id == XJT_VERSION_ID) || (id == RAS_ID) || (id == FACT_TEST_ID)) #define ALARM_GREATER(channel, alarm) ((g_model.frsky.channels[channel].alarms_greater >> alarm) & 1) #define ALARM_LEVEL(channel, alarm) ((g_model.frsky.channels[channel].alarms_level >> (2*alarm)) & 3) - #define TELEMETRY_STREAMING() (telemetryData.rssi.value > 0) - #define TELEMETRY_RSSI() (telemetryData.rssi.value) - #define TELEMETRY_RSSI_MIN() (telemetryData.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_CELL_VOLTAGE_MUTLIPLIER 1 - #define TELEMETRY_GPS_SPEED_BP telemetryData.hub.gpsSpeed_bp - #define TELEMETRY_GPS_SPEED_AP telemetryData.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 (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_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 (telemetryData.hub.baroAltitude / 100) - #define TELEMETRY_RELATIVE_BARO_ALT_AP (telemetryData.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(telemetryData.hub.baroAltitude, UNIT_DIST), 100) - #define TELEMETRY_BARO_ALT_FORMAT "%c%d.%02d," - #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 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 telemetryData.hub.gpsSpeed_bp, +#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 telemetryData.hub.baroAltitude < 0 ? '-' : ' ', abs(baroAltitudeDivision.quot), abs(baroAltitudeDivision.rem), +#define TELEMETRY_GPS_ALT_FORMAT "%c%d.%02d," +#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 telemetryData.hub.gpsSpeed_bp, - #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," +#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," - #define TELEMETRY_CURRENT_FORMAT "%d.%d," - #define TELEMETRY_CURRENT_ARGS telemetryData.hub.current / 10, telemetryData.hub.current % 10, - #define TELEMETRY_VFAS_FORMAT "%d.%d," - #define TELEMETRY_VFAS_ARGS telemetryData.hub.vfas / 10, telemetryData.hub.vfas % 10, - #define TELEMETRY_VSPEED_FORMAT "%c%d.%02d," - #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 telemetryData.hub.airSpeed / 10, telemetryData.hub.airSpeed % 10, - #define TELEMETRY_OPENXSENSOR() (0) +#define TELEMETRY_CURRENT_FORMAT "%d.%d," +#define TELEMETRY_CURRENT_ARGS telemetryData.hub.current / 10, telemetryData.hub.current % 10, +#define TELEMETRY_VFAS_FORMAT "%d.%d," +#define TELEMETRY_VFAS_ARGS telemetryData.hub.vfas / 10, telemetryData.hub.vfas % 10, +#define TELEMETRY_VSPEED_FORMAT "%c%d.%02d," +#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 telemetryData.hub.airSpeed / 10, telemetryData.hub.airSpeed % 10, +#define TELEMETRY_OPENXSENSOR() (0) #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 +#define START_STOP 0x7E +#define BYTESTUFF 0x7D #define STUFF_MASK 0x20 -#define frskySendAlarms() - typedef enum { TS_IDLE = 0, // waiting for 0x5e frame marker TS_DATA_ID, // waiting for dataID @@ -292,11 +280,21 @@ void telemetryInit(uint8_t protocol); void telemetryInterrupt10ms(); -struct TelemetryData { - TelemetryValue swr; - FilteredTelemetryValue rssi; - uint16_t xjtVersion; - bool varioHighPrecision; +class TelemetryData { + public: + TelemetryExpiringDecorator swrInternal; + TelemetryExpiringDecorator swrExternal; + TelemetryFilterDecorator rssi; + uint16_t xjtVersion; + bool varioHighPrecision; + + void setSwr(uint8_t origin, uint8_t value) + { + if (origin & 0x80) + swrExternal.set(0x00); + else + swrInternal.set(0x00); + } }; extern TelemetryData telemetryData; @@ -304,4 +302,9 @@ extern TelemetryData telemetryData; bool pushFrskyTelemetryData(uint8_t data); // returns true when end of frame detected void processFrskyTelemetryData(uint8_t data); +inline bool IS_RAS_VALUE_VALID(uint16_t version) +{ + return version != 0x0000 && version != 0x00ff; +} + #endif // _FRSKY_H_ diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp index 146b74bef..0632634d9 100644 --- a/radio/src/telemetry/frsky_sport.cpp +++ b/radio/src/telemetry/frsky_sport.cpp @@ -184,15 +184,14 @@ void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packe } else if (dataId == XJT_VERSION_ID) { telemetryData.xjtVersion = HUB_DATA_U16(packet); - if (!IS_RAS_VALUE_VALID()) { - telemetryData.swr.set(0x00); + if (!IS_RAS_VALUE_VALID(telemetryData.xjtVersion)) { + telemetryData.setSwr(origin, 0); } } else if (dataId == RAS_ID) { - if (IS_RAS_VALUE_VALID()) - telemetryData.swr.set(SPORT_DATA_U8(packet)); - else - telemetryData.swr.set(0x00); + if (!IS_RAS_VALUE_VALID(telemetryData.xjtVersion)) { + telemetryData.setSwr(origin, SPORT_DATA_U8(packet)); + } } if (TELEMETRY_STREAMING()/* because when Rx is OFF it happens that some old A1/A2 values are sent from the XJT module*/) { diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index 165e20607..530e30cf5 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -66,6 +66,69 @@ void processTelemetryData(uint8_t data) processFrskyTelemetryData(data); } +#if defined(NO_RAS) +inline bool IS_INTERNAL_RAS_VALUE_VALID() +{ + return false; +} + +inline bool IS_EXTERNAL_RAS_VALUE_VALID() +{ + return false; +} +#elif defined(PXX2) +inline bool IS_INTERNAL_RAS_VALUE_VALID() +{ + return get_tmr10ms() < telemetryData.swrInternal.expirationTime; +} + +inline bool IS_EXTERNAL_RAS_VALUE_VALID() +{ + return get_tmr10ms() < telemetryData.swrExternal.expirationTime; +} +#elif defined(PCBX10) +inline bool IS_INTERNAL_RAS_VALUE_VALID() +{ + return false; +} + +inline bool IS_EXTERNAL_RAS_VALUE_VALID() +{ + return false; +} +#elif defined(PCBX9DP) || defined(PCBX9E) +inline bool IS_INTERNAL_RAS_VALUE_VALID() +{ + return IS_RAS_VALUE_VALID(telemetryData.xjtVersion); +} + +inline bool IS_EXTERNAL_RAS_VALUE_VALID() +{ + return IS_RAS_VALUE_VALID(telemetryData.xjtVersion); +} +#else +inline bool IS_INTERNAL_RAS_VALUE_VALID() +{ + return false; +} + +inline bool IS_EXTERNAL_RAS_VALUE_VALID() +{ + return false; +} +#endif + +inline bool isBadAntennaDetected() +{ + if (IS_INTERNAL_RAS_VALUE_VALID() && telemetryData.swrInternal.value > 0x33) + return true; + + if (IS_EXTERNAL_RAS_VALUE_VALID() && telemetryData.swrExternal.value > 0x33) + return true; + + return false; +} + void telemetryWakeup() { uint8_t requiredTelemetryProtocol = modelTelemetryProtocol(); @@ -133,8 +196,6 @@ void telemetryWakeup() } #endif -#define FRSKY_BAD_ANTENNA() (IS_RAS_VALUE_VALID() && telemetryData.swr.value > 0x33) - static tmr10ms_t alarmsCheckTime = 0; #define SCHEDULE_NEXT_ALARMS_CHECK(seconds) alarmsCheckTime = get_tmr10ms() + (100*(seconds)) if (int32_t(get_tmr10ms() - alarmsCheckTime) > 0) { @@ -159,7 +220,7 @@ void telemetryWakeup() } #if defined(PCBTARANIS) || defined(PCBHORUS) - if ((/*isModulePXX2(INTERNAL_MODULE) || */isModulePXX(INTERNAL_MODULE)/* || isModulePXX2(EXTERNAL_MODULE)*/ || isModulePXX(EXTERNAL_MODULE)) && FRSKY_BAD_ANTENNA()) { + if (isBadAntennaDetected()) { AUDIO_RAS_RED(); POPUP_WARNING(STR_WARNING); const char * w = STR_ANTENNAPROBLEM; diff --git a/radio/src/telemetry/telemetry_holders.cpp b/radio/src/telemetry/telemetry_holders.cpp deleted file mode 100644 index f01977df5..000000000 --- a/radio/src/telemetry/telemetry_holders.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/* - * 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" - -void FilteredTelemetryValue::reset() -{ - memclear(this, sizeof(*this)); -} - -void FilteredTelemetryValue::set(uint8_t value) -{ - 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); - } -} - -void TelemetryValueWithMin::reset() -{ - memclear(this, sizeof(*this)); -} - -void TelemetryValueWithMin::set(uint8_t value) -{ - FilteredTelemetryValue::set(value); - if (!min || value < min) { - min = value; - } -} diff --git a/radio/src/telemetry/telemetry_holders.h b/radio/src/telemetry/telemetry_holders.h index dc076c541..a08302e11 100644 --- a/radio/src/telemetry/telemetry_holders.h +++ b/radio/src/telemetry/telemetry_holders.h @@ -36,23 +36,70 @@ class TelemetryValue { void reset() { - this->value = 0; + set(0); }; }; -class FilteredTelemetryValue: public TelemetryValue { +template +class TelemetryFilterDecorator: public T { public: - uint8_t value; // filtered value (average of last TELEMETRY_AVERAGE_COUNT+1 values) uint8_t values[TELEMETRY_AVERAGE_COUNT]; - void set(uint8_t value); - void reset(); + void set(uint8_t value) + { + 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); + } + } + void reset() + { + memclear(this, sizeof(*this)); + }; }; -class TelemetryValueWithMin: public FilteredTelemetryValue { +template +class TelemetryExpiringDecorator: public T { + public: + tmr10ms_t expirationTime; + void set(uint8_t value) + { + T::set(value); + expirationTime = get_tmr10ms(); + } + void reset() + { + memclear(this, sizeof(*this)); + } +}; + +template +class TelemetryMinDecorator: public T { public: uint8_t min; - void set(uint8_t value); - void reset(); + void set(uint8_t value) + { + T::set(value); + if (!min || value < min) { + min = value; + } + } + void reset() + { + memclear(this, sizeof(*this)); + } }; #endif // _TELEMETRY_HOLDERS_H_