/*
 * Authors (alphabetical order)
 * - Andre Bernet <bernet.andre@gmail.com>
 * - Andreas Weitl
 * - Bertrand Songis <bsongis@gmail.com>
 * - Bryan J. Rentoul (Gruvin) <gruvin@gmail.com>
 * - Cameron Weeks <th9xer@gmail.com>
 * - Erez Raviv
 * - Gabriel Birkus
 * - Jean-Pierre Parisy
 * - Karl Szmutny
 * - Michael Blandford
 * - Michal Hlavinka
 * - Pat Mackenzie
 * - Philip Moss
 * - Rob Thomson
 * - Romolo Manfredini <romolo.manfredini@gmail.com>
 * - Thomas Husterer
 *
 * opentx is based on code named
 * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/,
 * er9x by Erez Raviv: http://code.google.com/p/er9x/,
 * and the original (and ongoing) project by
 * Thomas Husterer, th9x: http://code.google.com/p/th9x/
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 */

#include "../opentx.h"

#define START_STOP              0x7e
#define BYTESTUFF               0x7d
#define STUFF_MASK              0x20

// FrSky PRIM IDs (1 byte)
#define DATA_FRAME              0x10

// FrSky old DATA IDs (1 byte)
#define GPS_ALT_BP_ID           0x01
#define TEMP1_ID                0x02
#define RPM_ID                  0x03
#define FUEL_ID                 0x04
#define TEMP2_ID                0x05
#define VOLTS_ID                0x06
#define GPS_ALT_AP_ID           0x09
#define BARO_ALT_BP_ID          0x10
#define GPS_SPEED_BP_ID         0x11
#define GPS_LONG_BP_ID          0x12
#define GPS_LAT_BP_ID           0x13
#define GPS_COURS_BP_ID         0x14
#define GPS_DAY_MONTH_ID        0x15
#define GPS_YEAR_ID             0x16
#define GPS_HOUR_MIN_ID         0x17
#define GPS_SEC_ID              0x18
#define GPS_SPEED_AP_ID         0x19
#define GPS_LONG_AP_ID          0x1A
#define GPS_LAT_AP_ID           0x1B
#define GPS_COURS_AP_ID         0x1C
#define BARO_ALT_AP_ID          0x21
#define GPS_LONG_EW_ID          0x22
#define GPS_LAT_NS_ID           0x23
#define ACCEL_X_ID              0x24
#define ACCEL_Y_ID              0x25
#define ACCEL_Z_ID              0x26
#define CURRENT_ID              0x28
#define VARIO_ID                0x30
#define VFAS_ID                 0x39
#define VOLTS_BP_ID             0x3A
#define VOLTS_AP_ID             0x3B
#define FRSKY_LAST_ID           0x3F

// FrSky new DATA IDs (2 bytes)
#define RSSI_ID                 0xf101
#define ADC1_ID                 0xf102
#define ADC2_ID                 0xf103
#define BATT_ID                 0xf104
#define SWR_ID                  0xf105
#define T1_FIRST_ID             0x0400
#define T1_LAST_ID              0x040f
#define T2_FIRST_ID             0x0410
#define T2_LAST_ID              0x041f
#define RPM_FIRST_ID            0x0500
#define RPM_LAST_ID             0x050f
#define FUEL_FIRST_ID           0x0600
#define FUEL_LAST_ID            0x060f
#define ALT_FIRST_ID            0x0100
#define ALT_LAST_ID             0x010f
#define VARIO_FIRST_ID          0x0110
#define VARIO_LAST_ID           0x011f
#define ACCX_FIRST_ID           0x0700
#define ACCX_LAST_ID            0x070f
#define ACCY_FIRST_ID           0x0710
#define ACCY_LAST_ID            0x071f
#define ACCZ_FIRST_ID           0x0720
#define ACCZ_LAST_ID            0x072f
#define CURR_FIRST_ID           0x0200
#define CURR_LAST_ID            0x020f
#define VFAS_FIRST_ID           0x0210
#define VFAS_LAST_ID            0x021f
#define CELLS_FIRST_ID          0x0300
#define CELLS_LAST_ID           0x030f
#define GPS_LONG_LATI_FIRST_ID  0x0800
#define GPS_LONG_LATI_LAST_ID   0x080f
#define GPS_ALT_FIRST_ID        0x0820
#define GPS_ALT_LAST_ID         0x082f
#define GPS_SPEED_FIRST_ID      0x0830
#define GPS_SPEED_LAST_ID       0x083f
#define GPS_COURS_FIRST_ID      0x0840
#define GPS_COURS_LAST_ID       0x084f
#define GPS_TIME_DATE_FIRST_ID  0x0850
#define GPS_TIME_DATE_LAST_ID   0x085f

