radio/src/pulses/sbus_arm.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
  24 #define BITLEN_SBUS          (10*2) // 100000 Baud => 10uS per bit
  25
  26
  27 /* The protocol reuse some the DSM2 definitions where they are identical */
  28
  29
  30 #if defined(PPM_PIN_SERIAL)
  31 void sendByteSbus(uint8_t b)
  32 {
  33   uint8_t parity = 1;
  34
  35   putDsm2SerialBit(0);           // Start bit
  36   for (uint8_t i=0; i<8; i++) {  // 8 data Bits
  37     putDsm2SerialBit(b & 1);
  38     parity = parity ^ (b & 1);
  39     b >>= 1;
  40   }
  41   putDsm2SerialBit(!parity);     // Even Parity bit
  42
  43   putDsm2SerialBit(1);           // Stop bit
  44   putDsm2SerialBit(1);           // Stop bit
  45 }
  46 #else
  47 static void _send_level(uint8_t v)
  48 {
  49   /* Copied over from DSM, this looks doubious and in my logic analyzer
  50      output the low->high is about 2 ns late */
  51   if (modulePulsesData[EXTERNAL_MODULE].dsm2.index & 1)
  52     v += 2;
  53   else
  54     v -= 2;
  55
  56   *modulePulsesData[EXTERNAL_MODULE].dsm2.ptr++ = v - 1;
  57   modulePulsesData[EXTERNAL_MODULE].dsm2.index+=1;
  58   modulePulsesData[EXTERNAL_MODULE].dsm2.rest -=v;
  59 }
  60
  61 void sendByteSbus(uint8_t b) //max 11 changes 0 10 10 10 10 P 1
  62 {
  63   bool    lev = 0;
  64   uint8_t parity = 1;
  65
  66   uint8_t len = BITLEN_SBUS; //max val: 10*20 < 256
  67   for (uint8_t i=0; i<=9; i++) { //8Bits + 1Parity + Stop=1
  68     bool nlev = b & 1; //lsb first
  69     parity = parity ^ (uint8_t)nlev;
  70     if (lev == nlev) {
  71       len += BITLEN_SBUS;
  72     }
  73     else {
  74       _send_level(len);
  75       len  = BITLEN_SBUS;
  76       lev  = nlev;
  77     }
  78     b = (b>>1) | 0x80; //shift in ones for stop bit and parity
  79     if (i==7)
  80       b = b ^ parity; // lowest bit is one from previous line
  81   }
  82   _send_level(len+ BITLEN_SBUS); // enlarge the last bit to be two stop bits long
  83 }
  84 #endif
  85
  86
  87 #define SBUS_NORMAL_CHANS           16
  88 #define SBUS_CHAN_BITS       11
  89
  90
  91 /* Definitions from CleanFlight/BetaFlight */
  92
  93 #define SBUS_FLAG_CHANNEL_17        (1 << 0)
  94 #define SBUS_FLAG_CHANNEL_18        (1 << 1)
  95 #define SBUS_FLAG_SIGNAL_LOSS       (1 << 2)
  96 #define SBUS_FLAG_FAILSAFE_ACTIVE   (1 << 3)
  97 #define SBUS_FRAME_BEGIN_BYTE       0x0F
  98
  99 #define SBUS_CHAN_CENTER            992
 100
 101 inline int getChannelValue(uint8_t port, int channel)
 102 {
 103   int ch = g_model.moduleData[port].channelsStart+channel;
 104   // We will ignore 17 and 18th if that brings us over the limit
 105   if (ch > 31)
 106     return 0;
 107   return channelOutputs[ch] + 2 * PPM_CH_CENTER(ch) - 2*PPM_CENTER;
 108 }
 109
 110 void setupPulsesSbus(uint8_t port)
 111 {
 112 #if defined(PPM_PIN_SERIAL)
 113   modulePulsesData[EXTERNAL_MODULE].dsm2.serialByte = 0;
 114   modulePulsesData[EXTERNAL_MODULE].dsm2.serialBitCount = 0;
 115 #else
 116   modulePulsesData[EXTERNAL_MODULE].dsm2.rest = SBUS_PERIOD_HALF_US;
 117   modulePulsesData[EXTERNAL_MODULE].dsm2.index = 0;
 118 #endif
 119
 120   modulePulsesData[EXTERNAL_MODULE].dsm2.ptr = modulePulsesData[EXTERNAL_MODULE].dsm2.pulses;
 121
 122   // Sync Byte
 123   sendByteSbus(SBUS_FRAME_BEGIN_BYTE);
 124
 125   uint32_t bits = 0;
 126   uint8_t bitsavailable = 0;
 127
 128   // byte 1-22, channels 0..2047, limits not really clear (B
 129   for (int i=0; i<SBUS_NORMAL_CHANS; i++) {
 130     int value = getChannelValue(port, i);
 131
 132     value =  value*8/10 + SBUS_CHAN_CENTER;
 133     bits |= limit(0, value, 2047) << bitsavailable;
 134     bitsavailable += SBUS_CHAN_BITS;
 135     while (bitsavailable >= 8) {
 136       sendByteSbus((uint8_t) (bits & 0xff));
 137       bits >>= 8;
 138       bitsavailable -= 8;
 139     }
 140   }
 141
 142   // flags
 143   uint8_t flags=0;
 144   if (getChannelValue(port, 16) > 0)
 145     flags |=SBUS_FLAG_CHANNEL_17;
 146   if (getChannelValue(port, 17) > 0)
 147     flags |=SBUS_FLAG_CHANNEL_18;
 148
 149   sendByteSbus(flags);
 150
 151   // last byte, always 0x0
 152   sendByteSbus(0x0);
 153
 154   putDsm2Flush();
 155
 156 }