src/main/rx/cc2500_frsky_d.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <stdlib.h>
  21 #include <string.h>
  22
  23 #include "platform.h"
  24
  25 #ifdef USE_RX_FRSKY_SPI_D
  26
  27 #include "build/build_config.h"
  28 #include "build/debug.h"
  29
  30 #include "common/maths.h"
  31 #include "common/utils.h"
  32
  33 #include "drivers/adc.h"
  34 #include "drivers/rx/rx_cc2500.h"
  35 #include "drivers/io.h"
  36 #include "drivers/system.h"
  37 #include "drivers/time.h"
  38
  39 #include "fc/config.h"
  40
  41 #include "rx/cc2500_frsky_common.h"
  42 #include "rx/cc2500_frsky_shared.h"
  43
  44 #include "sensors/battery.h"
  45
  46 #include "telemetry/frsky_hub.h"
  47
  48 #include "cc2500_frsky_d.h"
  49
  50 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
  51 static uint8_t frame[20];
  52 static uint8_t telemetryId;
  53
  54 #if defined(USE_TELEMETRY_FRSKY_HUB)
  55 static bool telemetryEnabled = false;
  56
  57 #define MAX_SERIAL_BYTES 64
  58
  59 static uint8_t telemetryBytesGenerated;
  60 static uint8_t serialBuffer[MAX_SERIAL_BYTES]; // buffer for telemetry serial data
  61
  62 static uint8_t appendFrSkyHubData(uint8_t *buf)
  63 {
  64     static uint8_t telemetryBytesSent = 0;
  65     static uint8_t telemetryBytesAcknowledged = 0;
  66     static uint8_t telemetryIdExpected = 0;
  67
  68     if (telemetryId == telemetryIdExpected) {
  69         telemetryBytesAcknowledged = telemetryBytesSent;
  70
  71         telemetryIdExpected = (telemetryId + 1) & 0x1F;
  72         if (!telemetryBytesGenerated) {
  73             telemetryBytesSent = 0;
  74
  75             processFrSkyHubTelemetry(micros());
  76         }
  77     } else { // rx re-requests last packet
  78         telemetryBytesSent = telemetryBytesAcknowledged;
  79     }
  80
  81     uint8_t index = 0;
  82     for (uint8_t i = 0; i < 10; i++) {
  83         if (telemetryBytesSent == telemetryBytesGenerated) {
  84             telemetryBytesGenerated = 0;
  85
  86             break;
  87         }
  88         buf[i] = serialBuffer[telemetryBytesSent];
  89         telemetryBytesSent = (telemetryBytesSent + 1) & (MAX_SERIAL_BYTES - 1);
  90         index++;
  91     }
  92     return index;
  93 }
  94
  95 static void frSkyDTelemetryWriteByte(const char data)
  96 {
  97     if (telemetryBytesGenerated < MAX_SERIAL_BYTES) {
  98         serialBuffer[telemetryBytesGenerated++] = data;
  99     }
 100 }
 101 #endif
 102
 103 static void buildTelemetryFrame(uint8_t *packet)
 104 {
 105     const uint16_t adcExternal1Sample = adcGetChannel(ADC_EXTERNAL1);
 106     const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
 107     telemetryId = packet[4];
 108     frame[0] = 0x11; // length
 109     frame[1] = rxFrSkySpiConfig()->bindTxId[0];
 110     frame[2] = rxFrSkySpiConfig()->bindTxId[1];
 111     frame[3] = (uint8_t)((adcExternal1Sample & 0xff0) >> 4); // A1
 112     frame[4] = (uint8_t)((adcRssiSample & 0xff0) >> 4);      // A2
 113     frame[5] = (uint8_t)rssiDbm;
 114     uint8_t bytesUsed = 0;
 115 #if defined(USE_TELEMETRY_FRSKY_HUB)
 116     if (telemetryEnabled) {
 117         bytesUsed = appendFrSkyHubData(&frame[8]);
 118     }
 119  #endif
 120     frame[6] = bytesUsed;
 121     frame[7] = telemetryId;
 122 }
 123 #endif // USE_RX_FRSKY_SPI_TELEMETRY
 124
 125 #define FRSKY_D_CHANNEL_SCALING (2.0f / 3)
 126
 127 static void decodeChannelPair(uint16_t *channels, const uint8_t *packet, const uint8_t highNibbleOffset) {
 128     channels[0] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf) << 8 | packet[0]);
 129     channels[1] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf0) << 4 | packet[1]);
 130 }
 131
 132 void frSkyDSetRcData(uint16_t *rcData, const uint8_t *packet)
 133 {
 134     static uint16_t dataErrorCount = 0;
 135
 136     uint16_t channels[RC_CHANNEL_COUNT_FRSKY_D];
 137     bool dataError = false;
 138
 139     decodeChannelPair(channels, packet + 6, 4);
 140     decodeChannelPair(channels + 2, packet + 8, 3);
 141     decodeChannelPair(channels + 4, packet + 12, 4);
 142     decodeChannelPair(channels + 6, packet + 14, 3);
 143
 144     for (int i = 0; i < RC_CHANNEL_COUNT_FRSKY_D; i++) {
 145         if ((channels[i] < 800) || (channels[i] > 2200)) {
 146             dataError = true;
 147
 148             break;
 149         }
 150     }
 151
 152     if (!dataError) {
 153         for (int i = 0; i < RC_CHANNEL_COUNT_FRSKY_D; i++) {
 154             rcData[i] = channels[i];
 155         }
 156     } else {
 157         DEBUG_SET(DEBUG_RX_FRSKY_SPI, DEBUG_DATA_ERROR_COUNT, ++dataErrorCount);
 158     }
 159 }
 160
 161 rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const protocolState)
 162 {
 163     static timeUs_t lastPacketReceivedTime = 0;
 164     static timeUs_t telemetryTimeUs;
 165
 166     static bool ledIsOn;
 167
 168     rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
 169
 170     const timeUs_t currentPacketReceivedTime = micros();
 171
 172     switch (*protocolState) {
 173     case STATE_STARTING:
 174         listLength = 47;
 175         initialiseData(0);
 176         *protocolState = STATE_UPDATE;
 177         nextChannel(1);
 178         cc2500Strobe(CC2500_SRX);
 179
 180         break;
 181     case STATE_UPDATE:
 182         lastPacketReceivedTime = currentPacketReceivedTime;
 183         *protocolState = STATE_DATA;
 184
 185         if (checkBindRequested(false)) {
 186             lastPacketReceivedTime = 0;
 187             timeoutUs = 50;
 188             missingPackets = 0;
 189
 190             *protocolState = STATE_INIT;
 191
 192             break;
 193         }
 194         FALLTHROUGH; //!!TODO -check this fall through is correct
 195     // here FS code could be
 196     case STATE_DATA:
 197         if (IORead(gdoPin)) {
 198             uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
 199             if (ccLen >= 20) {
 200                 cc2500ReadFifo(packet, 20);
 201                 if (packet[19] & 0x80) {
 202                     missingPackets = 0;
 203                     timeoutUs = 1;
 204                     if (packet[0] == 0x11) {
 205                         if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) &&
 206                             (packet[2] == rxFrSkySpiConfig()->bindTxId[1])) {
 207                             LedOn();
 208                             nextChannel(1);
 209 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
 210                             if ((packet[3] % 4) == 2) {
 211                                 telemetryTimeUs = micros();
 212                                 setRssiDbm(packet[18]);
 213                                 buildTelemetryFrame(packet);
 214                                 *protocolState = STATE_TELEMETRY;
 215                             } else
 216 #endif
 217                             {
 218                                 cc2500Strobe(CC2500_SRX);
 219                                 *protocolState = STATE_UPDATE;
 220                             }
 221                             ret = RX_SPI_RECEIVED_DATA;
 222                             lastPacketReceivedTime = currentPacketReceivedTime;
 223                         }
 224                     }
 225                 }
 226             }
 227         }
 228
 229         if (cmpTimeUs(currentPacketReceivedTime, lastPacketReceivedTime) > (timeoutUs * SYNC_DELAY_MAX)) {
 230 #if defined(USE_RX_FRSKY_SPI_PA_LNA)
 231             TxDisable();
 232 #endif
 233             if (timeoutUs == 1) {
 234 #if defined(USE_RX_FRSKY_SPI_PA_LNA) && defined(USE_RX_FRSKY_SPI_DIVERSITY) // SE4311 chip
 235                 if (missingPackets >= 2) {
 236                     switchAntennae();
 237                 }
 238 #endif
 239
 240                 if (missingPackets > MAX_MISSING_PKT) {
 241                     timeoutUs = 50;
 242
 243 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
 244                     setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL);
 245 #endif
 246                 }
 247
 248                 missingPackets++;
 249                 nextChannel(1);
 250             } else {
 251                 if (ledIsOn) {
 252                     LedOff();
 253                 } else {
 254                     LedOn();
 255                 }
 256                 ledIsOn = !ledIsOn;
 257
 258 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
 259                 setRssiUnfiltered(0, RSSI_SOURCE_RX_PROTOCOL);
 260 #endif
 261                 nextChannel(13);
 262             }
 263
 264             cc2500Strobe(CC2500_SRX);
 265             *protocolState = STATE_UPDATE;
 266         }
 267         break;
 268 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
 269     case STATE_TELEMETRY:
 270         if (cmpTimeUs(micros(), telemetryTimeUs) >= 1380) {
 271             cc2500Strobe(CC2500_SIDLE);
 272             cc2500SetPower(6);
 273             cc2500Strobe(CC2500_SFRX);
 274 #if defined(USE_RX_FRSKY_SPI_PA_LNA)
 275             TxEnable();
 276 #endif
 277             cc2500Strobe(CC2500_SIDLE);
 278             cc2500WriteFifo(frame, frame[0] + 1);
 279             *protocolState = STATE_DATA;
 280             ret = RX_SPI_RECEIVED_DATA;
 281             lastPacketReceivedTime = currentPacketReceivedTime;
 282         }
 283
 284         break;
 285
 286 #endif
 287     }
 288
 289     return ret;
 290 }
 291
 292 void frSkyDInit(void)
 293 {
 294 #if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_FRSKY_HUB)
 295     telemetryEnabled = initFrSkyHubTelemetryExternal(frSkyDTelemetryWriteByte);
 296 #endif
 297 }
 298 #endif