radio/src/pulses/dsm2.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 #define DSM2_SEND_BIND                     (1 << 7)
  24 #define DSM2_SEND_RANGECHECK               (1 << 5)
  25
  26 #if defined(PCBSKY9X)
  27 uint8_t  dsm2BindTimer = DSM2_BIND_TIMEOUT;
  28 #endif
  29
  30 // DSM2 control bits
  31 #define DSM2_CHANS           6
  32 #define FRANCE_BIT           0x10
  33 #define DSMX_BIT             0x08
  34 #define BAD_DATA             0x47
  35
  36 #define BITLEN_DSM2          (8*2) //125000 Baud => 8uS per bit
  37
  38 #if defined(PPM_PIN_SERIAL)
  39 void putDsm2SerialBit(uint8_t bit)
  40 {
  41   extmodulePulsesData.dsm2.serialByte >>= 1;
  42   if (bit & 1) {
  43     extmodulePulsesData.dsm2.serialByte |= 0x80;
  44   }
  45   if (++extmodulePulsesData.dsm2.serialBitCount >= 8) {
  46     *extmodulePulsesData.dsm2.ptr++ = extmodulePulsesData.dsm2.serialByte;
  47     extmodulePulsesData.dsm2.serialBitCount = 0;
  48   }
  49 }
  50
  51 void sendByteDsm2(uint8_t b)     // max 10changes 0 10 10 10 10 1
  52 {
  53   putDsm2SerialBit(0);           // Start bit
  54   for (uint8_t i=0; i<8; i++) {  // 8 data Bits
  55     putDsm2SerialBit(b & 1);
  56     b >>= 1;
  57   }
  58
  59   putDsm2SerialBit(1);           // Stop bit
  60   putDsm2SerialBit(1);           // Stop bit
  61 }
  62
  63 void putDsm2Flush()
  64 {
  65   for (int i=0; i<16; i++) {
  66     putDsm2SerialBit(1);         // 16 extra stop bits
  67   }
  68 }
  69 #else
  70 void _send_1(uint8_t v)
  71 {
  72   if (extmodulePulsesData.dsm2.index & 1)
  73     v += 2;
  74   else
  75     v -= 2;
  76
  77   *extmodulePulsesData.dsm2.ptr++ = v - 1;
  78   extmodulePulsesData.dsm2.index += 1;
  79   extmodulePulsesData.dsm2.rest -= v;
  80 }
  81
  82 void sendByteDsm2(uint8_t b) // max 10 changes 0 10 10 10 10 1
  83 {
  84   bool    lev = 0;
  85   uint8_t len = BITLEN_DSM2; // max val: 9*16 < 256
  86   for (uint8_t i=0; i<=8; i++) { // 8Bits + Stop=1
  87     bool nlev = b & 1; // lsb first
  88     if (lev == nlev) {
  89       len += BITLEN_DSM2;
  90     }
  91     else {
  92       _send_1(len);
  93       len  = BITLEN_DSM2;
  94       lev  = nlev;
  95     }
  96     b = (b>>1) | 0x80; // shift in stop bit
  97   }
  98   _send_1(len); // stop bit (len is already BITLEN_DSM2)
  99 }
 100
 101 void putDsm2Flush()
 102 {
 103   if (extmodulePulsesData.dsm2.index & 1)
 104     *extmodulePulsesData.dsm2.ptr++ = extmodulePulsesData.dsm2.rest;
 105   else
 106     *(extmodulePulsesData.dsm2.ptr - 1) = extmodulePulsesData.dsm2.rest;
 107 }
 108 #endif
 109
 110 // This is the data stream to send, prepare after 19.5 mS
 111 // Send after 22.5 mS
 112
 113 void setupPulsesDSM2()
 114 {
 115   uint8_t dsmDat[14];
 116
 117 #if defined(PPM_PIN_SERIAL)
 118   extmodulePulsesData.dsm2.serialByte = 0 ;
 119   extmodulePulsesData.dsm2.serialBitCount = 0 ;
 120 #else
 121   extmodulePulsesData.dsm2.index = 0;
 122   extmodulePulsesData.dsm2.rest = DSM2_PERIOD * 2000;
 123 #endif
 124
 125   extmodulePulsesData.dsm2.ptr = extmodulePulsesData.dsm2.pulses;
 126
 127   switch (moduleState[EXTERNAL_MODULE].protocol) {
 128     case PROTOCOL_CHANNELS_DSM2_LP45:
 129       dsmDat[0] = 0x00;
 130       break;
 131     case PROTOCOL_CHANNELS_DSM2_DSM2:
 132       dsmDat[0] = 0x10;
 133       break;
 134     default: // DSMX
 135       dsmDat[0] = 0x10 | DSMX_BIT;
 136       break;
 137   }
 138
 139 #if defined(PCBSKY9X)
 140   if (dsm2BindTimer > 0) {
 141     dsm2BindTimer--;
 142     if (switchState(SW_DSM2_BIND)) {
 143       moduleState[EXTERNAL_MODULE].mode = MODULE_MODE_BIND;
 144       dsmDat[0] |= DSM2_SEND_BIND;
 145     }
 146   }
 147   else if (moduleState[EXTERNAL_MODULE].mode == MODULE_MODE_RANGECHECK) {
 148     dsmDat[0] |= DSM2_SEND_RANGECHECK;
 149   }
 150   else {
 151     moduleState[EXTERNAL_MODULE].mode = 0;
 152   }
 153 #else
 154   if (moduleState[EXTERNAL_MODULE].mode == MODULE_MODE_BIND) {
 155     dsmDat[0] |= DSM2_SEND_BIND;
 156   }
 157   else if (moduleState[EXTERNAL_MODULE].mode == MODULE_MODE_RANGECHECK) {
 158     dsmDat[0] |= DSM2_SEND_RANGECHECK;
 159   }
 160 #endif
 161
 162   dsmDat[1] = g_model.header.modelId[EXTERNAL_MODULE]; // DSM2 Header second byte for model match
 163
 164   for (int i=0; i<DSM2_CHANS; i++) {
 165     int channel = g_model.moduleData[EXTERNAL_MODULE].channelsStart+i;
 166     int value = channelOutputs[channel] + 2*PPM_CH_CENTER(channel) - 2*PPM_CENTER;
 167     uint16_t pulse = limit(0, ((value*13)>>5)+512, 1023);
 168     dsmDat[2+2*i] = (i<<2) | ((pulse>>8)&0x03);
 169     dsmDat[3+2*i] = pulse & 0xff;
 170   }
 171
 172   for (int i=0; i<14; i++) {
 173     sendByteDsm2(dsmDat[i]);
 174   }
 175
 176   putDsm2Flush();
 177 }