// FrSky wrong IDs ?
#define BETA_VARIO_ID           0x8030
#define BETA_BARO_ALT_ID        0x8010

uint8_t telemetryState = TELEMETRY_INIT;

uint8_t frskyRxBuffer[FRSKY_RX_PACKET_SIZE];   // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
uint8_t frskyTxBuffer[FRSKY_TX_PACKET_SIZE];   // Ditto for transmit buffer
#if !defined(CPUARM)
uint8_t frskyTxBufferCount = 0;
#endif
uint8_t link_counter = 0;
FrskyData frskyData;

#if defined(DEBUG) && !defined(SIMU)
extern FIL g_telemetryFile;
#endif

Fifo<512> telemetryFifo;

void setBaroAltitude(int32_t baroAltitude)
{
  // First received barometer altitude => Altitude offset
  if (!frskyData.hub.baroAltitudeOffset)
    frskyData.hub.baroAltitudeOffset = -baroAltitude;

  baroAltitude += frskyData.hub.baroAltitudeOffset;
  frskyData.hub.baroAltitude = baroAltitude;

  baroAltitude /= 100;
  if (baroAltitude > frskyData.hub.maxAltitude)
    frskyData.hub.maxAltitude = baroAltitude;
  if (baroAltitude < frskyData.hub.minAltitude)
    frskyData.hub.minAltitude = baroAltitude;
}

