Telemetry refactoring - Fixes #1634

2014-11-08 12:06:42 +01:00
parent 7d8ca6931b
commit 88d9691d16
105 changed files with 5789 additions and 3449 deletions
@@ -48,7 +48,7 @@
 #define RSSI2PKT        0xf6
 #define RSSI_REQUEST    0xf1

-#if defined(FRSKY_HUB) || defined(WS_HOW_HIGH)
+#if !defined(CPUARM) && (defined(FRSKY_HUB) || defined(WS_HOW_HIGH))
 void checkMinMaxAltitude()
 {
  if (TELEMETRY_RELATIVE_BARO_ALT_BP > frskyData.hub.maxAltitude)
@@ -93,7 +93,11 @@ void parseTelemHubByte(uint8_t byte)
      state = TS_IDLE;
    }
    else {
-      structPos = byte*2;
+#if defined(CPUARM)
+      structPos = byte;
+#else
+      structPos = byte * 2;
+#endif
      state = TS_DATA_LOW;
    }
    return;
@@ -106,6 +110,10 @@ void parseTelemHubByte(uint8_t byte)

  state = TS_IDLE;

+#if defined(CPUARM)
+  processHubPacket(structPos, (byte << 8) + lowByte);
+#else
+
 #if defined(GPS)
  if ((uint8_t)structPos == offsetof(FrskySerialData, gpsLatitude_bp)) {
    if (lowByte || byte)
@@ -256,10 +264,11 @@ void parseTelemHubByte(uint8_t byte)
      break;
 #endif
  }
+#endif
 }
 #endif

-#if defined(WS_HOW_HIGH)
+#if defined(WS_HOW_HIGH) && !defined(CPUARM)
 void parseTelemWSHowHighByte(uint8_t byte)
 {
  if (frskyUsrStreaming < (WSHH_TIMEOUT10ms - 10)) {
@@ -275,6 +284,31 @@ void parseTelemWSHowHighByte(uint8_t byte)
 }
 #endif  

+#if defined(CPUARM)
+void frskyDProcessPacket(uint8_t *packet)
+{
+  // What type of packet?
+  switch (packet[0])
+  {
+    case LINKPKT: // A1/A2/RSSI values
+    {
+      setTelemetryValue(TELEM_PROTO_FRSKY_D, D_A1_ID, 0, packet[1], UNIT_VOLTS, 0);
+      setTelemetryValue(TELEM_PROTO_FRSKY_D, D_A2_ID, 0, packet[2], UNIT_VOLTS, 0);
+      setTelemetryValue(TELEM_PROTO_FRSKY_D, D_RSSI_ID, 0, packet[3], UNIT_RAW, 0);
+      frskyData.rssi.set(packet[3]);
+      frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected
+      break;
+    }
+
+    case USRPKT: // User Data packet
+      uint8_t numBytes = 3 + (packet[1] & 0x07); // sanitize in case of data corruption leading to buffer overflow
+      for (uint8_t i=3; i<numBytes; i++) {
+        parseTelemHubByte(packet[i]);
+      }
+      break;
+  }
+}
+#else
 void frskyDProcessPacket(uint8_t *packet)
 {
  // What type of packet?
@@ -315,6 +349,7 @@ void frskyDProcessPacket(uint8_t *packet)
 #endif
  }
 }
+#endif

 #if !defined(PCBTARANIS)

@@ -347,6 +382,7 @@ void frskyDSendNextAlarm(void)
  }
 #endif

+#if 0
  uint8_t *ptr = &frskyTxBuffer[0];

  *ptr++ = START_STOP;        // Start of packet
@@ -376,6 +412,9 @@ void frskyDSendNextAlarm(void)
  *ptr++ = START_STOP; // End of packet

  telemetryTransmitBuffer(frskyTxBuffer, ptr - &frskyTxBuffer[0]);
+#elif !defined(WIN32)
+#warning "FrSky module alarms removed!"
+#endif
 }

 #else
@@ -460,4 +499,160 @@ bool isFrskyAlarmRaised(uint8_t idx)
 }
 #endif

+#if defined(CPUARM)
+struct FrSkyDSensor {
+  const uint8_t id;
+  const char * name;
+  const TelemetryUnit unit;
+  const uint8_t prec;
+};

