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