radio/src/pulses/pxx1.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 "pulses/pxx1.h"
  23
  24 template <class PxxTransport>
  25 void Pxx1Pulses<PxxTransport>::addFlag1(uint8_t module, uint8_t sendFailsafe)
  26 {
  27   uint8_t flag1 = (g_model.moduleData[module].subType << 6);
  28
  29   if (moduleState[module].mode == MODULE_MODE_BIND) {
  30     flag1 |= (g_eeGeneral.countryCode << 1) | PXX_SEND_BIND;
  31   }
  32   else if (moduleState[module].mode == MODULE_MODE_RANGECHECK) {
  33     flag1 |= PXX_SEND_RANGECHECK;
  34   }
  35
  36   if (sendFailsafe && g_model.moduleData[module].failsafeMode != FAILSAFE_NOT_SET && g_model.moduleData[module].failsafeMode != FAILSAFE_RECEIVER) {
  37     flag1 |= PXX_SEND_FAILSAFE;
  38   }
  39
  40   PxxTransport::addByte(flag1);
  41 }
  42
  43 template <class PxxTransport>
  44 void Pxx1Pulses<PxxTransport>::addExtraFlags(uint8_t module)
  45 {
  46   // Ext. flag (holds antenna selection on Horus internal module, 0x00 otherwise)
  47   uint8_t extraFlags = 0;
  48
  49 #if defined(EXTERNAL_ANTENNA)
  50   if (module == INTERNAL_MODULE && isExternalAntennaEnabled()) {
  51     extraFlags |= (1 << 0);
  52   }
  53 #endif
  54
  55   extraFlags |= (g_model.moduleData[module].pxx.receiverTelemetryOff << 1);
  56   extraFlags |= (g_model.moduleData[module].pxx.receiverHigherChannels << 2);
  57   if (isModuleR9MNonAccess(module)) {
  58     extraFlags |= (min<uint8_t>(g_model.moduleData[module].pxx.power, isModuleR9M_FCC_VARIANT(module) ? (uint8_t)R9M_FCC_POWER_MAX : (uint8_t)R9M_LBT_POWER_MAX) << 3);
  59     if (isModuleR9M_EUPLUS(module))
  60       extraFlags |= (1 << 6);
  61   }
  62
  63   // Disable S.PORT if internal module is active
  64   if (module == EXTERNAL_MODULE && isSportLineUsedByInternalModule()) {
  65     extraFlags |= (1 << 5);
  66   }
  67
  68   PxxTransport::addByte(extraFlags);
  69 }
  70
  71 template <class PxxTransport>
  72 void Pxx1Pulses<PxxTransport>::addChannels(uint8_t port, uint8_t sendFailsafe, uint8_t sendUpperChannels)
  73 {
  74   uint16_t pulseValue = 0;
  75   uint16_t pulseValueLow = 0;
  76
  77   for (int8_t i=0; i<8; i++) {
  78     if (sendFailsafe) {
  79       if (g_model.moduleData[port].failsafeMode == FAILSAFE_HOLD) {
  80         pulseValue = (i < sendUpperChannels ? 4095 : 2047);
  81       }
  82       else if (g_model.moduleData[port].failsafeMode == FAILSAFE_NOPULSES) {
  83         pulseValue = (i < sendUpperChannels ? 2048 : 0);
  84       }
  85       else {
  86         if (i < sendUpperChannels) {
  87           int16_t failsafeValue = g_model.failsafeChannels[8+i];
  88           if (failsafeValue == FAILSAFE_CHANNEL_HOLD) {
  89             pulseValue = 4095;
  90           }
  91           else if (failsafeValue == FAILSAFE_CHANNEL_NOPULSE) {
  92             pulseValue = 2048;
  93           }
  94           else {
  95             failsafeValue += 2*PPM_CH_CENTER(8+g_model.moduleData[port].channelsStart+i) - 2*PPM_CENTER;
  96             pulseValue = limit(2049, (failsafeValue * 512 / 682) + 3072, 4094);
  97           }
  98         }
  99         else {
 100           int16_t failsafeValue = g_model.failsafeChannels[i];
 101           if (failsafeValue == FAILSAFE_CHANNEL_HOLD) {
 102             pulseValue = 2047;
 103           }
 104           else if (failsafeValue == FAILSAFE_CHANNEL_NOPULSE) {
 105             pulseValue = 0;
 106           }
 107           else {
 108             failsafeValue += 2*PPM_CH_CENTER(g_model.moduleData[port].channelsStart+i) - 2*PPM_CENTER;
 109             pulseValue = limit(1, (failsafeValue * 512 / 682) + 1024, 2046);
 110           }
 111         }
 112       }
 113     }
 114     else {
 115       if (i < sendUpperChannels) {
 116         int channel = 8 + g_model.moduleData[port].channelsStart + i;
 117         int value = channelOutputs[channel] + 2*PPM_CH_CENTER(channel) - 2*PPM_CENTER;
 118         pulseValue = limit(2049, (value * 512 / 682) + 3072, 4094);
 119       }
 120       else if (i < sentModuleChannels(port)) {
 121         int channel = g_model.moduleData[port].channelsStart + i;
 122         int value = channelOutputs[channel] + 2*PPM_CH_CENTER(channel) - 2*PPM_CENTER;
 123         pulseValue = limit(1, (value * 512 / 682) + 1024, 2046);
 124       }
 125       else {
 126         pulseValue = 1024;
 127       }
 128     }
 129
 130     if (i & 1) {
 131       PxxTransport::addByte(pulseValueLow); // Low byte of channel
 132       PxxTransport::addByte(((pulseValueLow >> 8) & 0x0F) | (pulseValue << 4));  // 4 bits each from 2 channels
 133       PxxTransport::addByte(pulseValue >> 4);  // High byte of channel
 134     }
 135     else {
 136       pulseValueLow = pulseValue;
 137     }
 138   }
 139 }
 140
 141 template <class PxxTransport>
 142 void Pxx1Pulses<PxxTransport>::add8ChannelsFrame(uint8_t module, uint8_t sendUpperChannels, uint8_t sendFailsafe)
 143 {
 144   PxxTransport::initCrc();
 145
 146   // Sync
 147   addHead();
 148
 149   // RX Number
 150   PxxTransport::addByte(g_model.header.modelId[module]);
 151
 152   // Flag1
 153   addFlag1(module, sendFailsafe);
 154
 155   // Flag2
 156   PxxTransport::addByte(0);
 157
 158   // Channels
 159   addChannels(module, sendFailsafe, sendUpperChannels);
 160
 161   // Extra flags
 162   addExtraFlags(module);
 163
 164   // CRC
 165   addCrc();
 166
 167   // Sync = HEAD
 168   addHead();
 169
 170   // Tail
 171   PxxTransport::addTail();
 172 }
 173
 174 template <class PxxTransport>
 175 void Pxx1Pulses<PxxTransport>::setupFrame(uint8_t module)
 176 {
 177   uint8_t sendUpperChannels = 0;
 178   uint8_t sendFailsafe = 0;
 179
 180   PxxTransport::initFrame(PXX_PULSES_PERIOD);
 181
 182 #if defined(PXX_FREQUENCY_HIGH)
 183   if (moduleState[module].protocol == PROTOCOL_CHANNELS_PXX1_SERIAL) {
 184     if (moduleState[module].counter-- == 0) {
 185       sendFailsafe = 1;
 186       moduleState[module].counter = 1000;
 187     }
 188     add8ChannelsFrame(module, 0, sendFailsafe);
 189     if (sentModuleChannels(module) > 8) {
 190       add8ChannelsFrame(module, 8, sendFailsafe);
 191     }
 192     return;
 193   }
 194 #endif
 195
 196   if (moduleState[module].counter & 0x01) {
 197     sendUpperChannels = g_model.moduleData[module].channelsCount;
 198     if (sendUpperChannels && moduleState[module].counter == 1) {
 199       sendFailsafe = 1;
 200     }
 201   }
 202   else {
 203     if (moduleState[module].counter == 0) {
 204       sendFailsafe = 1;
 205     }
 206   }
 207
 208   add8ChannelsFrame(module, sendUpperChannels, sendFailsafe);
 209
 210   if (moduleState[module].counter-- == 0) {
 211     moduleState[module].counter = 1000;
 212   }
 213 }
 214
 215 template class Pxx1Pulses<StandardPxx1Transport<PwmPxxBitTransport> >;
 216 template class Pxx1Pulses<StandardPxx1Transport<SerialPxxBitTransport> >;
 217 template class Pxx1Pulses<UartPxx1Transport>;