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