radio/src/telemetry/frsky.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 processFrskyTelemetryData(uint8_t data)
  24 {
  25 #if defined(PCBSKY9X) && defined(BLUETOOTH)
  26   // TODO if (g_model.bt_telemetry)
  27   btPushByte(data);
  28 #endif
  29
  30 #if defined(AUX_SERIAL)
  31   if (g_eeGeneral.auxSerialMode == UART_MODE_TELEMETRY_MIRROR) {
  32     auxSerialPutc(data);
  33   }
  34 #endif
  35
  36   if (pushFrskyTelemetryData(data)) {
  37     if (IS_FRSKY_SPORT_PROTOCOL()) {
  38       sportProcessTelemetryPacket(telemetryRxBuffer);
  39     }
  40     else {
  41       frskyDProcessPacket(telemetryRxBuffer);
  42     }
  43   }
  44 }
  45
  46 bool pushFrskyTelemetryData(uint8_t data)
  47 {
  48   static uint8_t dataState = STATE_DATA_IDLE;
  49
  50   switch (dataState) {
  51     case STATE_DATA_START:
  52       if (data == START_STOP) {
  53         if (IS_FRSKY_SPORT_PROTOCOL()) {
  54           dataState = STATE_DATA_IN_FRAME ;
  55           telemetryRxBufferCount = 0;
  56         }
  57       }
  58       else {
  59         if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
  60           telemetryRxBuffer[telemetryRxBufferCount++] = data;
  61         }
  62         dataState = STATE_DATA_IN_FRAME;
  63       }
  64       break;
  65
  66     case STATE_DATA_IN_FRAME:
  67       if (data == BYTE_STUFF) {
  68         dataState = STATE_DATA_XOR; // XOR next byte
  69       }
  70       else if (data == START_STOP) {
  71         if (IS_FRSKY_SPORT_PROTOCOL()) {
  72           dataState = STATE_DATA_IN_FRAME ;
  73           telemetryRxBufferCount = 0;
  74         }
  75         else {
  76           // end of frame detected
  77           dataState = STATE_DATA_IDLE;
  78           return true;
  79         }
  80         break;
  81       }
  82       else if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
  83         telemetryRxBuffer[telemetryRxBufferCount++] = data;
  84       }
  85       break;
  86
  87     case STATE_DATA_XOR:
  88       if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
  89         telemetryRxBuffer[telemetryRxBufferCount++] = data ^ STUFF_MASK;
  90       }
  91       dataState = STATE_DATA_IN_FRAME;
  92       break;
  93
  94     case STATE_DATA_IDLE:
  95       if (data == START_STOP) {
  96         telemetryRxBufferCount = 0;
  97         dataState = STATE_DATA_START;
  98       }
  99       break;
 100
 101   } // switch
 102
 103   if (IS_FRSKY_SPORT_PROTOCOL() && telemetryRxBufferCount >= FRSKY_SPORT_PACKET_SIZE) {
 104     // end of frame detected
 105     dataState = STATE_DATA_IDLE;
 106     return true;
 107   }
 108
 109   return false;
 110 }
 111
 112