radio/src/telemetry/frsky_d.cpp

   1 /*
   2  * Copyright (C) OpenTX
   3  *
   4  * Based on code named
   5  *   th9x - http://code.google.com/p/th9x
   6  *   er9x - http://code.google.com/p/er9x
   7  *   gruvin9x - http://code.google.com/p/gruvin9x
   8  *
   9  * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License version 2 as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  */
  20
  21 #include "opentx.h"
  22
  23 void parseTelemHubByte(uint8_t byte)
  24 {
  25   static int8_t structPos;
  26   static uint8_t lowByte;
  27   static TS_STATE state = TS_IDLE;
  28
  29   if (byte == 0x5e) {
  30     state = TS_DATA_ID;
  31     return;
  32   }
  33   if (state == TS_IDLE) {
  34     return;
  35   }
  36   if (state & TS_XOR) {
  37     byte = byte ^ 0x60;
  38     state = (TS_STATE)(state - TS_XOR);
  39   }
  40   else if (byte == 0x5d) {
  41     state = (TS_STATE)(state | TS_XOR);
  42     return;
  43   }
  44   if (state == TS_DATA_ID) {
  45     if (byte > 0x3f) {
  46       state = TS_IDLE;
  47     }
  48     else {
  49       structPos = byte;
  50       state = TS_DATA_LOW;
  51     }
  52     return;
  53   }
  54   if (state == TS_DATA_LOW) {
  55     lowByte = byte;
  56     state = TS_DATA_HIGH;
  57     return;
  58   }
  59
  60   state = TS_IDLE;
  61   processHubPacket(structPos, (byte << 8) + lowByte);
  62 }
  63
  64 void frskyDProcessPacket(const uint8_t *packet)
  65 {
  66   // What type of packet?
  67   switch (packet[0])
  68   {
  69     case LINKPKT: // A1/A2/RSSI values
  70     {
  71       setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_D, D_A1_ID, 0, 0, packet[1], UNIT_VOLTS, 0);
  72       setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_D, D_A2_ID, 0, 0, packet[2], UNIT_VOLTS, 0);
  73       setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_D, D_RSSI_ID, 0, 0, packet[3], UNIT_RAW, 0);
  74 #if defined(MULTIMODULE)
  75       if (telemetryProtocol == PROTOCOL_TELEMETRY_MULTIMODULE) {
  76         setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_D, TX_RSSI_ID, 0, 0, packet[4]>>1, UNIT_DB,  0);
  77         setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_D, RX_LQI_ID,  0, 0, packet[5]   , UNIT_RAW, 0);
  78         setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_D, TX_LQI_ID , 0, 0, packet[6]   , UNIT_RAW, 0);
  79       }
  80 #endif
  81       telemetryData.rssi.set(packet[3]);
  82       telemetryStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected
  83       break;
  84     }
  85
  86     case USRPKT: // User Data packet
  87       uint8_t numBytes = 3 + (packet[1] & 0x07); // sanitize in case of data corruption leading to buffer overflow
  88       for (uint8_t i=3; i<numBytes; i++) {
  89         parseTelemHubByte(packet[i]);
  90       }
  91       break;
  92   }
  93 }
  94
  95 struct FrSkyDSensor {
  96   const uint8_t id;
  97   const char * name;
  98   const TelemetryUnit unit;
  99   const uint8_t prec;
 100 };
 101
 102 const FrSkyDSensor frskyDSensors[] = {
 103   { D_RSSI_ID, ZSTR_RSSI, UNIT_RAW, 0 },
 104   { D_A1_ID, ZSTR_A1, UNIT_VOLTS, 1 },
 105   { D_A2_ID, ZSTR_A2, UNIT_VOLTS, 1 },
 106   { RPM_ID, ZSTR_RPM, UNIT_RPMS, 0 },
 107   { FUEL_ID, ZSTR_FUEL, UNIT_PERCENT, 0 },
 108   { TEMP1_ID, ZSTR_TEMP1, UNIT_CELSIUS, 0 },
 109   { TEMP2_ID, ZSTR_TEMP2, UNIT_CELSIUS, 0 },
 110   { CURRENT_ID, ZSTR_CURR, UNIT_AMPS, 1 },
 111   { ACCEL_X_ID, ZSTR_ACCX, UNIT_G, 3 },
 112   { ACCEL_Y_ID, ZSTR_ACCY, UNIT_G, 3 },
 113   { ACCEL_Z_ID, ZSTR_ACCZ, UNIT_G, 3 },
 114   { VARIO_ID, ZSTR_VSPD, UNIT_METERS_PER_SECOND, 2 },
 115   { VFAS_ID, ZSTR_VFAS, UNIT_VOLTS, 2 },
 116   { BARO_ALT_AP_ID, ZSTR_ALT, UNIT_METERS, 1 },   // we map hi precision vario into PREC1!
 117   { VOLTS_AP_ID, ZSTR_VFAS, UNIT_VOLTS, 2 },
 118   { GPS_SPEED_BP_ID, ZSTR_GSPD, UNIT_KTS, 0 },
 119   { GPS_COURS_BP_ID, ZSTR_HDG, UNIT_DEGREE, 0 },
 120   { VOLTS_ID, ZSTR_CELLS, UNIT_CELLS, 2 },
 121   { GPS_ALT_BP_ID, ZSTR_GPSALT, UNIT_METERS, 0 },
 122   { GPS_HOUR_MIN_ID, ZSTR_GPSDATETIME, UNIT_DATETIME, 0 },
 123   { GPS_LAT_AP_ID, ZSTR_GPS, UNIT_GPS, 0 },
 124   { 0, NULL, UNIT_RAW, 0 } // sentinel
 125 };
 126
 127 const FrSkyDSensor * getFrSkyDSensor(uint8_t id)
 128 {
 129   const FrSkyDSensor * result = NULL;
 130   for (const FrSkyDSensor * sensor = frskyDSensors; sensor->id; sensor++) {
 131     if (id == sensor->id) {
 132       result = sensor;
 133       break;
 134     }
 135   }
 136   return result;
 137 }
 138
 139 uint8_t lastId = 0;
 140 uint16_t lastBPValue = 0;
 141 uint16_t lastAPValue = 0;
 142
 143 int32_t getFrSkyDProtocolGPSValue(int32_t sign)
 144 {
 145   div_t qr = div(lastBPValue, 100);
 146   return sign * (((uint32_t) (qr.quot) * 1000000) + (((uint32_t) (qr.rem) * 10000 + lastAPValue) * 5) / 3);
 147 }
 148
 149 void processHubPacket(uint8_t id, int16_t value)
 150 {
 151   TelemetryUnit unit = UNIT_RAW;
 152   uint8_t precision = 0;
 153   int32_t data = value;
 154
 155   if (id > FRSKY_LAST_ID || id == GPS_SPEED_AP_ID || id == GPS_ALT_AP_ID || id == GPS_COURS_AP_ID) {
 156     return;
 157   }
 158
 159   if (id == GPS_LAT_BP_ID || id == GPS_LONG_BP_ID || id == BARO_ALT_BP_ID || id == VOLTS_BP_ID) {
 160     lastId = id;
 161     lastBPValue = value;
 162     return;
 163   }
 164
 165   if (id == GPS_LAT_AP_ID) {
 166     if (lastId == GPS_LAT_BP_ID) {
 167       lastId = id;
 168       lastAPValue = data;
 169     }
 170     return;
 171   }
 172   else if (id == GPS_LONG_AP_ID) {
 173     if (lastId == GPS_LONG_BP_ID) {
 174       lastId = id;
 175       lastAPValue = data;
 176     }
 177     return;
 178   }
 179   else if (id == GPS_LAT_NS_ID) {
 180     if (lastId == GPS_LAT_AP_ID) {
 181       id = GPS_LAT_AP_ID;
 182       unit = UNIT_GPS_LATITUDE;
 183       data = getFrSkyDProtocolGPSValue(value == 'N' ? 1 : -1);
 184     }
 185     else {
 186       return;
 187     }
 188   }
 189   else if (id == GPS_LONG_EW_ID) {
 190     if (lastId == GPS_LONG_AP_ID) {
 191       id = GPS_LAT_AP_ID;
 192       unit = UNIT_GPS_LONGITUDE;
 193       data = getFrSkyDProtocolGPSValue(value == 'E' ? 1 : -1);
 194     }
 195     else {
 196       return;
 197     }
 198   }
 199   else if (id == BARO_ALT_AP_ID) {
 200     if (lastId == BARO_ALT_BP_ID) {
 201       if (data > 9 || telemetryData.varioHighPrecision) {
 202         telemetryData.varioHighPrecision = true;
 203         data /= 10;    // map hi precision vario into low precision. Altitude is stored in 0.1m anyways
 204       }
 205       data = (int16_t)lastBPValue * 10 + (((int16_t)lastBPValue < 0) ? -data : data);
 206       unit = UNIT_METERS;
 207       precision = 1;
 208     }
 209     else {
 210       return;
 211     }
 212   }
 213   else if (id == VOLTS_AP_ID) {
 214     if (lastId == VOLTS_BP_ID) {
 215 #if defined(FAS_PROTOTYPE)
 216       data = lastBPValue * 100 + value * 10;
 217 #else
 218       data = ((lastBPValue * 100 + value * 10) * 210) / 110;
 219 #endif
 220       unit = UNIT_VOLTS;
 221       precision = 2;
 222     }
 223     else {
 224       return;
 225     }
 226   }
 227   else if (id == VOLTS_ID) {
 228     unit = UNIT_CELLS;
 229     uint32_t cellData = (uint32_t)data;
 230     data = ((cellData & 0x00F0) << 12) + (((((cellData & 0xFF00) >> 8) + ((cellData & 0x000F) << 8))) / 5);
 231   }
 232   else if (id == GPS_DAY_MONTH_ID) {
 233     id = GPS_HOUR_MIN_ID;
 234     unit = UNIT_DATETIME_DAY_MONTH;
 235   }
 236   else if (id == GPS_HOUR_MIN_ID) {
 237     unit = UNIT_DATETIME_HOUR_MIN;
 238   }
 239   else if (id == GPS_SEC_ID) {
 240     id = GPS_HOUR_MIN_ID;
 241     unit = UNIT_DATETIME_SEC;
 242   }
 243   else if (id == GPS_YEAR_ID) {
 244     id = GPS_HOUR_MIN_ID;
 245     unit = UNIT_DATETIME_YEAR;
 246   }
 247   else {
 248     const FrSkyDSensor * sensor = getFrSkyDSensor(id);
 249     if (sensor) {
 250       unit = sensor->unit;
 251       precision = sensor->prec;
 252     }
 253   }
 254   if (id == RPM_ID) {
 255     data = data * 60;
 256   }
 257   else if (id == VFAS_ID) {
 258     if (data >= VFAS_D_HIPREC_OFFSET) {
 259       // incoming value has a resolution of 0.01V and added offset of VFAS_D_HIPREC_OFFSET
 260       data -= VFAS_D_HIPREC_OFFSET;
 261     }
 262     else {
 263       // incoming value has a resolution of 0.1V
 264       data *= 10;
 265     }
 266   }
 267
 268   setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_D, id, 0, 0, data, unit, precision);
 269 }
 270
 271 void frskyDSetDefault(int index, uint16_t id)
 272 {
 273   TelemetrySensor &telemetrySensor = g_model.telemetrySensors[index];
 274
 275   telemetrySensor.id = id;
 276   telemetrySensor.instance = 0;
 277
 278 #if defined(MULTIMODULE)
 279   if (id == TX_RSSI_ID) {
 280     telemetrySensor.init(ZSTR_TX_RSSI, UNIT_DB, 0);
 281     telemetrySensor.filter = 1;
 282   }
 283   else if (id == TX_LQI_ID) {
 284     telemetrySensor.init(ZSTR_TX_QUALITY, UNIT_RAW, 0);
 285     telemetrySensor.filter = 1;
 286   }
 287   else if (id == RX_LQI_ID) {
 288     telemetrySensor.init(ZSTR_RX_QUALITY, UNIT_RAW, 0);
 289     telemetrySensor.filter = 1;
 290   }
 291   else
 292 #endif
 293   {
 294     const FrSkyDSensor * sensor = getFrSkyDSensor(id);
 295     if (sensor) {
 296       TelemetryUnit unit = sensor->unit;
 297       uint8_t prec = min<uint8_t>(2, sensor->prec);
 298       telemetrySensor.init(sensor->name, unit, prec);
 299       if (id == D_RSSI_ID) {
 300         telemetrySensor.filter = 1;
 301         telemetrySensor.logs = true;
 302       }
 303       else if (id >= D_A1_ID && id <= D_A2_ID) {
 304         telemetrySensor.custom.ratio = 132;
 305         telemetrySensor.filter = 1;
 306       }
 307       else if (id == CURRENT_ID) {
 308         telemetrySensor.onlyPositive = 1;
 309       }
 310       else if (id == BARO_ALT_AP_ID) {
 311         telemetrySensor.autoOffset = 1;
 312       }
 313       if (unit == UNIT_RPMS) {
 314         telemetrySensor.custom.ratio = 1;
 315         telemetrySensor.custom.offset = 1;
 316       }
 317       else if (unit == UNIT_METERS) {
 318         if (IS_IMPERIAL_ENABLE()) {
 319           telemetrySensor.unit = UNIT_FEET;
 320         }
 321       }
 322     }
 323     else {
 324       telemetrySensor.init(id);
 325     }
 326   }
 327
 328   storageDirty(EE_MODEL);
 329 }