void processHubPacket(uint8_t id, uint16_t value)
{
  if (id > FRSKY_LAST_ID)
    return;

  if (id == GPS_LAT_BP_ID) {
    if (value)
      frskyData.hub.gpsFix = 1;
    else if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLatitude_bp > 1)
      frskyData.hub.gpsFix = 0;
  }
  else if (id == GPS_LONG_BP_ID) {
    if (value)
      frskyData.hub.gpsFix = 1;
    else if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLongitude_bp > 1)
      frskyData.hub.gpsFix = 0;
  }

  if (id == GPS_ALT_BP_ID || (id >= GPS_ALT_AP_ID && id <= GPS_LAT_NS_ID && id != BARO_ALT_BP_ID && id != BARO_ALT_AP_ID)) {
    // if we don't have a fix, we may discard the value
    if (frskyData.hub.gpsFix <= 0)
      return;
  }

  ((uint16_t*)&frskyData.hub)[id] = value;

  switch (id) {

    case RPM_ID:
      frskyData.hub.rpm *= (uint8_t)60/(g_model.frsky.blades+2);
      if (frskyData.hub.rpm > frskyData.hub.maxRpm)
        frskyData.hub.maxRpm = frskyData.hub.rpm;
      break;
      
    case TEMP1_ID:
      if (frskyData.hub.temperature1 > frskyData.hub.maxTemperature1)
        frskyData.hub.maxTemperature1 = frskyData.hub.temperature1;
      break;

    case TEMP2_ID:
      if (frskyData.hub.temperature2 > frskyData.hub.maxTemperature2)
        frskyData.hub.maxTemperature2 = frskyData.hub.temperature2;
      break;

    case CURRENT_ID:
      if ((int16_t)frskyData.hub.current > 0 && ((int16_t)frskyData.hub.current + g_model.frsky.fasOffset) > 0)
        frskyData.hub.current += g_model.frsky.fasOffset;
      else
        frskyData.hub.current = 0;
      if (frskyData.hub.current > frskyData.hub.maxCurrent)
        frskyData.hub.maxCurrent = frskyData.hub.current;
      break;

    case VOLTS_AP_ID:
#if defined(FAS_BSS)
      frskyData.hub.vfas = (frskyData.hub.volts_bp * 10 + frskyData.hub.volts_ap);
#else
      frskyData.hub.vfas = ((frskyData.hub.volts_bp * 100 + frskyData.hub.volts_ap * 10) * 21) / 110;
#endif
      if (!frskyData.hub.minVfas || frskyData.hub.vfas < frskyData.hub.minVfas)
        frskyData.hub.minVfas = frskyData.hub.vfas;
      break;

    case BARO_ALT_AP_ID:
      if (frskyData.hub.baroAltitude_ap > 9)
        frskyData.hub.varioHighPrecision = true;
      if (!frskyData.hub.varioHighPrecision)
        frskyData.hub.baroAltitude_ap *= 10;
      setBaroAltitude((int32_t)100 * frskyData.hub.baroAltitude_bp + (frskyData.hub.baroAltitude_bp >= 0 ? frskyData.hub.baroAltitude_ap : -frskyData.hub.baroAltitude_ap));
      break;

    case GPS_ALT_AP_ID:
    {
      frskyData.hub.gpsAltitude = (frskyData.hub.gpsAltitude_bp * 100) + frskyData.hub.gpsAltitude_ap;
      if (!frskyData.hub.gpsAltitudeOffset)
        frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude;
      if (!frskyData.hub.baroAltitudeOffset) {
        int altitude = TELEMETRY_RELATIVE_GPS_ALT_BP;
        if (altitude > frskyData.hub.maxAltitude)
          frskyData.hub.maxAltitude = altitude;
        if (altitude < frskyData.hub.minAltitude)
          frskyData.hub.minAltitude = altitude;
      }
      if (!frskyData.hub.pilotLatitude && !frskyData.hub.pilotLongitude) {
        // First received GPS position => Pilot GPS position
        getGpsPilotPosition();
      }
      else if (frskyData.hub.gpsDistNeeded || g_menuStack[g_menuStackPtr] == menuTelemetryFrsky) {
        getGpsDistance();
      }
      break;
    }

    case GPS_SPEED_BP_ID:
      // Speed => Max speed
      frskyData.hub.gpsSpeed_bp = (frskyData.hub.gpsSpeed_bp * 46) / 25;
      if (frskyData.hub.gpsSpeed_bp > frskyData.hub.maxGpsSpeed)
        frskyData.hub.maxGpsSpeed = frskyData.hub.gpsSpeed_bp;
      break;

    case VOLTS_ID:
    {
      // Voltage => Cell number + Cell voltage
      uint8_t battnumber = ((frskyData.hub.volts & 0x00F0) >> 4);
      if (battnumber < 12) {
        if (frskyData.hub.cellsCount < battnumber+1) {
          frskyData.hub.cellsCount = battnumber+1;
        }
        uint16_t cellVolts = (uint16_t)(((((frskyData.hub.volts & 0xFF00) >> 8) + ((frskyData.hub.volts & 0x000F) << 8))) / 5);
        frskyData.hub.cellVolts[battnumber] = cellVolts;
        if (!frskyData.hub.minCellVolts || cellVolts<frskyData.hub.minCellVolts || battnumber==frskyData.hub.minCellIdx) {
          frskyData.hub.minCellIdx = battnumber;
          frskyData.hub.minCellVolts = cellVolts;
          if (!frskyData.hub.minCell || frskyData.hub.minCellVolts < frskyData.hub.minCell)
            frskyData.hub.minCell = frskyData.hub.minCellVolts;
        }
      }
      break;
    }

    case GPS_HOUR_MIN_ID:
      frskyData.hub.hour = ((uint8_t)(frskyData.hub.hour + g_eeGeneral.timezone + 24)) % 24;
      break;

    case ACCEL_X_ID:
    case ACCEL_Y_ID:
    case ACCEL_Z_ID:
      ((int16_t*)(&frskyData.hub))[id] /= 10;
      break;

  }
}

bool checkSportPacket(uint8_t *packet)
{
  short crc = 0;
  for (int i=1; i<FRSKY_RX_PACKET_SIZE; i++) {
    crc += packet[i]; //0-1FF
    crc += crc >> 8; //0-100
    crc &= 0x00ff;
    crc += crc >> 8; //0-0FF
    crc &= 0x00ff;
  }
  return (crc == 0x00ff);
}

