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(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     case FRSKY_SPI_A1_SOURCE_CONST:
 125         a1Value = A1_CONST_D & 0xff;
 126         break;
 127     case FRSKY_SPI_A1_SOURCE_VBAT:
 128     default:
 129         a1Value = (getBatteryVoltage() / 5) & 0xff;
 130         break;
 131     }
 132     const uint8_t a2Value = (adcGetChannel(ADC_RSSI)) >> 4;
 133     telemetryId = packet[4];
 134     frame[0] = 0x11; // length
 135     frame[1] = rxCc2500SpiConfig()->bindTxId[0];
 136     frame[2] = rxCc2500SpiConfig()->bindTxId[1];
 137     frame[3] = a1Value;
 138     frame[4] = a2Value;
 139     frame[5] = (uint8_t)cc2500getRssiDbm();
 140     uint8_t bytesUsed = 0;
 141 #if defined(USE_TELEMETRY_FRSKY_HUB)
 142     if (telemetryEnabled) {
 143         bytesUsed = appendFrSkyHubData(&frame[8]);
 144     }
 145  #endif
 146     frame[6] = bytesUsed;
 147     frame[7] = telemetryId;
 148 }
 149 #endif // USE_RX_FRSKY_SPI_TELEMETRY
 150
 151 #define FRSKY_D_CHANNEL_SCALING (2.0f / 3)
 152
 153 static void decodeChannelPair(uint16_t *channels, const uint8_t *packet, const uint8_t highNibbleOffset) {
 154     channels[0] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf) << 8 | packet[0]);
 155     channels[1] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf0) << 4 | packet[1]);
 156 }
 157
 158 void frSkyDSetRcData(uint16_t *rcData, const uint8_t *packet)
 159 {
 160     static uint16_t dataErrorCount = 0;
 161
 162     uint16_t channels[RC_CHANNEL_COUNT_FRSKY_D];
 163     bool dataError = false;
 164
 165     decodeChannelPair(channels, packet + 6, 4);
 166     decodeChannelPair(channels + 2, packet + 8, 3);
 167     decodeChannelPair(channels + 4, packet + 12, 4);
 168     decodeChannelPair(channels + 6, packet + 14, 3);
 169
 170     for (int i = 0; i < RC_CHANNEL_COUNT_FRSKY_D; i++) {
 171         if ((channels[i] < 800) || (channels[i] > 2200)) {
 172             dataError = true;
 173
 174             break;
 175         }
 176     }
 177
 178     if (!dataError) {
 179         for (int i = 0; i < RC_CHANNEL_COUNT_FRSKY_D; i++) {
 180             rcData[i] = channels[i];
 181         }
 182     } else {
 183         DEBUG_SET(DEBUG_RX_FRSKY_SPI, DEBUG_DATA_ERROR_COUNT, ++dataErrorCount);
 184     }
 185 }
 186
 187 rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const protocolState)
 188 {
 189     static timeUs_t lastPacketReceivedTime = 0;
 190     static timeUs_t telemetryTimeUs;
 191
 192     rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
 193
 194     const timeUs_t currentPacketReceivedTime = micros();
 195
 196     switch (*protocolState) {
 197     case STATE_STARTING:
 198         listLength = 47;
 199         initialiseData(false);
 200         *protocolState = STATE_UPDATE;
 201         nextChannel(1);
 202         cc2500Strobe(CC2500_SRX);
 203
 204         break;
 205     case STATE_UPDATE:
 206         lastPacketReceivedTime = currentPacketReceivedTime;
 207         *protocolState = STATE_DATA;
 208
 209         if (rxSpiCheckBindRequested(false)) {
 210             lastPacketReceivedTime = 0;
 211             timeoutUs = 50;
 212             missingPackets = 0;
 213
 214             *protocolState = STATE_INIT;
 215
 216             break;
 217         }
 218         FALLTHROUGH; //!!TODO -check this fall through is correct
 219     // here FS code could be
 220     case STATE_DATA:
 221         if (rxSpiGetExtiState()) {
 222             uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
 223             bool packetOk = false;
 224             if (ccLen >= 20) {
 225                 cc2500ReadFifo(packet, 20);
 226                 if (packet[19] & 0x80) {
 227                     packetOk = true;
 228                     missingPackets = 0;
 229                     timeoutUs = 1;
 230                     if (packet[0] == 0x11) {
 231                         if ((packet[1] == rxCc2500SpiConfig()->bindTxId[0]) &&
 232                             (packet[2] == rxCc2500SpiConfig()->bindTxId[1]) &&
 233                             (packet[5] == rxCc2500SpiConfig()->bindTxId[2])) {
 234                             rxSpiLedOn();
 235                             nextChannel(1);
 236                             cc2500setRssiDbm(packet[18]);
 237 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
 238                             if ((packet[3] % 4) == 2) {
 239                                 telemetryTimeUs = micros();
 240                                 buildTelemetryFrame(packet);
 241                                 *protocolState = STATE_TELEMETRY;
 242                             } else
 243 #endif
 244                             {
 245                                 cc2500Strobe(CC2500_SRX);
 246                                 *protocolState = STATE_UPDATE;
 247                             }
 248                             ret = RX_SPI_RECEIVED_DATA;
 249                             lastPacketReceivedTime = currentPacketReceivedTime;
 250                         }
 251                     }
 252                 }
 253             }
 254             if (!packetOk) {
 255                 cc2500Strobe(CC2500_SFRX);
 256             }
 257         }
 258
 259         if (cmpTimeUs(currentPacketReceivedTime, lastPacketReceivedTime) > (timeoutUs * SYNC_DELAY_MAX)) {
 260 #if defined(USE_RX_CC2500_SPI_PA_LNA)
 261             cc2500TxDisable();
 262 #endif
 263             if (timeoutUs == 1) {
 264 #if defined(USE_RX_CC2500_SPI_PA_LNA) && defined(USE_RX_CC2500_SPI_DIVERSITY) // SE4311 chip
 265                 if (missingPackets >= 2) {
 266                     cc2500switchAntennae();
 267                 }
 268 #endif
 269
 270                 if (missingPackets > MAX_MISSING_PKT) {
 271                     timeoutUs = 50;
 272
 273                     setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL);
 274                 }
 275
 276                 missingPackets++;
 277                 nextChannel(1);
 278             } else {
 279                 rxSpiLedToggle();
 280
 281                 setRssi(0, RSSI_SOURCE_RX_PROTOCOL);
 282                 nextChannel(13);
 283             }
 284
 285             cc2500Strobe(CC2500_SRX);
 286             *protocolState = STATE_UPDATE;
 287         }
 288         break;
 289 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
 290     case STATE_TELEMETRY:
 291         if (cmpTimeUs(micros(), telemetryTimeUs) >= 1380) {
 292             cc2500Strobe(CC2500_SIDLE);
 293             cc2500SetPower(6);
 294             cc2500Strobe(CC2500_SFRX);
 295 #if defined(USE_RX_CC2500_SPI_PA_LNA)
 296             cc2500TxEnable();
 297 #endif
 298             cc2500Strobe(CC2500_SIDLE);
 299             cc2500WriteFifo(frame, frame[0] + 1);
 300             *protocolState = STATE_DATA;
 301             lastPacketReceivedTime = currentPacketReceivedTime;
 302         }
 303
 304         break;
 305
 306 #endif
 307     }
 308
 309     return ret;
 310 }
 311
 312 void frSkyDInit(void)
 313 {
 314 #if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_FRSKY_HUB)
 315     if (featureIsEnabled(FEATURE_TELEMETRY)) {
 316         telemetryEnabled = initFrSkyHubTelemetryExternal(frSkyDTelemetryWriteByte);
 317     }
 318 #endif
 319 }
 320 #endif