src/main/rx/cc2500_frsky_d.c

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