#define SPORT_DATA_U8(packet)   (packet[4])
#define SPORT_DATA_S32(packet)  (*((int32_t *)(packet+4)))
#define SPORT_DATA_U32(packet)  (*((uint32_t *)(packet+4)))
#define HUB_DATA_U16(packet)    (*((uint16_t *)(packet+4)))

void processSportPacket(uint8_t *packet)
{
  /* uint8_t  dataId = packet[0]; */
  uint8_t  prim   = packet[1];
  uint16_t appId  = *((uint16_t *)(packet+2));

  if (!checkSportPacket(packet)) {
    return;
  }

  switch (prim)
  {
    case DATA_FRAME:

      if (appId == RSSI_ID) {
        frskyData.rssi[0].set(SPORT_DATA_U8(packet));
      }
      if (appId == SWR_ID) {
        frskyData.swr.set(SPORT_DATA_U8(packet));
      }
      else if (appId == ADC1_ID || appId == ADC2_ID) {
        // A1/A2 of DxR receivers
        frskyData.analog[appId-ADC1_ID].set(SPORT_DATA_U8(packet));
#if defined(VARIO)
        uint8_t varioSource = g_model.frsky.varioSource - VARIO_SOURCE_A1;
        if (varioSource == appId-ADC1_ID) {
          frskyData.hub.varioSpeed = applyChannelRatio(varioSource, frskyData.analog[varioSource].value);
        }
#endif
      }
      else if (appId == BATT_ID) {
        frskyData.analog[0].set(SPORT_DATA_U8(packet));
      }
      else if ((appId >> 8) == 0) {
        // The old FrSky IDs
        uint8_t  id = (uint8_t)appId;
        uint16_t value = HUB_DATA_U16(packet);
        processHubPacket(id, value);
      }
      else if (appId == BETA_BARO_ALT_ID) {
        int32_t baroAltitude = SPORT_DATA_S32(packet);
        setBaroAltitude(10 * (baroAltitude >> 8));
      }
      else if (appId == BETA_VARIO_ID) {
        int32_t varioSpeed = SPORT_DATA_S32(packet);
        frskyData.hub.varioSpeed = 10 * (varioSpeed >> 8);
      }
      else if (appId >= T1_FIRST_ID && appId <= T1_LAST_ID) {
        frskyData.hub.temperature1 = SPORT_DATA_S32(packet);
        if (frskyData.hub.temperature1 > frskyData.hub.maxTemperature1)
          frskyData.hub.maxTemperature1 = frskyData.hub.temperature1;
      }
      else if (appId >= T2_FIRST_ID && appId <= T2_LAST_ID) {
        frskyData.hub.temperature2 = SPORT_DATA_S32(packet);
        if (frskyData.hub.temperature2 > frskyData.hub.maxTemperature2)
          frskyData.hub.maxTemperature2 = frskyData.hub.temperature2;
      }
      else if (appId >= RPM_FIRST_ID && appId <= RPM_LAST_ID) {
        frskyData.hub.rpm = SPORT_DATA_U32(packet) / (g_model.frsky.blades+2);
        if (frskyData.hub.rpm > frskyData.hub.maxRpm)
          frskyData.hub.maxRpm = frskyData.hub.rpm;
      }
      else if (appId >= FUEL_FIRST_ID && appId <= FUEL_LAST_ID) {
        frskyData.hub.fuelLevel = SPORT_DATA_U32(packet);
      }
      else if (appId >= ALT_FIRST_ID && appId <= ALT_LAST_ID) {
        setBaroAltitude(SPORT_DATA_S32(packet));
      }
      else if (appId >= VARIO_FIRST_ID && appId <= VARIO_LAST_ID) {
        frskyData.hub.varioSpeed = SPORT_DATA_S32(packet);
      }
      else if (appId >= ACCX_FIRST_ID && appId <= ACCX_LAST_ID) {
        frskyData.hub.accelX = SPORT_DATA_S32(packet);
      }
      else if (appId >= ACCY_FIRST_ID && appId <= ACCY_LAST_ID) {
        frskyData.hub.accelY = SPORT_DATA_S32(packet);
      }
      else if (appId >= ACCZ_FIRST_ID && appId <= ACCZ_LAST_ID) {
        frskyData.hub.accelZ = SPORT_DATA_S32(packet);
      }
      else if (appId >= CURR_FIRST_ID && appId <= CURR_LAST_ID) {
        frskyData.hub.current = SPORT_DATA_U32(packet);
        if (((int16_t)frskyData.hub.current + g_model.frsky.fasOffset)>0)
          frskyData.hub.current += g_model.frsky.fasOffset;
        else
          frskyData.hub.current = 0;
        if (frskyData.hub.current > frskyData.hub.maxCurrent)
          frskyData.hub.maxCurrent = frskyData.hub.current;
      }
      else if (appId >= VFAS_FIRST_ID && appId <= VFAS_LAST_ID) {
        frskyData.hub.vfas = SPORT_DATA_U32(packet)/10;   //TODO: remove /10 and display with PREC2 when using SPORT
      }
      else if (appId >= GPS_SPEED_FIRST_ID && appId <= GPS_SPEED_LAST_ID) {
        frskyData.hub.gpsSpeed_bp = SPORT_DATA_U32(packet);
        frskyData.hub.gpsSpeed_bp = (frskyData.hub.gpsSpeed_bp * 46) / 25 / 1000;
        if (frskyData.hub.gpsSpeed_bp > frskyData.hub.maxGpsSpeed)
          frskyData.hub.maxGpsSpeed = frskyData.hub.gpsSpeed_bp;
      }
      else if (appId >= GPS_TIME_DATE_FIRST_ID && appId <= GPS_TIME_DATE_LAST_ID) {
        uint32_t gps_time_date = SPORT_DATA_U32(packet);
        if (gps_time_date & 0x000000ff) {
          frskyData.hub.year = (uint16_t) ((gps_time_date & 0xff000000) >> 24);
          frskyData.hub.month = (uint8_t) ((gps_time_date & 0x00ff0000) >> 16);
          frskyData.hub.day = (uint8_t) ((gps_time_date & 0x0000ff00) >> 8);
        }
        else {
          frskyData.hub.hour = (uint8_t) ((gps_time_date & 0xff000000) >> 24);
          frskyData.hub.min = (uint8_t) ((gps_time_date & 0x00ff0000) >> 16);
          frskyData.hub.sec = (uint16_t) ((gps_time_date & 0x0000ff00) >> 8);
          frskyData.hub.hour = ((uint8_t) (frskyData.hub.hour + g_eeGeneral.timezone + 24)) % 24;
        }
      }
      else if (appId >= GPS_COURS_FIRST_ID && appId <= GPS_COURS_LAST_ID) {
        uint32_t course = SPORT_DATA_U32(packet);
        frskyData.hub.gpsCourse_bp = course / 100;
        frskyData.hub.gpsCourse_ap = course % 100;
      }
      else if (appId >= GPS_ALT_FIRST_ID && appId <= GPS_ALT_LAST_ID) {
        frskyData.hub.gpsAltitude = SPORT_DATA_S32(packet);
        
        if (!frskyData.hub.gpsAltitudeOffset)
          frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude;

        if (!frskyData.hub.baroAltitudeOffset) {
          int altitude = TELEMETRY_RELATIVE_GPS_ALT_BP;
          if (altitude > frskyData.hub.maxAltitude)
            frskyData.hub.maxAltitude = altitude;
          if (altitude < frskyData.hub.minAltitude)
            frskyData.hub.minAltitude = altitude;
        }

        if (frskyData.hub.gpsFix > 0) {
          if (!frskyData.hub.pilotLatitude && !frskyData.hub.pilotLongitude) {
            // First received GPS position => Pilot GPS position
	    getGpsPilotPosition();
          }
          else if (frskyData.hub.gpsDistNeeded || g_menuStack[g_menuStackPtr] == menuTelemetryFrsky) {
            getGpsDistance();
          }
        }
      }
      else if (appId >= GPS_LONG_LATI_FIRST_ID && appId <= GPS_LONG_LATI_LAST_ID) {
        uint32_t gps_long_lati_data = SPORT_DATA_U32(packet);
        uint32_t gps_long_lati_b1w, gps_long_lati_a1w;
        gps_long_lati_b1w = (gps_long_lati_data & 0x3fffffff) / 10000;
        gps_long_lati_a1w = (gps_long_lati_data & 0x3fffffff) % 10000;
        switch ((gps_long_lati_data & 0xc0000000) >> 30) {
          case 0:
            frskyData.hub.gpsLatitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
            frskyData.hub.gpsLatitude_ap = gps_long_lati_a1w;
            frskyData.hub.gpsLatitudeNS = 'N';
            break;
          case 1:
            frskyData.hub.gpsLatitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
            frskyData.hub.gpsLatitude_ap = gps_long_lati_a1w;
            frskyData.hub.gpsLatitudeNS = 'S';
            break;
          case 2:
            frskyData.hub.gpsLongitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
            frskyData.hub.gpsLongitude_ap = gps_long_lati_a1w;
            frskyData.hub.gpsLongitudeEW = 'E';
            break;
          case 3:
            frskyData.hub.gpsLongitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
            frskyData.hub.gpsLongitude_ap = gps_long_lati_a1w;
            frskyData.hub.gpsLongitudeEW = 'W';
            break;
        }
        if (frskyData.hub.gpsLongitudeEW && frskyData.hub.gpsLatitudeNS) {
          frskyData.hub.gpsFix = 1;
        }
        else if (frskyData.hub.gpsFix > 0) {
          frskyData.hub.gpsFix = 0;
        }
      }
	  
      else if (appId >= CELLS_FIRST_ID && appId <= CELLS_LAST_ID) {
        uint32_t cells = SPORT_DATA_U32(packet);
        uint8_t battnumber = cells & 0xF;
        uint32_t minCell, minCellNum;
        
        //TODO: Use reported total voltages (bits 4-7)?
        frskyData.hub.cellVolts[battnumber] = ((cells & 0x000FFF00) >> 8) / 5;
        frskyData.hub.cellVolts[battnumber+1] = ((cells & 0xFFF00000) >> 20) / 5;
        
        if (frskyData.hub.cellsCount < battnumber+2)
          frskyData.hub.cellsCount = battnumber+2;
        if (frskyData.hub.cellVolts[battnumber+1] == 0)
          frskyData.hub.cellsCount--;
        
        if ((frskyData.hub.cellVolts[battnumber] < frskyData.hub.cellVolts[battnumber+1]) || (frskyData.hub.cellVolts[battnumber+1] == 0)) {
          minCell = frskyData.hub.cellVolts[battnumber];
          minCellNum = battnumber;
        }
        else {
          minCell = frskyData.hub.cellVolts[battnumber+1];
          minCellNum = battnumber+1;
        }
        	
        if (!frskyData.hub.minCellVolts || minCell < frskyData.hub.minCellVolts || minCellNum==frskyData.hub.minCellIdx) {
          frskyData.hub.minCellIdx = minCellNum;
          frskyData.hub.minCellVolts = minCell;
        }
      }
      break;

  }
}

