radio/src/sbus.cpp

   1 /*
   2  * Authors (alphabetical order)
   3  * - Andre Bernet <bernet.andre@gmail.com>
   4  * - Andreas Weitl
   5  * - Bertrand Songis <bsongis@gmail.com>
   6  * - Bryan J. Rentoul (Gruvin) <gruvin@gmail.com>
   7  * - Cameron Weeks <th9xer@gmail.com>
   8  * - Erez Raviv
   9  * - Gabriel Birkus
  10  * - Jean-Pierre Parisy
  11  * - Karl Szmutny
  12  * - Michael Blandford
  13  * - Michal Hlavinka
  14  * - Pat Mackenzie
  15  * - Philip Moss
  16  * - Rob Thomson
  17  * - Romolo Manfredini <romolo.manfredini@gmail.com>
  18  * - Thomas Husterer
  19  *
  20  * opentx is based on code named
  21  * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/,
  22  * er9x by Erez Raviv: http://code.google.com/p/er9x/,
  23  * and the original (and ongoing) project by
  24  * Thomas Husterer, th9x: http://code.google.com/p/th9x/
  25  *
  26  * This program is free software; you can redistribute it and/or modify
  27  * it under the terms of the GNU General Public License version 2 as
  28  * published by the Free Software Foundation.
  29  *
  30  * This program is distributed in the hope that it will be useful,
  31  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  32  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  33  * GNU General Public License for more details.
  34  *
  35  */
  36
  37 #include "opentx.h"
  38
  39 #define SBUS_MIN_FRAME_SIZE   23
  40 #define SBUS_MAX_FRAME_SIZE   28
  41
  42 #define SBUS_FRAME_GAP_DELAY 1000 // 500uS
  43
  44 #define SBUS_START_BYTE     0x0F
  45 #define SBUS_FLAGS_IDX        23
  46 #define SBUS_FRAMELOST_BIT     2
  47 #define SBUS_FAILSAFE_BIT      3
  48
  49 #define SBUS_CH_BITS          11
  50 #define SBUS_CH_MASK ((1<<SBUS_CH_BITS)-1)
  51
  52 #define SBUS_CH_CENTER     0x3E0
  53
  54 Fifo<32> sbusFifo;
  55 uint8_t SbusFrame[SBUS_MAX_FRAME_SIZE];
  56 uint16_t SbusTimer ;
  57 uint8_t SbusIndex = 0 ;
  58
  59 void processSbusFrame(uint8_t *sbus, int16_t *pulses, uint32_t size)
  60 {
  61   uint32_t inputbitsavailable = 0;
  62   uint32_t inputbits = 0;
  63
  64   if (sbus[0] != SBUS_START_BYTE) {
  65     return; // not a valid SBUS frame
  66   }
  67
  68   if (size < SBUS_MIN_FRAME_SIZE) {
  69     return;
  70   }
  71
  72   if (size > SBUS_FLAGS_IDX &&
  73       (sbus[SBUS_FLAGS_IDX-1] & (1<<SBUS_FAILSAFE_BIT))) {
  74     return; // SBUS failsafe mode
  75   }
  76
  77   // Skip start byte
  78   sbus++;
  79
  80   for (uint32_t i=0; i<NUM_TRAINER; i++) {
  81     while (inputbitsavailable < SBUS_CH_BITS) {
  82       inputbits |= *sbus++ << inputbitsavailable;
  83       inputbitsavailable += 8;
  84     }
  85     *pulses++ = ((int32_t) (inputbits & SBUS_CH_MASK) - SBUS_CH_CENTER) * 5 / 8;
  86     inputbitsavailable -= SBUS_CH_BITS;
  87     inputbits >>= SBUS_CH_BITS;
  88   }
  89
  90   ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT;
  91 }
  92
  93 void processSbusInput()
  94 {
  95   uint8_t rxchar;
  96   uint32_t active = 0;
  97   while (sbusFifo.pop(rxchar)) {
  98     active = 1;
  99     SbusFrame[SbusIndex++] = rxchar;
 100     if (SbusIndex > SBUS_MAX_FRAME_SIZE-1) {
 101       SbusIndex = SBUS_MAX_FRAME_SIZE-1;
 102     }
 103   }
 104   if (active) {
 105     SbusTimer = getTmr2MHz();
 106     return;
 107   }
 108   else {
 109     if (SbusIndex) {
 110       if ((uint16_t) (getTmr2MHz() - SbusTimer) > SBUS_FRAME_GAP_DELAY) {
 111         processSbusFrame(SbusFrame, ppmInput, SbusIndex);
 112         SbusIndex = 0;
 113       }
 114     }
 115   }
 116 }