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   {CF_VARIO_ID,    0, ZSTR_VSPD,        UNIT_METERS_PER_SECOND, 2},
  48   {0,              0, "UNKNOWN",        UNIT_RAW,               0},
  49 };
  50
  51 const CrossfireSensor & getCrossfireSensor(uint8_t id, uint8_t subId)
  52 {
  53   if (id == LINK_ID)
  54     return crossfireSensors[RX_RSSI1_INDEX+subId];
  55   else if (id == BATTERY_ID)
  56     return crossfireSensors[BATT_VOLTAGE_INDEX+subId];
  57   else if (id == GPS_ID)
  58     return crossfireSensors[GPS_LATITUDE_INDEX+subId];
  59   else if (id == CF_VARIO_ID)
  60     return crossfireSensors[VERTICAL_SPEED_INDEX];
  61   else if (id == ATTITUDE_ID)
  62     return crossfireSensors[ATTITUDE_PITCH_INDEX+subId];
  63   else if (id == FLIGHT_MODE_ID)
  64     return crossfireSensors[FLIGHT_MODE_INDEX];
  65   else
  66     return crossfireSensors[UNKNOWN_INDEX];
  67 }
  68
  69 void processCrossfireTelemetryValue(uint8_t index, int32_t value)
  70 {
  71   if(!TELEMETRY_STREAMING())
  72     return;
  73
  74   const CrossfireSensor & sensor = crossfireSensors[index];
  75   setTelemetryValue(PROTOCOL_TELEMETRY_CROSSFIRE, sensor.id, 0, sensor.subId, value, sensor.unit, sensor.precision);
  76 }
  77
  78 bool checkCrossfireTelemetryFrameCRC()
  79 {
  80   uint8_t len = telemetryRxBuffer[1];
  81   uint8_t crc = crc8(&telemetryRxBuffer[2], len-1);
  82   return (crc == telemetryRxBuffer[len+1]);
  83 }
  84
  85 template<int N>
  86 bool getCrossfireTelemetryValue(uint8_t index, int32_t & value)
  87 {
  88   bool result = false;
  89   uint8_t * byte = &telemetryRxBuffer[index];
  90   value = (*byte & 0x80) ? -1 : 0;
  91   for (uint8_t i=0; i<N; i++) {
  92     value <<= 8;
  93     if (*byte != 0xff) {
  94       result = true;
  95     }
  96     value += *byte++;
  97   }
  98   return result;
  99 }
 100
 101 void processCrossfireTelemetryFrame()
 102 {
 103   if (!checkCrossfireTelemetryFrameCRC()) {
 104     TRACE("[XF] CRC error");
 105     return;
 106   }
 107
 108   uint8_t id = telemetryRxBuffer[2];
 109   int32_t value;
 110   switch(id) {
 111     case CF_VARIO_ID:
 112       if (getCrossfireTelemetryValue<2>(3, value))
 113         processCrossfireTelemetryValue(VERTICAL_SPEED_INDEX, value);
 114       break;
 115
 116     case GPS_ID:
 117       if (getCrossfireTelemetryValue<4>(3, value))
 118         processCrossfireTelemetryValue(GPS_LATITUDE_INDEX, value/10);
 119       if (getCrossfireTelemetryValue<4>(7, value))
 120         processCrossfireTelemetryValue(GPS_LONGITUDE_INDEX, value/10);
 121       if (getCrossfireTelemetryValue<2>(11, value))
 122         processCrossfireTelemetryValue(GPS_GROUND_SPEED_INDEX, value);
 123       if (getCrossfireTelemetryValue<2>(13, value))
 124         processCrossfireTelemetryValue(GPS_HEADING_INDEX, value);
 125       if (getCrossfireTelemetryValue<2>(15, value))
 126         processCrossfireTelemetryValue(GPS_ALTITUDE_INDEX,  value - 1000);
 127       if (getCrossfireTelemetryValue<1>(17, value))
 128         processCrossfireTelemetryValue(GPS_SATELLITES_INDEX, value);
 129       break;
 130
 131     case LINK_ID:
 132       for (unsigned int i=0; i<=TX_SNR_INDEX; i++) {
 133         if (getCrossfireTelemetryValue<1>(3+i, value)) {
 134           if (i == TX_POWER_INDEX) {
 135             static const int32_t power_values[] = { 0, 10, 25, 100, 500, 1000, 2000, 250 };
 136             value = ((unsigned)value < DIM(power_values) ? power_values[value] : 0);
 137           }
 138           processCrossfireTelemetryValue(i, value);
 139           if (i == RX_QUALITY_INDEX) {
 140             if (value) {
 141               telemetryData.rssi.set(value);
 142               telemetryStreaming = TELEMETRY_TIMEOUT10ms;
 143             }
 144             else {
 145               telemetryData.rssi.reset();
 146               telemetryStreaming = 0;
 147             }
 148           }
 149         }
 150       }
 151       break;
 152
 153     case BATTERY_ID:
 154       if (getCrossfireTelemetryValue<2>(3, value))
 155         processCrossfireTelemetryValue(BATT_VOLTAGE_INDEX, value);
 156       if (getCrossfireTelemetryValue<2>(5, value))
 157         processCrossfireTelemetryValue(BATT_CURRENT_INDEX, value);
 158       if (getCrossfireTelemetryValue<3>(7, value))
 159         processCrossfireTelemetryValue(BATT_CAPACITY_INDEX, value);
 160       break;
 161
 162     case ATTITUDE_ID:
 163       if (getCrossfireTelemetryValue<2>(3, value))
 164         processCrossfireTelemetryValue(ATTITUDE_PITCH_INDEX, value/10);
 165       if (getCrossfireTelemetryValue<2>(5, value))
 166         processCrossfireTelemetryValue(ATTITUDE_ROLL_INDEX, value/10);
 167       if (getCrossfireTelemetryValue<2>(7, value))
 168         processCrossfireTelemetryValue(ATTITUDE_YAW_INDEX, value/10);
 169       break;
 170
 171     case FLIGHT_MODE_ID:
 172     {
 173       const CrossfireSensor & sensor = crossfireSensors[FLIGHT_MODE_INDEX];
 174       for (int i=0; i<min<int>(16, telemetryRxBuffer[1]-2); i+=4) {
 175         uint32_t value = *((uint32_t *)&telemetryRxBuffer[3+i]);
 176         setTelemetryValue(PROTOCOL_TELEMETRY_CROSSFIRE, sensor.id, 0, sensor.subId, value, sensor.unit, i);
 177       }
 178       break;
 179     }
 180
 181 #if defined(LUA)
 182     default:
 183       if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(telemetryRxBufferCount-2) ) {
 184         for (uint8_t i=1; i<telemetryRxBufferCount-1; i++) {
 185           // destination address and CRC are skipped
 186           luaInputTelemetryFifo->push(telemetryRxBuffer[i]);
 187         }
 188       }
 189       break;
 190 #endif
 191   }
 192 }
 193
 194 void processCrossfireTelemetryData(uint8_t data)
 195 {
 196 #if defined(AUX_SERIAL)
 197   if (g_eeGeneral.auxSerialMode == UART_MODE_TELEMETRY_MIRROR) {
 198     auxSerialPutc(data);
 199   }
 200 #endif
 201
 202   if (telemetryRxBufferCount == 0 && data != RADIO_ADDRESS) {
 203     TRACE("[XF] address 0x%02X error", data);
 204     return;
 205   }
 206
 207   if (telemetryRxBufferCount == 1 && (data < 2 || data > TELEMETRY_RX_PACKET_SIZE-2)) {
 208     TRACE("[XF] length 0x%02X error", data);
 209     telemetryRxBufferCount = 0;
 210     return;
 211   }
 212
 213   if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
 214     telemetryRxBuffer[telemetryRxBufferCount++] = data;
 215   }
 216   else {
 217     TRACE("[XF] array size %d error", telemetryRxBufferCount);
 218     telemetryRxBufferCount = 0;
 219   }
 220
 221   if (telemetryRxBufferCount > 4) {
 222     uint8_t length = telemetryRxBuffer[1];
 223     if (length + 2 == telemetryRxBufferCount) {
 224 #if defined(BLUETOOTH)
 225       if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY && bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
 226         bluetooth.write(telemetryRxBuffer, telemetryRxBufferCount);
 227       }
 228 #endif
 229       processCrossfireTelemetryFrame();
 230       telemetryRxBufferCount = 0;
 231     }
 232   }
 233 }
 234
 235 void crossfireSetDefault(int index, uint8_t id, uint8_t subId)
 236 {
 237   TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
 238
 239   telemetrySensor.id = id;
 240   telemetrySensor.instance = subId;
 241
 242   const CrossfireSensor & sensor = getCrossfireSensor(id, subId);
 243   TelemetryUnit unit = sensor.unit;
 244   if (unit == UNIT_GPS_LATITUDE || unit == UNIT_GPS_LONGITUDE)
 245     unit = UNIT_GPS;
 246   uint8_t prec = min<uint8_t>(2, sensor.precision);
 247   telemetrySensor.init(sensor.name, unit, prec);
 248   if (id == LINK_ID) {
 249     telemetrySensor.logs = true;
 250   }
 251
 252   storageDirty(EE_MODEL);
 253 }