// Receive buffer state machine state enum
enum FrSkyDataState {
  STATE_DATA_IDLE,
  STATE_DATA_IN_FRAME,
  STATE_DATA_XOR,
};

void processSerialData(uint8_t data)
{
  static uint8_t numPktBytes = 0;
  static uint8_t dataState = STATE_DATA_IDLE;

#if defined(BLUETOOTH)
  // TODO if (g_model.bt_telemetry)
  btPushByte(data);
#endif

  if (g_eeGeneral.uart3Mode == UART_MODE_SPORT) {
    uart3Putc(data);
  }

  if (data == START_STOP) {
    dataState = STATE_DATA_IN_FRAME;
    numPktBytes = 0;
  }
  else {
    switch (dataState) {
      case STATE_DATA_XOR:
        frskyRxBuffer[numPktBytes++] = data ^ STUFF_MASK;
        dataState = STATE_DATA_IN_FRAME;
        break;

      case STATE_DATA_IN_FRAME:
        if (data == BYTESTUFF)
          dataState = STATE_DATA_XOR; // XOR next byte
        else
          frskyRxBuffer[numPktBytes++] = data;
        break;
    }
  }

  if (numPktBytes == FRSKY_RX_PACKET_SIZE) {
    processSportPacket(frskyRxBuffer);
    dataState = STATE_DATA_IDLE;
  }
}

