[ARM boards] S.PORT GPS was completely broken

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);

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<MAX_TELEMETRY_SENSORS; i++) {
       if (isTelemetryFieldAvailable(i)) {
-        uint8_t lastReceived = telemetryItems[i].lastReceived;
-        if (lastReceived < TELEMETRY_VALUE_TIMER_CYCLE && uint8_t(now - lastReceived) > 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;
           }
         }
       }

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,

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) {

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];