radio/src/telemetry/crossfire.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 const CrossfireSensor crossfireSensors[] = {
  24   {LINK_ID,        0, ZSTR_RX_RSSI1,    UNIT_DB,            0},
  25   {LINK_ID,        1, ZSTR_RX_RSSI2,    UNIT_DB,            0},
  26   {LINK_ID,        2, ZSTR_RX_QUALITY,  UNIT_PERCENT,       0},
  27   {LINK_ID,        3, ZSTR_RX_SNR,      UNIT_DB,            0},
  28   {LINK_ID,        4, ZSTR_ANTENNA,     UNIT_RAW,           0},
  29   {LINK_ID,        5, ZSTR_RF_MODE,     UNIT_RAW,           0},
  30   {LINK_ID,        6, ZSTR_TX_POWER,    UNIT_MILLIWATTS,    0},
  31   {LINK_ID,        7, ZSTR_TX_RSSI,     UNIT_DB,            0},
  32   {LINK_ID,        8, ZSTR_TX_QUALITY,  UNIT_PERCENT,       0},
  33   {LINK_ID,        9, ZSTR_TX_SNR,      UNIT_DB,            0},
  34   {BATTERY_ID,     0, ZSTR_BATT,        UNIT_VOLTS,         1},
  35   {BATTERY_ID,     1, ZSTR_CURR,        UNIT_AMPS,          1},
  36   {BATTERY_ID,     2, ZSTR_CAPACITY,    UNIT_MAH,           0},
  37   {GPS_ID,         0, ZSTR_GPS,         UNIT_GPS_LATITUDE,  0},
  38   {GPS_ID,         0, ZSTR_GPS,         UNIT_GPS_LONGITUDE, 0},
  39   {GPS_ID,         2, ZSTR_GSPD,        UNIT_KMH,           1},
  40   {GPS_ID,         3, ZSTR_HDG,         UNIT_DEGREE,        3},
  41   {GPS_ID,         4, ZSTR_ALT,         UNIT_METERS,        0},
  42   {GPS_ID,         5, ZSTR_SATELLITES,  UNIT_RAW,           0},
  43   {ATTITUDE_ID,    0, ZSTR_PITCH,       UNIT_RADIANS,       3},
  44   {ATTITUDE_ID,    1, ZSTR_ROLL,        UNIT_RADIANS,       3},
  45   {ATTITUDE_ID,    2, ZSTR_YAW,         UNIT_RADIANS,       3},
  46   {FLIGHT_MODE_ID, 0, ZSTR_FLIGHT_MODE, UNIT_TEXT,          0},
  47   {0,              0, "UNKNOWN",        UNIT_RAW,           0},
  48 };
  49
  50 const CrossfireSensor & getCrossfireSensor(uint8_t id, uint8_t subId)
  51 {
  52   if (id == LINK_ID)
  53     return crossfireSensors[RX_RSSI1_INDEX+subId];
  54   else if (id == BATTERY_ID)
  55     return crossfireSensors[BATT_VOLTAGE_INDEX+subId];
  56   else if (id == GPS_ID)
  57     return crossfireSensors[GPS_LATITUDE_INDEX+subId];
  58   else if (id == ATTITUDE_ID)
  59     return crossfireSensors[ATTITUDE_PITCH_INDEX+subId];
  60   else if (id == FLIGHT_MODE_ID)
  61     return crossfireSensors[FLIGHT_MODE_INDEX];
  62   else
  63     return crossfireSensors[UNKNOWN_INDEX];
  64 }
  65
  66 void processCrossfireTelemetryValue(uint8_t index, int32_t value)
  67 {
  68   const CrossfireSensor & sensor = crossfireSensors[index];
  69   setTelemetryValue(TELEM_PROTO_CROSSFIRE, sensor.id, 0, sensor.subId, value, sensor.unit, sensor.precision);
  70 }
  71
  72 bool checkCrossfireTelemetryFrameCRC()
  73 {
  74   uint8_t len = telemetryRxBuffer[1];
  75   uint8_t crc = crc8(&telemetryRxBuffer[2], len-1);
  76   return (crc == telemetryRxBuffer[len+1]);
  77 }
  78
  79 template<int N>
  80 bool getCrossfireTelemetryValue(uint8_t index, int32_t & value)
  81 {
  82   bool result = false;
  83   uint8_t * byte = &telemetryRxBuffer[index];
  84   value = (*byte & 0x80) ? -1 : 0;
  85   for (uint8_t i=0; i<N; i++) {
  86     value <<= 8;
  87     if (*byte != 0xff) {
  88       result = true;
  89     }
  90     value += *byte++;
  91   }
  92   return result;
  93 }
  94
  95 void processCrossfireTelemetryFrame()
  96 {
  97   if (!checkCrossfireTelemetryFrameCRC()) {
  98     TRACE("[XF] CRC error");
  99     return;
 100   }
 101
 102   uint8_t id = telemetryRxBuffer[2];
 103   int32_t value;
 104   switch(id) {
 105     case GPS_ID:
 106       if (getCrossfireTelemetryValue<4>(3, value))
 107         processCrossfireTelemetryValue(GPS_LATITUDE_INDEX, value/10);
 108       if (getCrossfireTelemetryValue<4>(7, value))
 109         processCrossfireTelemetryValue(GPS_LONGITUDE_INDEX, value/10);
 110       if (getCrossfireTelemetryValue<2>(11, value))
 111         processCrossfireTelemetryValue(GPS_GROUND_SPEED_INDEX, value);
 112       if (getCrossfireTelemetryValue<2>(13, value))
 113         processCrossfireTelemetryValue(GPS_HEADING_INDEX, value);
 114       if (getCrossfireTelemetryValue<2>(15, value))
 115         processCrossfireTelemetryValue(GPS_ALTITUDE_INDEX,  value - 1000);
 116       if (getCrossfireTelemetryValue<1>(17, value))
 117         processCrossfireTelemetryValue(GPS_SATELLITES_INDEX, value);
 118       break;
 119
 120     case LINK_ID:
 121       for (unsigned int i=0; i<=TX_SNR_INDEX; i++) {
 122         if (getCrossfireTelemetryValue<1>(3+i, value)) {
 123           if (i == TX_POWER_INDEX) {
 124             static const int32_t power_values[] = { 0, 10, 25, 100, 500, 1000, 2000 };
 125             value = ((unsigned)value < DIM(power_values) ? power_values[value] : 0);
 126           }
 127           processCrossfireTelemetryValue(i, value);
 128           if (i == RX_QUALITY_INDEX) {
 129             telemetryData.rssi.set(value);
 130             telemetryStreaming = TELEMETRY_TIMEOUT10ms;
 131           }
 132         }
 133       }
 134       break;
 135
 136     case BATTERY_ID:
 137       if (getCrossfireTelemetryValue<2>(3, value))
 138         processCrossfireTelemetryValue(BATT_VOLTAGE_INDEX, value);
 139       if (getCrossfireTelemetryValue<2>(5, value))
 140         processCrossfireTelemetryValue(BATT_CURRENT_INDEX, value);
 141       if (getCrossfireTelemetryValue<3>(7, value))
 142         processCrossfireTelemetryValue(BATT_CAPACITY_INDEX, value);
 143       break;
 144
 145     case ATTITUDE_ID:
 146       if (getCrossfireTelemetryValue<2>(3, value))
 147         processCrossfireTelemetryValue(ATTITUDE_PITCH_INDEX, value/10);
 148       if (getCrossfireTelemetryValue<2>(5, value))
 149         processCrossfireTelemetryValue(ATTITUDE_ROLL_INDEX, value/10);
 150       if (getCrossfireTelemetryValue<2>(7, value))
 151         processCrossfireTelemetryValue(ATTITUDE_YAW_INDEX, value/10);
 152       break;
 153
 154     case FLIGHT_MODE_ID:
 155     {
 156       const CrossfireSensor & sensor = crossfireSensors[FLIGHT_MODE_INDEX];
 157       for (int i=0; i<min<int>(16, telemetryRxBuffer[1]-2); i+=4) {
 158         uint32_t value = *((uint32_t *)&telemetryRxBuffer[3+i]);
 159         setTelemetryValue(TELEM_PROTO_CROSSFIRE, sensor.id, 0, sensor.subId, value, sensor.unit, i);
 160       }
 161       break;
 162     }
 163
 164 #if defined(LUA)
 165     default:
 166       if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(telemetryRxBufferCount-2) ) {
 167         for (uint8_t i=1; i<telemetryRxBufferCount-1; i++) {
 168           // destination address and CRC are skipped
 169           luaInputTelemetryFifo->push(telemetryRxBuffer[i]);
 170         }
 171       }
 172       break;
 173 #endif
 174   }
 175 }
 176
 177 bool isCrossfireOutputBufferAvailable()
 178 {
 179   return outputTelemetryBufferSize == 0;
 180 }
 181
 182 void processCrossfireTelemetryData(uint8_t data)
 183 {
 184   if (telemetryRxBufferCount == 0 && data != RADIO_ADDRESS) {
 185     TRACE("[XF] address 0x%02X error", data);
 186     return;
 187   }
 188
 189   if (telemetryRxBufferCount == 1 && (data < 2 || data > TELEMETRY_RX_PACKET_SIZE-2)) {
 190     TRACE("[XF] length 0x%02X error", data);
 191     telemetryRxBufferCount = 0;
 192     return;
 193   }
 194
 195   if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
 196     telemetryRxBuffer[telemetryRxBufferCount++] = data;
 197   }
 198   else {
 199     TRACE("[XF] array size %d error", telemetryRxBufferCount);
 200     telemetryRxBufferCount = 0;
 201   }
 202
 203   if (telemetryRxBufferCount > 4) {
 204     uint8_t length = telemetryRxBuffer[1];
 205     if (length + 2 == telemetryRxBufferCount) {
 206       processCrossfireTelemetryFrame();
 207       telemetryRxBufferCount = 0;
 208     }
 209   }
 210 }
 211
 212 void crossfireSetDefault(int index, uint8_t id, uint8_t subId)
 213 {
 214   TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
 215
 216   telemetrySensor.id = id;
 217   telemetrySensor.instance = subId;
 218
 219   const CrossfireSensor & sensor = getCrossfireSensor(id, subId);
 220   TelemetryUnit unit = sensor.unit;
 221   if (unit == UNIT_GPS_LATITUDE || unit == UNIT_GPS_LONGITUDE)
 222     unit = UNIT_GPS;
 223   uint8_t prec = min<uint8_t>(2, sensor.precision);
 224   telemetrySensor.init(sensor.name, unit, prec);
 225   if (id == LINK_ID) {
 226     telemetrySensor.logs = true;
 227   }
 228
 229   storageDirty(EE_MODEL);
 230 }