uint8_t frskyAlarmsSendState = 0 ;


void telemetryInterrupt10ms()
{
  uint16_t voltage = 0; /* unit: 1/10 volts */

#if defined(FRSKY_HUB)
  for (uint8_t i=0; i<frskyData.hub.cellsCount; i++)
    voltage += frskyData.hub.cellVolts[i];
  voltage /= 10;
  frskyData.hub.cellsSum = voltage;
  if (frskyData.hub.cellsSum < frskyData.hub.minCells) {
    frskyData.hub.minCells = frskyData.hub.cellsSum;
  }
#endif

  if (TELEMETRY_STREAMING()) {
    uint8_t channel = g_model.frsky.voltsSource;
    if (channel <= 1) {
      voltage = applyChannelRatio(channel, frskyData.analog[channel].value) / 10;
    }
#if defined(FRSKY_HUB)
    else if (channel == 2) {
      voltage = frskyData.hub.vfas;
    }
#endif

#if defined(FRSKY_HUB)
    uint16_t current = frskyData.hub.current; /* unit: 1/10 amps */
#else
    uint16_t current = 0;
#endif

    channel = g_model.frsky.currentSource - FRSKY_CURRENT_SOURCE_A1;
    if (channel <= 1) {
      current = applyChannelRatio(channel, frskyData.analog[channel].value) / 10;
    }

    frskyData.hub.power = (current * voltage) / 100;
    if (frskyData.hub.power > frskyData.hub.maxPower)
      frskyData.hub.maxPower = frskyData.hub.power;

    frskyData.hub.currentPrescale += current;
    if (frskyData.hub.currentPrescale >= 3600) {
      frskyData.hub.currentConsumption += 1;
      frskyData.hub.currentPrescale -= 3600;
    }
  }
}

