radio/src/sbus.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 #include "sbus.h"
  23
  24 #define SBUS_FRAME_GAP_DELAY   1000 // 500uS
  25
  26 #define SBUS_START_BYTE        0x0F
  27 #define SBUS_END_BYTE          0x00
  28 #define SBUS_FLAGS_IDX         23
  29 #define SBUS_FRAMELOST_BIT     2
  30 #define SBUS_FAILSAFE_BIT      3
  31
  32 #define SBUS_CH_BITS           11
  33 #define SBUS_CH_MASK           ((1<<SBUS_CH_BITS) - 1)
  34
  35 #define SBUS_CH_CENTER         0x3E0
  36
  37 // Range for pulses (ppm input) is [-512:+512]
  38 void processSbusFrame(uint8_t * sbus, int16_t * pulses, uint32_t size)
  39 {
  40   if (size != SBUS_FRAME_SIZE || sbus[0] != SBUS_START_BYTE || sbus[SBUS_FRAME_SIZE-1] != SBUS_END_BYTE) {
  41     return; // not a valid SBUS frame
  42   }
  43   if ((sbus[SBUS_FLAGS_IDX] & (1<<SBUS_FAILSAFE_BIT)) || (sbus[SBUS_FLAGS_IDX] & (1<<SBUS_FRAMELOST_BIT))) {
  44     return; // SBUS invalid frame or failsafe mode
  45   }
  46
  47   sbus++; // skip start byte
  48
  49   uint32_t inputbitsavailable = 0;
  50   uint32_t inputbits = 0;
  51   for (uint32_t i=0; i<MAX_TRAINER_CHANNELS; i++) {
  52     while (inputbitsavailable < SBUS_CH_BITS) {
  53       inputbits |= *sbus++ << inputbitsavailable;
  54       inputbitsavailable += 8;
  55     }
  56     *pulses++ = ((int32_t) (inputbits & SBUS_CH_MASK) - SBUS_CH_CENTER) * 5 / 8;
  57     inputbitsavailable -= SBUS_CH_BITS;
  58     inputbits >>= SBUS_CH_BITS;
  59   }
  60
  61   ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT;
  62 }
  63
  64 void processSbusInput()
  65 {
  66 #if !defined(SIMU)
  67   uint8_t rxchar;
  68   uint32_t active = 0;
  69   static uint8_t SbusIndex = 0;
  70   static uint16_t SbusTimer;
  71   static uint8_t SbusFrame[SBUS_FRAME_SIZE];
  72
  73   while (sbusGetByte(&rxchar)) {
  74     active = 1;
  75     if (SbusIndex > SBUS_FRAME_SIZE-1) {
  76       SbusIndex = SBUS_FRAME_SIZE-1;
  77     }
  78     SbusFrame[SbusIndex++] = rxchar;
  79   }
  80   if (active) {
  81     SbusTimer = getTmr2MHz();
  82     return;
  83   }
  84   else {
  85     if (SbusIndex) {
  86       if ((uint16_t) (getTmr2MHz() - SbusTimer) > SBUS_FRAME_GAP_DELAY) {
  87         processSbusFrame(SbusFrame, ppmInput, SbusIndex);
  88         SbusIndex = 0;
  89       }
  90     }
  91   }
  92 #endif
  93 }