radio/src/pulses/multi_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 // for the  MULTI protocol definition
  24 // see https://github.com/pascallanger/DIY-Multiprotocol-TX-Module
  25 // file Multiprotocol/multiprotocol.h
  26
  27 #define MULTI_SEND_BIND                     (1 << 7)
  28 #define MULTI_SEND_RANGECHECK               (1 << 5)
  29 #define MULTI_SEND_AUTOBIND                 (1 << 6)
  30
  31 #define MULTI_CHANS                         16
  32 #define MULTI_CHAN_BITS                     11
  33
  34 static void sendFrameProtocolHeader(uint8_t port, bool failsafe);
  35
  36 void sendChannels(uint8_t port);
  37
  38 static void sendSetupFrame()
  39 {
  40   // Old multi firmware will mark config messsages as invalid frame and throw them away
  41   sendByteSbus('M');
  42   sendByteSbus('P');
  43   sendByteSbus(0x80);           // Module Configuration
  44   sendByteSbus(1);              // 1 byte data
  45   uint8_t config = 0x01 | 0x02; // inversion + multi_telemetry
  46 #if !defined(PPM_PIN_SERIAL)
  47   // TODO why PPM_PIN_SERIAL would change MULTI protocol?
  48   config |= 0x04;               // input synchronsisation
  49 #endif
  50
  51   sendByteSbus(config);
  52 }
  53
  54 static void sendFailsafeChannels(uint8_t port)
  55 {
  56   uint32_t bits = 0;
  57   uint8_t bitsavailable = 0;
  58
  59   for (int i = 0; i < MULTI_CHANS; i++) {
  60     int16_t failsafeValue = g_model.moduleData[port].failsafeChannels[i];
  61     int pulseValue;
  62     if (g_model.moduleData[port].failsafeMode == FAILSAFE_HOLD)
  63       failsafeValue = FAILSAFE_CHANNEL_HOLD;
  64
  65     if (g_model.moduleData[port].failsafeMode == FAILSAFE_NOPULSES)
  66       failsafeValue = FAILSAFE_CHANNEL_NOPULSE;
  67
  68     if (failsafeValue == FAILSAFE_CHANNEL_HOLD) {
  69       pulseValue = 0;
  70     }
  71     else if (failsafeValue == FAILSAFE_CHANNEL_NOPULSE) {
  72       pulseValue = 2047;
  73     }
  74     else {
  75       failsafeValue += 2 * PPM_CH_CENTER(g_model.moduleData[port].channelsStart + i) - 2 * PPM_CENTER;
  76       pulseValue = limit(1, (failsafeValue * 800 / 1000) + 1024, 2047);
  77     }
  78
  79     bits |= pulseValue << bitsavailable;
  80     bitsavailable += MULTI_CHAN_BITS;
  81     while (bitsavailable >= 8) {
  82       sendByteSbus((uint8_t) (bits & 0xff));
  83       bits >>= 8;
  84       bitsavailable -= 8;
  85     }
  86   }
  87 }
  88
  89 void setupPulsesMultimodule(uint8_t port)
  90 {
  91   static int counter = 0;
  92
  93 #if defined(PPM_PIN_SERIAL)
  94   modulePulsesData[EXTERNAL_MODULE].dsm2.serialByte = 0 ;
  95   modulePulsesData[EXTERNAL_MODULE].dsm2.serialBitCount = 0 ;
  96 #else
  97   modulePulsesData[EXTERNAL_MODULE].dsm2.rest = multiSyncStatus.getAdjustedRefreshRate();
  98   modulePulsesData[EXTERNAL_MODULE].dsm2.index = 0;
  99 #endif
 100
 101   modulePulsesData[EXTERNAL_MODULE].dsm2.ptr = modulePulsesData[EXTERNAL_MODULE].dsm2.pulses;
 102
 103   // Every 1000 cycles (=9s) send a config packet that configures the multimodule (inversion, telemetry type)
 104   counter++;
 105   if (counter  % 1000== 500) {
 106     sendSetupFrame();
 107   } else if (counter % 1000 == 0 && g_model.moduleData[port].failsafeMode != FAILSAFE_NOT_SET && g_model.moduleData[port].failsafeMode != FAILSAFE_RECEIVER) {
 108     sendFrameProtocolHeader(port, true);
 109     sendFailsafeChannels(port);
 110   } else {
 111     // Normal Frame
 112     sendFrameProtocolHeader(port, false);
 113     sendChannels(port);
 114   }
 115
 116   putDsm2Flush();
 117 }
 118
 119
 120 void sendChannels(uint8_t port)
 121 {
 122   uint32_t bits = 0;
 123   uint8_t bitsavailable = 0;
 124
 125   // byte 4-25, channels 0..2047
 126   // Range for pulses (channelsOutputs) is [-1024:+1024] for [-100%;100%]
 127   // Multi uses [204;1843] as [-100%;100%]
 128   for (int i = 0; i < MULTI_CHANS; i++) {
 129     int channel = g_model.moduleData[port].channelsStart + i;
 130     int value = channelOutputs[channel] + 2 * PPM_CH_CENTER(channel) - 2 * PPM_CENTER;
 131
 132     // Scale to 80%
 133     value = value * 800 / 1000 + 1024;
 134     value = limit(0, value, 2047);
 135
 136     bits |= value << bitsavailable;
 137     bitsavailable += MULTI_CHAN_BITS;
 138     while (bitsavailable >= 8) {
 139       sendByteSbus((uint8_t) (bits & 0xff));
 140       bits >>= 8;
 141       bitsavailable -= 8;
 142     }
 143   }
 144 }
 145
 146 void sendFrameProtocolHeader(uint8_t port, bool failsafe)
 147 {// byte 1+2, protocol information
 148
 149   // Our enumeration starts at 0
 150   int type = g_model.moduleData[port].getMultiProtocol(false) + 1;
 151   int subtype = g_model.moduleData[port].subType;
 152   int8_t optionValue = g_model.moduleData[port].multi.optionValue;
 153
 154   uint8_t protoByte = 0;
 155   if (moduleFlag[port] == MODULE_BIND)
 156     protoByte |= MULTI_SEND_BIND;
 157   else if (moduleFlag[port] == MODULE_RANGECHECK)
 158     protoByte |= MULTI_SEND_RANGECHECK;
 159
 160   // rfProtocol
 161   if (g_model.moduleData[port].getMultiProtocol(true) == MM_RF_PROTO_DSM2) {
 162
 163     // Autobinding should always be done in DSMX 11ms
 164     if (g_model.moduleData[port].multi.autoBindMode && moduleFlag[port] == MODULE_BIND)
 165       subtype = MM_RF_DSM2_SUBTYPE_AUTO;
 166
 167     // Multi module in DSM mode wants the number of channels to be used as option value
 168     optionValue = NUM_CHANNELS(EXTERNAL_MODULE);
 169
 170   }
 171
 172   // 15  for Multimodule is FrskyX or D16 which we map as a subprotocol of 3 (FrSky)
 173   // all protos > frskyx are therefore also off by one
 174   if (type >= 15)
 175     type = type + 1;
 176
 177   // 25 is again a FrSky protocol (FrskyV) so shift again
 178   if (type >= 25)
 179     type = type + 1;
 180
 181   if (g_model.moduleData[port].getMultiProtocol(true) == MM_RF_PROTO_FRSKY) {
 182     if (subtype == MM_RF_FRSKY_SUBTYPE_D8) {
 183       //D8
 184       type = 3;
 185       subtype = 0;
 186     } else if (subtype == MM_RF_FRSKY_SUBTYPE_V8) {
 187       //V8
 188       type = 25;
 189       subtype = 0;
 190     } else {
 191       type = 15;
 192       if (subtype == MM_RF_FRSKY_SUBTYPE_D16_8CH) // D16 8ch
 193         subtype = 1;
 194       else if (subtype == MM_RF_FRSKY_SUBTYPE_D16)
 195         subtype = 0;  // D16
 196       else if (subtype == MM_RF_FRSKY_SUBTYPE_D16_LBT)
 197         subtype = 2;
 198       else
 199         subtype = 3; // MM_RF_FRSKY_SUBTYPE_D16_LBT_8CH
 200     }
 201   }
 202
 203   // Set the highest bit of option byte in AFHDS2A protocol to instruct MULTI to passthrough telemetry bytes instead
 204   // of sending Frsky D telemetry
 205   if (g_model.moduleData[port].getMultiProtocol(false) == MM_RF_PROTO_FS_AFHDS2A)
 206     optionValue = optionValue | 0x80;
 207
 208   // For custom protocol send unmodified type byte
 209   if (g_model.moduleData[port].getMultiProtocol(true) == MM_RF_CUSTOM_SELECTED)
 210     type = g_model.moduleData[port].getMultiProtocol(false);
 211
 212
 213   uint8_t headerByte = 0x54;
 214   if (failsafe)
 215     headerByte = 0x56;
 216
 217     // header, byte 0,  0x55 for proto 0-31 0x54 for 32-63
 218   if (type <= 31)
 219     sendByteSbus(headerByte+1);
 220   else
 221     sendByteSbus(headerByte);
 222
 223
 224   // protocol byte
 225   protoByte |= (type & 0x1f);
 226   if (g_model.moduleData[port].getMultiProtocol(true) != MM_RF_PROTO_DSM2)
 227     protoByte |= (g_model.moduleData[port].multi.autoBindMode << 6);
 228
 229   sendByteSbus(protoByte);
 230
 231   // byte 2, subtype, powermode, model id
 232   sendByteSbus((uint8_t) ((g_model.header.modelId[port] & 0x0f)
 233                            | ((subtype & 0x7) << 4)
 234                            | (g_model.moduleData[port].multi.lowPowerMode << 7))
 235   );
 236
 237   // byte 3
 238   sendByteSbus((uint8_t) optionValue);
 239 }