inline bool alarmRaised(uint8_t channel, uint8_t idx)
{
  return g_model.frsky.channels[channel].ratio > 0 && g_model.frsky.channels[channel].alarms_value[idx] > 0 && frskyData.analog[channel].value < g_model.frsky.channels[channel].alarms_value[idx];
}

void telemetryWakeup()
{
#if defined(PCBTARANIS)
  uint8_t data;
#if defined(DEBUG) && !defined(SIMU)
  static tmr10ms_t lastTime = 0;
  tmr10ms_t newTime = get_tmr10ms();
  struct gtm utm;
  gettime(&utm);
#endif
  while (telemetryFifo.pop(data)) {
    processSerialData(data);
#if defined(DEBUG) && !defined(SIMU)
    if (lastTime != newTime) {
      f_printf(&g_telemetryFile, "\r\n%4d-%02d-%02d,%02d:%02d:%02d.%02d0: %02X", utm.tm_year+1900, utm.tm_mon+1, utm.tm_mday, utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100, data);
      lastTime = newTime;
    }
    else {
      f_printf(&g_telemetryFile, " %02X", data);
    }
#endif
  }
#elif defined(PCBSKY9X)
  rxPdcUsart(processSerialData);              // Receive serial data here
#endif

#if 0
  // Attempt to transmit any waiting Fr-Sky alarm set packets every 50ms (subject to packet buffer availability)
  static uint8_t frskyTxDelay = 5;
  if (frskyAlarmsSendState && (--frskyTxDelay == 0)) {
    frskyTxDelay = 5; // 50ms
    // frskySendNextAlarm();
  }
#endif

#if defined(VARIO)
  if (TELEMETRY_STREAMING() && !IS_FAI_ENABLED()) {
    varioWakeup();
  }
#endif

  static tmr10ms_t alarmsCheckTime = 0;
  static uint8_t alarmsCheckStep = 0;

  if (int32_t(get_tmr10ms() - alarmsCheckTime) > 0) {

    alarmsCheckTime = get_tmr10ms() + 100; /* next check in 1second */

    if (alarmsCheckStep == 0) {
      if ((IS_MODULE_XJT(0) || IS_MODULE_XJT(1)) && frskyData.swr.value > 0x33) {
        AUDIO_SWR_RED();
        s_global_warning = STR_ANTENNAPROBLEM;
        alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
      }
    }
    else if (TELEMETRY_STREAMING()) {
      if (alarmsCheckStep == 1) {
        if (getRssiAlarmValue(1) && frskyData.rssi[0].value < getRssiAlarmValue(1)) {
          AUDIO_RSSI_RED();
          alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
        }
        else if (getRssiAlarmValue(0) && frskyData.rssi[0].value < getRssiAlarmValue(0)) {
          AUDIO_RSSI_ORANGE();
          alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
        }
      }
      else if (alarmsCheckStep == 2) {
        if (alarmRaised(1, 1)) {
          AUDIO_A2_RED();
          alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
        }
        else if (alarmRaised(1, 0)) {
          AUDIO_A2_ORANGE();
          alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
        }
      }
      else if (alarmsCheckStep == 3) {
        if (alarmRaised(0, 1)) {
          AUDIO_A1_RED();
          alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
        }
        else if (alarmRaised(0, 0)) {
          AUDIO_A1_ORANGE();
          alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
        }
      }
    }

    if (++alarmsCheckStep == 4) {
      alarmsCheckStep = 0;
    }
  }

  if (TELEMETRY_STREAMING()) {
    if (telemetryState == TELEMETRY_KO) {
      AUDIO_TELEMETRY_BACK();
    }
    telemetryState = TELEMETRY_OK;
  }
  else if (telemetryState == TELEMETRY_OK) {
    telemetryState = TELEMETRY_KO;
    AUDIO_TELEMETRY_LOST();
  }
}