+const FrSkyDSensor frskyDSensors[] = {
+  { D_RSSI_ID, ZSTR_RSSI, UNIT_RAW, 0 },
+  { D_A1_ID, ZSTR_A1, UNIT_VOLTS, 0 },
+  { D_A2_ID, ZSTR_A2, UNIT_VOLTS, 0 },
+  { RPM_ID, ZSTR_RPM, UNIT_RAW, 0 },
+  { FUEL_ID, ZSTR_FUEL, UNIT_PERCENT, 0 },
+  { TEMP1_ID, ZSTR_TEMP, UNIT_CELSIUS, 0 },
+  { TEMP2_ID, ZSTR_TEMP, UNIT_CELSIUS, 0 },
+  { CURRENT_ID, ZSTR_CURR, UNIT_AMPS, 1 },
+  { ACCEL_X_ID, ZSTR_ACCX, UNIT_G, 3 },
+  { ACCEL_Y_ID, ZSTR_ACCY, UNIT_G, 3 },
+  { ACCEL_Z_ID, ZSTR_ACCZ, UNIT_G, 3 },
+  { VARIO_ID, ZSTR_VSPD, UNIT_METERS_PER_SECOND, 2 },
+  { VFAS_ID, ZSTR_VFAS, UNIT_VOLTS, 2 },
+  { BARO_ALT_AP_ID, ZSTR_ALT, UNIT_METERS, 2 },
+  { GPS_SPEED_BP_ID, ZSTR_GSPD, UNIT_KTS, 0 },
+  { GPS_COURS_BP_ID, ZSTR_HDG, UNIT_DEGREE, 0 },
+  { VOLTS_ID, ZSTR_CELLS, UNIT_CELLS, 2 },
+  { GPS_ALT_BP_ID, ZSTR_GPSALT, UNIT_METERS, 0 },
+  { GPS_HOUR_MIN_ID, ZSTR_GPSDATETIME, UNIT_DATETIME, 0 },
+  { GPS_LAT_AP_ID, ZSTR_GPS, UNIT_GPS, 0 },
+  { 0, NULL, UNIT_RAW, 0 } // sentinel
+};
+
+const FrSkyDSensor * getFrSkyDSensor(uint8_t id)
+{
+  const FrSkyDSensor * result = NULL;
+  for (const FrSkyDSensor * sensor = frskyDSensors; sensor->id; sensor++) {
+    if (id == sensor->id) {
+      result = sensor;
+      break;
+    }
+  }
+  return result;
+}
+
+void processHubPacket(uint8_t id, int16_t value)
+{
+  static uint8_t lastId = 0;
+  static uint16_t lastValue = 0;
+  TelemetryUnit unit = UNIT_RAW;
+  uint8_t precision = 0;
+  int32_t data = value;
+
+  if (id > FRSKY_LAST_ID || id == GPS_SPEED_AP_ID || id == GPS_ALT_AP_ID || id == GPS_COURS_AP_ID) {
+    return;
+  }
+
+  if (id == GPS_LAT_BP_ID || id == GPS_LONG_BP_ID || id == GPS_ALT_BP_ID || id == BARO_ALT_BP_ID || id == VOLTS_BP_ID) {
+    lastId = id;
+    lastValue = value;
+    return;
+  }
+
+  if (id == GPS_LAT_AP_ID) {
+    if (lastId == GPS_LAT_BP_ID) {
+      data += lastValue << 16;
+      unit = UNIT_GPS_LATITUDE;
+    }
+    else {
+      return;
+    }
+  }
+  else if (id == GPS_LONG_AP_ID) {
+    if (lastId == GPS_LONG_BP_ID) {
+      data += lastValue << 16;
+      id = GPS_LAT_AP_ID;
+      unit = UNIT_GPS_LONGITUDE;
+    }
+    else {
+      return;
+    }
+  }
+  else if (id == GPS_LAT_NS_ID) {
+    id = GPS_LAT_AP_ID;
+    unit = UNIT_GPS_LATITUDE_NS;
+  }
+  else if (id == GPS_LONG_EW_ID) {
+    id = GPS_LAT_AP_ID;
+    unit = UNIT_GPS_LONGITUDE_EW;
+  }
+  else if (id == BARO_ALT_AP_ID) {
+    if (lastId == BARO_ALT_BP_ID) {
+      data += lastValue * 100;
+      unit = UNIT_METERS;
+      precision = 2;
+    }
+    else {
+      return;
+    }
+  }
+  else if (id == VOLTS_ID) {
+    unit = UNIT_CELLS;
+  }
+  else if (id == GPS_DAY_MONTH_ID) {
+    id = GPS_HOUR_MIN_ID;
+    unit = UNIT_DATETIME_DAY_MONTH;
+  }
+  else if (id == GPS_HOUR_MIN_ID) {
+    unit = UNIT_DATETIME_HOUR_MIN;
+  }
+  else if (id == GPS_SEC_ID) {
+    id = GPS_HOUR_MIN_ID;
+    unit = UNIT_DATETIME_SEC;
+  }
+  else if (id == GPS_YEAR_ID) {
+    id = GPS_HOUR_MIN_ID;
+    unit = UNIT_DATETIME_YEAR;
+  }
+  else {
+    const FrSkyDSensor * sensor = getFrSkyDSensor(id);
+    if (sensor) {
+      unit = sensor->unit;
+      precision = sensor->prec;
+    }
+  }
+  setTelemetryValue(TELEM_PROTO_FRSKY_D, id, 0, data, unit, precision);
+}
+
+void frskyDSetDefault(int index, uint16_t id)
+{
+  TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
+
+  telemetrySensor.id = id;
+  telemetrySensor.instance = 0;
+
+  const FrSkyDSensor * sensor = getFrSkyDSensor(id);
+  if (sensor) {
+    TelemetryUnit unit = sensor->unit;
+    if (unit == UNIT_CELLS)
+      unit = UNIT_VOLTS;
+    uint8_t prec = min<uint8_t>(2, sensor->prec);
+    telemetrySensor.init(sensor->name, unit, prec);
+    if (id >= D_A1_ID && id <= D_A2_ID) {
+      telemetrySensor.prec = 1;
+      telemetrySensor.custom.ratio = 132;
+      telemetrySensor.inputFlags = TELEM_INPUT_FLAGS_FILTERING;
+    }
+    else if (id == D_RSSI_ID) {
+      telemetrySensor.inputFlags = TELEM_INPUT_FLAGS_FILTERING;
+    }
+  }
+  else {
+    telemetrySensor.init(id);
+  }
+
+  eeDirty(EE_MODEL);
+}
+#endif