diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp index f55a2723e..7d5ef6970 100644 --- a/radio/src/telemetry/frsky_sport.cpp +++ b/radio/src/telemetry/frsky_sport.cpp @@ -64,8 +64,7 @@ const FrSkySportSensor sportSensors[] = { { CELLS_FIRST_ID, CELLS_LAST_ID, 0, ZSTR_CELLS, UNIT_CELLS, 2 }, { GPS_ALT_FIRST_ID, GPS_ALT_LAST_ID, 0, ZSTR_GPSALT, UNIT_METERS, 2 }, { GPS_TIME_DATE_FIRST_ID, GPS_TIME_DATE_LAST_ID, 0, ZSTR_GPSDATETIME, UNIT_DATETIME, 0 }, - { GPS_LONG_LATI_FIRST_ID, GPS_LONG_LATI_LAST_ID, 0, ZSTR_GPS, UNIT_GPS_LATITUDE, 0 }, - { GPS_LONG_LATI_FIRST_ID, GPS_LONG_LATI_LAST_ID, 1, ZSTR_GPS, UNIT_GPS_LONGITUDE, 0 }, + { GPS_LONG_LATI_FIRST_ID, GPS_LONG_LATI_LAST_ID, 0, ZSTR_GPS, UNIT_GPS, 0 }, { FUEL_QTY_FIRST_ID, FUEL_QTY_LAST_ID, 0, ZSTR_FUEL, UNIT_MILLILITERS, 2 }, { GPS_COURS_FIRST_ID, GPS_COURS_LAST_ID, 0, ZSTR_HDG, UNIT_DEGREE, 2 }, { RBOX_BATT1_FIRST_ID, RBOX_BATT1_LAST_ID, 0, ZSTR_BATT1_VOLTAGE, UNIT_VOLTS, 3 }, @@ -165,13 +164,13 @@ void processSportUpdatePacket(uint8_t * packet) } } -void processSportPacket(uint16_t id, uint8_t subId, uint8_t instance, uint32_t data) +void processSportPacket(uint16_t id, uint8_t subId, uint8_t instance, uint32_t data, TelemetryUnit unit=UNIT_RAW) { const FrSkySportSensor * sensor = getFrSkySportSensor(id, subId); - TelemetryUnit unit = UNIT_RAW; uint8_t precision = 0; if (sensor) { - unit = sensor->unit; + if (unit == UNIT_RAW) + unit = sensor->unit; precision = sensor->prec; } if (unit == UNIT_CELLS) { @@ -280,9 +279,9 @@ void processSportPacket(uint8_t * packet) value = -value; value = (value * 5) / 3; // min/10000 => deg/1000000 if (data & (1 << 31)) - processSportPacket(id, 0, instance, value); // latitude + processSportPacket(id, 0, instance, value, UNIT_GPS_LATITUDE); else - processSportPacket(id, 1, instance, value); // longitude + processSportPacket(id, 0, instance, value, UNIT_GPS_LONGITUDE); } else if (id >= RBOX_BATT1_FIRST_ID && id <= RBOX_BATT2_LAST_ID) { processSportPacket(id, 0, instance, data & 0xffff); diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index addf22f0b..9977528d4 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -155,18 +155,15 @@ void telemetryWakeup() 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; + TelemetryItem & item = telemetryItems[i]; + if (item.hasReceiveTime() && item.getDelaySinceLastValue() > TELEMETRY_VALUE_OLD_THRESHOLD) { TelemetrySensor * sensor = & g_model.telemetrySensors[i]; - if (sensor->unit == UNIT_DATETIME) { - telemetryItems[i].datetime.datestate = 0; - telemetryItems[i].datetime.timestate = 0; + if (sensor->unit != UNIT_DATETIME) { + item.setOld(); + sensor_lost = true; } } } diff --git a/radio/src/telemetry/telemetry.h b/radio/src/telemetry/telemetry.h index 1236e4dd3..cc582619b 100644 --- a/radio/src/telemetry/telemetry.h +++ b/radio/src/telemetry/telemetry.h @@ -79,11 +79,7 @@ extern uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE]; extern uint8_t telemetryRxBufferCount; #if defined(CPUARM) -#define TELEMETRY_VALUE_TIMER_CYCLE 200 /*20 seconds*/ -#define TELEMETRY_VALUE_OLD_THRESHOLD 150 /*15 seconds*/ -#define TELEMETRY_VALUE_UNAVAILABLE 255 -#define TELEMETRY_VALUE_OLD 254 -#define TELEMETRY_AVERAGE_COUNT 3 +#define TELEMETRY_AVERAGE_COUNT 3 enum { TELEM_CELL_INDEX_LOWEST, diff --git a/radio/src/telemetry/telemetry_sensors.cpp b/radio/src/telemetry/telemetry_sensors.cpp index 4ccb0169e..67b7a6ad3 100644 --- a/radio/src/telemetry/telemetry_sensors.cpp +++ b/radio/src/telemetry/telemetry_sensors.cpp @@ -227,21 +227,6 @@ void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32 lastReceived = now(); } -bool TelemetryItem::isAvailable() -{ - return (lastReceived != TELEMETRY_VALUE_UNAVAILABLE); -} - -bool TelemetryItem::isFresh() -{ - return (lastReceived < TELEMETRY_VALUE_TIMER_CYCLE) && (uint8_t(now() - lastReceived) < 2); -} - -bool TelemetryItem::isOld() -{ - return (lastReceived == TELEMETRY_VALUE_OLD); -} - void TelemetryItem::per10ms(const TelemetrySensor & sensor) { switch (sensor.formula) { diff --git a/radio/src/telemetry/telemetry_sensors.h b/radio/src/telemetry/telemetry_sensors.h index 0ec79755b..2cfceb214 100644 --- a/radio/src/telemetry/telemetry_sensors.h +++ b/radio/src/telemetry/telemetry_sensors.h @@ -23,6 +23,11 @@ #include "telemetry.h" +#define TELEMETRY_VALUE_TIMER_CYCLE 128 /* x160ms ~= 20.5s ; must be multiple of 2 to avoid the modulo */ +#define TELEMETRY_VALUE_OLD_THRESHOLD 62 /* x160ms ~= 10s */ +#define TELEMETRY_VALUE_UNAVAILABLE 255 +#define TELEMETRY_VALUE_OLD 254 + class TelemetryItem { public: @@ -77,7 +82,8 @@ class TelemetryItem static uint8_t now() { - return (get_tmr10ms() / 10) % TELEMETRY_VALUE_TIMER_CYCLE; + // 160ms granularity + return (get_tmr10ms() >> 4) & (TELEMETRY_VALUE_TIMER_CYCLE - 1); } TelemetryItem() @@ -95,10 +101,39 @@ class TelemetryItem 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(); + + inline bool isAvailable() + { + return (lastReceived != TELEMETRY_VALUE_UNAVAILABLE); + } + + inline bool isOld() + { + return (lastReceived == TELEMETRY_VALUE_OLD); + } + + inline bool hasReceiveTime() + { + return (lastReceived < TELEMETRY_VALUE_TIMER_CYCLE); + } + + inline uint8_t getDelaySinceLastValue() + { + // assumes lastReceived is not a special value (OLD / UNAVAILABLE) + return (now() - lastReceived) & (TELEMETRY_VALUE_TIMER_CYCLE - 1); + } + + inline bool isFresh() + { + return (hasReceiveTime() && getDelaySinceLastValue() <= 1); + } + + inline void setOld() + { + lastReceived = TELEMETRY_VALUE_OLD; + } + + void gpsReceived(); // TODO seems not used }; extern TelemetryItem telemetryItems[MAX_TELEMETRY_SENSORS];