void FRSKY_Init(void)
{
  // clear frsky variables
  resetTelemetry();
}

void FRSKY_End(void)
{
}

void FrskyValueWithMinMax::set(uint8_t value)
{
  this->value = value;

  if (value) {
    if (!max || value > max)
      max = value;
    if (!min || value < min)
      min = value;
  }
}

void resetTelemetry()
{
  memclear(&frskyData, sizeof(frskyData));
  telemetryState = TELEMETRY_INIT;

#if defined(FRSKY_HUB)
  frskyData.hub.gpsLatitude_bp = 2;
  frskyData.hub.gpsLongitude_bp = 2;
  frskyData.hub.gpsFix = -1;
#endif

#if defined(SIMU)
  frskyData.analog[0].set(120);
  frskyData.analog[1].set(240);
  frskyData.rssi[0].value = 75;
  frskyData.swr.value = 30;
  frskyData.hub.fuelLevel = 75;
  frskyData.hub.rpm = 12000;
  frskyData.hub.vfas = 100;
  frskyData.hub.minVfas = 90;

  frskyData.hub.gpsFix = 1;
  frskyData.hub.gpsLatitude_bp = 4401;
  frskyData.hub.gpsLatitude_ap = 7710;
  frskyData.hub.gpsLongitude_bp = 1006;
  frskyData.hub.gpsLongitude_ap = 8872;
  getGpsPilotPosition();

  frskyData.hub.gpsLatitude_bp = 4401;
  frskyData.hub.gpsLatitude_ap = 7455;
  frskyData.hub.gpsLongitude_bp = 1006;
  frskyData.hub.gpsLongitude_ap = 9533;
  getGpsDistance();

  frskyData.hub.gpsSpeed_bp = 100;
  frskyData.hub.gpsSpeed_ap = 50;

  frskyData.hub.cellsCount = 6;

  frskyData.hub.baroAltitudeOffset = 500 * 100;
  frskyData.hub.baroAltitude = 50 * 100;
  frskyData.hub.gpsAltitude_bp = 50;

  frskyData.hub.minAltitude = 10;
  frskyData.hub.maxAltitude = 500;

  frskyData.hub.accelY = 100;
  frskyData.hub.temperature1 = -30;
  frskyData.hub.maxTemperature1 = 100;
  
  frskyData.hub.current = 5;
  frskyData.hub.maxCurrent = 56;
#endif
}