radio/src/pulses/multi.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 "multi.h"
  23
  24 // for the  MULTI protocol definition
  25 // see https://github.com/pascallanger/DIY-Multiprotocol-TX-Module
  26 // file Multiprotocol/multiprotocol.h
  27
  28 #define MULTI_SEND_BIND                     (1 << 7)
  29 #define MULTI_SEND_RANGECHECK               (1 << 5)
  30 #define MULTI_SEND_AUTOBIND                 (1 << 6)
  31
  32 #define MULTI_CHANS                         16
  33 #define MULTI_CHAN_BITS                     11
  34
  35 #define MULTI_NORMAL   0x00
  36 #define MULTI_FAILSAFE 0x01
  37 #define MULTI_DATA     0x02
  38
  39 static void sendFrameProtocolHeader(uint8_t moduleIdx, bool failsafe);
  40 void sendChannels(uint8_t moduleIdx);
  41 #if defined(LUA)
  42 static void sendSport(uint8_t moduleIdx);
  43 static void sendHott(uint8_t moduleIdx);
  44 #endif
  45
  46 void multiPatchCustom(uint8_t moduleIdx)
  47 {
  48   if (g_model.moduleData[moduleIdx].multi.customProto) {
  49     uint8_t type = g_model.moduleData[moduleIdx].getMultiProtocol() - 1;  // custom where starting at 1, otx list at 0
  50     int subtype = g_model.moduleData[moduleIdx].subType;
  51
  52     g_model.moduleData[moduleIdx].multi.customProto = 0;
  53
  54     if (type == 2) {  // multi PROTO_FRSKYD
  55       g_model.moduleData[moduleIdx].subType = 1;    // D8
  56       return;
  57     }
  58     else if (type == 14) { // multi PROTO_FRSKYX
  59       g_model.moduleData[moduleIdx].setMultiProtocol(2);
  60       switch (subtype) {
  61         case 0:       //D16-16
  62           g_model.moduleData[moduleIdx].subType = 0;
  63           break;
  64         case 1:       //D16-8
  65           g_model.moduleData[moduleIdx].subType = 2;
  66           break;
  67         case 2:       //EU-16
  68           g_model.moduleData[moduleIdx].subType = 4;
  69           break;
  70         case 3:       //EU-8
  71           g_model.moduleData[moduleIdx].subType = 5;
  72           break;
  73       }
  74       return;
  75     }
  76     else if (type == 24) {  // multi PROTO_FRSKYV
  77       g_model.moduleData[moduleIdx].setMultiProtocol(2);
  78       g_model.moduleData[moduleIdx].subType = 3;
  79       return;
  80     }
  81     if (type > 14)
  82       type -= 1;
  83     if (type > 24)
  84       type -= 1;
  85     g_model.moduleData[moduleIdx].setMultiProtocol(type);
  86   }
  87 }
  88
  89 static void sendMulti(uint8_t moduleIdx, uint8_t b)
  90 {
  91 #if defined(HARDWARE_INTERNAL_MODULE)
  92   if (moduleIdx == INTERNAL_MODULE) {
  93     intmodulePulsesData.multi.sendByte(b);
  94   }
  95   else
  96 #endif
  97     sendByteSbus(b);
  98 }
  99
 100 static void sendFailsafeChannels(uint8_t moduleIdx)
 101 {
 102   uint32_t bits = 0;
 103   uint8_t bitsavailable = 0;
 104
 105   for (int i = 0; i < MULTI_CHANS; i++) {
 106     int16_t failsafeValue = g_model.failsafeChannels[i];
 107     int pulseValue;
 108
 109     if (g_model.moduleData[moduleIdx].failsafeMode == FAILSAFE_HOLD) {
 110       pulseValue = 2047;
 111     }
 112     else if (g_model.moduleData[moduleIdx].failsafeMode == FAILSAFE_NOPULSES) {
 113       pulseValue = 0;
 114     }
 115     else {
 116       failsafeValue += 2 * PPM_CH_CENTER(g_model.moduleData[moduleIdx].channelsStart + i) - 2 * PPM_CENTER;
 117       pulseValue = limit(1, (failsafeValue * 800 / 1000) + 1024, 2047);
 118     }
 119
 120     bits |= pulseValue << bitsavailable;
 121     bitsavailable += MULTI_CHAN_BITS;
 122     while (bitsavailable >= 8) {
 123       sendMulti(moduleIdx, (uint8_t) (bits & 0xff));
 124       bits >>= 8;
 125       bitsavailable -= 8;
 126     }
 127   }
 128 }
 129
 130 void setupPulsesMulti(uint8_t moduleIdx)
 131 {
 132   static int counter[2] = {0,0}; //TODO
 133   static uint8_t invert[2] = {0x00,        //internal
 134 #if defined(PCBTARANIS) || defined(PCBHORUS)
 135     0x08        //external
 136 #else
 137     0x00        //external
 138 #endif
 139   };
 140   uint8_t type=MULTI_NORMAL;
 141
 142   // Failsafe packets
 143   if (counter[moduleIdx] % 1000 == 0 && g_model.moduleData[moduleIdx].failsafeMode != FAILSAFE_NOT_SET && g_model.moduleData[moduleIdx].failsafeMode != FAILSAFE_RECEIVER) {
 144     type|=MULTI_FAILSAFE;
 145   }
 146
 147   // Invert telemetry if needed
 148   if (invert[moduleIdx] & 0x80 && !g_model.moduleData[moduleIdx].multi.disableTelemetry) {
 149     if (getMultiModuleStatus(moduleIdx).isValid()) {
 150       invert[moduleIdx] &= 0x08;    // Telemetry received, stop searching
 151     }
 152     else if (counter[moduleIdx] % 100 == 0) {
 153       invert[moduleIdx] ^= 0x08;  // Try inverting telemetry
 154     }
 155   }
 156
 157   counter[moduleIdx]++;
 158
 159   // Send header
 160   sendFrameProtocolHeader(moduleIdx, type&MULTI_FAILSAFE);
 161
 162   // Send channels
 163   if (type & MULTI_FAILSAFE)
 164     sendFailsafeChannels(moduleIdx);
 165   else
 166     sendChannels(moduleIdx);
 167
 168   // Multi V1.3.X.X -> Send byte 26, Protocol (bits 7 & 6), RX_Num (bits 5 & 4), invert, not used, disable telemetry, disable mapping
 169   sendMulti(moduleIdx, (uint8_t) (((g_model.moduleData[moduleIdx].getMultiProtocol()+3)&0xC0)
 170                                   | (g_model.header.modelId[moduleIdx] & 0x30)
 171                                   | (invert[moduleIdx] & 0x08)
 172                                   //| 0x04 // Future use
 173                                   | (g_model.moduleData[moduleIdx].multi.disableTelemetry << 1)
 174                                   | g_model.moduleData[moduleIdx].multi.disableMapping));
 175
 176   // Multi V1.3.X.X -> Send protocol additional data: max 9 bytes
 177 #if defined(LUA)
 178   if (getMultiModuleStatus(moduleIdx).isValid()) {
 179     MultiModuleStatus &status = getMultiModuleStatus(moduleIdx);
 180     if (status.minor >= 3 && !(status.flags & 0x80)) { //Version 1.3.x.x or more and Buffer not full
 181       // SPort send
 182       if (IS_D16_MULTI(moduleIdx) && outputTelemetryBuffer.destination == TELEMETRY_ENDPOINT_SPORT && outputTelemetryBuffer.size) {
 183         sendSport(moduleIdx);        //8 bytes of additional data
 184       }
 185       else if (IS_HOTT_MULTI(moduleIdx)) {
 186         sendHott(moduleIdx);        //1 byte of additional data
 187       }
 188     }
 189   }
 190   #endif
 191 }
 192
 193 void setupPulsesMultiExternalModule()
 194 {
 195 #if defined(PPM_PIN_SERIAL)
 196   extmodulePulsesData.dsm2.serialByte = 0 ;
 197   extmodulePulsesData.dsm2.serialBitCount = 0 ;
 198 #else
 199   extmodulePulsesData.dsm2.rest = getMultiSyncStatus(EXTERNAL_MODULE).getAdjustedRefreshRate();
 200   extmodulePulsesData.dsm2.index = 0;
 201 #endif
 202
 203   extmodulePulsesData.dsm2.ptr = extmodulePulsesData.dsm2.pulses;
 204
 205   setupPulsesMulti(EXTERNAL_MODULE);
 206   putDsm2Flush();
 207 }
 208
 209 #if defined(INTERNAL_MODULE_MULTI)
 210 void setupPulsesMultiInternalModule()
 211 {
 212   intmodulePulsesData.multi.initFrame();
 213   setupPulsesMulti(INTERNAL_MODULE);
 214 }
 215 #endif
 216
 217 void sendChannels(uint8_t moduleIdx)
 218 {
 219   uint32_t bits = 0;
 220   uint8_t bitsavailable = 0;
 221
 222   // byte 4-25, channels 0..2047
 223   // Range for pulses (channelsOutputs) is [-1024:+1024] for [-100%;100%]
 224   // Multi uses [204;1843] as [-100%;100%]
 225   for (int i = 0; i < MULTI_CHANS; i++) {
 226     int channel = g_model.moduleData[moduleIdx].channelsStart + i;
 227     int value = channelOutputs[channel] + 2 * PPM_CH_CENTER(channel) - 2 * PPM_CENTER;
 228
 229     // Scale to 80%
 230     value = value * 800 / 1000 + 1024;
 231     value = limit(0, value, 2047);
 232
 233     bits |= value << bitsavailable;
 234     bitsavailable += MULTI_CHAN_BITS;
 235     while (bitsavailable >= 8) {
 236       sendMulti(moduleIdx, (uint8_t) (bits & 0xff));
 237       bits >>= 8;
 238       bitsavailable -= 8;
 239     }
 240   }
 241 }
 242
 243 void sendFrameProtocolHeader(uint8_t moduleIdx, bool failsafe)
 244 {// byte 1+2, protocol information
 245
 246   // Our enumeration starts at 0
 247   int type = g_model.moduleData[moduleIdx].getMultiProtocol() + 1;
 248   int subtype = g_model.moduleData[moduleIdx].subType;
 249   int8_t optionValue = g_model.moduleData[moduleIdx].multi.optionValue;
 250
 251   uint8_t protoByte = 0;
 252
 253   if (moduleState[moduleIdx].mode == MODULE_MODE_SPECTRUM_ANALYSER) {
 254     sendMulti(moduleIdx, (uint8_t) 0x54);  // Header byte
 255     sendMulti(moduleIdx, (uint8_t) 54);    // Spectrum custom protocol
 256     sendMulti(moduleIdx, (uint8_t) 0);
 257     sendMulti(moduleIdx, (uint8_t) 0);
 258     return;
 259   }
 260
 261   if (moduleState[moduleIdx].mode == MODULE_MODE_BIND)
 262     protoByte |= MULTI_SEND_BIND;
 263   else if (moduleState[moduleIdx].mode ==  MODULE_MODE_RANGECHECK)
 264     protoByte |= MULTI_SEND_RANGECHECK;
 265
 266   // rfProtocol
 267   if (g_model.moduleData[moduleIdx].getMultiProtocol() == MODULE_SUBTYPE_MULTI_DSM2) {
 268
 269     // Autobinding should always be done in DSMX 11ms
 270     if (g_model.moduleData[moduleIdx].multi.autoBindMode && moduleState[moduleIdx].mode == MODULE_MODE_BIND)
 271       subtype = MM_RF_DSM2_SUBTYPE_AUTO;
 272
 273     // Multi module in DSM mode wants the number of channels to be used as option value
 274     if (optionValue)
 275       optionValue = 0x80 | sentModuleChannels(moduleIdx); // Max throw
 276     else
 277       optionValue = sentModuleChannels(moduleIdx);
 278 }
 279
 280   // 15  for Multimodule is FrskyX or D16 which we map as a subprotocol of 3 (FrSky)
 281   // all protos > frskyx are therefore also off by one
 282   if (type >= 15)
 283     type = type + 1;
 284
 285   // 25 is again a FrSky protocol (FrskyV) so shift again
 286   if (type >= 25)
 287     type = type + 1;
 288
 289   if (g_model.moduleData[moduleIdx].getMultiProtocol() == MODULE_SUBTYPE_MULTI_FRSKY) {
 290     if (subtype == MM_RF_FRSKY_SUBTYPE_D8) {
 291       //D8
 292       type = 3;
 293       subtype = 0;
 294     } else if (subtype == MM_RF_FRSKY_SUBTYPE_V8) {
 295       //V8
 296       type = 25;
 297       subtype = 0;
 298     } else {
 299       type = 15;
 300       if (subtype == MM_RF_FRSKY_SUBTYPE_D16_8CH) // D16 8ch
 301         subtype = 1;
 302       else if (subtype == MM_RF_FRSKY_SUBTYPE_D16)
 303         subtype = 0;  // D16
 304       else if (subtype == MM_RF_FRSKY_SUBTYPE_D16_LBT)
 305         subtype = 2;
 306       else
 307         subtype = 3; // MM_RF_FRSKY_SUBTYPE_D16_LBT_8CH
 308     }
 309   }
 310
 311   // Set the highest bit of option byte in AFHDS2A protocol to instruct MULTI to passthrough telemetry bytes instead
 312   // of sending Frsky D telemetry
 313   if (g_model.moduleData[moduleIdx].getMultiProtocol() == MODULE_SUBTYPE_MULTI_FS_AFHDS2A)
 314     optionValue = optionValue | 0x80;
 315
 316   // For custom protocol send unmodified type byte
 317   if (g_model.moduleData[moduleIdx].getMultiProtocol() == MM_RF_CUSTOM_SELECTED)
 318     type = g_model.moduleData[moduleIdx].getMultiProtocol();
 319
 320
 321   uint8_t headerByte = 0x55;
 322   // header, byte 0,  0x55 for proto 0-31, 0x54 for proto 32-63
 323   if (type & 0x20)
 324     headerByte &= 0xFE;
 325
 326   if (failsafe)
 327     headerByte |= 0x02;
 328
 329   sendMulti(moduleIdx, headerByte);
 330
 331   // protocol byte
 332   protoByte |= (type & 0x1f);
 333   if (g_model.moduleData[moduleIdx].getMultiProtocol() != MODULE_SUBTYPE_MULTI_DSM2)
 334     protoByte |= (g_model.moduleData[moduleIdx].multi.autoBindMode << 6);
 335
 336   sendMulti(moduleIdx, protoByte);
 337
 338   // byte 2, subtype, powermode, model id
 339   sendMulti(moduleIdx, (uint8_t) ((g_model.header.modelId[moduleIdx] & 0x0f)
 340                            | ((subtype & 0x7) << 4)
 341                            | (g_model.moduleData[moduleIdx].multi.lowPowerMode << 7))
 342   );
 343
 344   // byte 3
 345   sendMulti(moduleIdx, (uint8_t) optionValue);
 346 }
 347
 348 #if defined(LUA)
 349 void sendSport(uint8_t moduleIdx)
 350 {
 351   // example: B7 30 30 0C 80 00 00 00 13
 352   uint8_t j=0;
 353
 354   // unstuff and remove crc
 355   for (uint8_t i = 0; i < outputTelemetryBuffer.size - 1 && j < 8; i++, j++) {
 356     if (outputTelemetryBuffer.data[i] == BYTE_STUFF) {
 357       i++;
 358       sendMulti(moduleIdx, outputTelemetryBuffer.data[i] ^ STUFF_MASK);
 359     }
 360     else {
 361       sendMulti(moduleIdx, outputTelemetryBuffer.data[i]);
 362     }
 363   }
 364
 365   outputTelemetryBuffer.reset(); // empty buffer
 366 }
 367
 368 void sendHott(uint8_t moduleIdx)
 369 {
 370   if (Multi_Buffer && memcmp(Multi_Buffer, "HoTT", 4) == 0 && Multi_Buffer[5] >= 0xD7 && Multi_Buffer[5] <= 0xDF) {
 371     // HoTT Lua script is running
 372     sendMulti(moduleIdx, Multi_Buffer[5]);
 373   }
 374 }
 375 #endif