diff --git a/src/main/build/debug.c b/src/main/build/debug.c index bbab9a8d4b..2a06bb965e 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -50,7 +50,7 @@ const char * const debugModeNames[DEBUG_COUNT] = { "FFT", "FFT_TIME", "FFT_FREQ", - "FRSKY_D_RX", + "RX_FRSKY_SPI", "GYRO_RAW", "MAX7456_SIGNAL", "MAX7456_SPICLOCK", diff --git a/src/main/build/debug.h b/src/main/build/debug.h index fa7178e4bf..9284757e09 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -68,7 +68,7 @@ typedef enum { DEBUG_FFT, DEBUG_FFT_TIME, DEBUG_FFT_FREQ, - DEBUG_FRSKY_D_RX, + DEBUG_RX_FRSKY_SPI, DEBUG_GYRO_RAW, DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SPICLOCK, diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index d525e2723b..28c554f939 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -113,7 +113,7 @@ #define PG_SPI_PIN_CONFIG 520 #define PG_ESCSERIAL_CONFIG 521 #define PG_CAMERA_CONTROL_CONFIG 522 -#define PG_FRSKY_D_CONFIG 523 +#define PG_RX_FRSKY_SPI_CONFIG 523 #define PG_MAX7456_CONFIG 524 #define PG_FLYSKY_CONFIG 525 #define PG_TIME_CONFIG 526 diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index 4d5e17d81a..aba38767ec 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -118,7 +118,8 @@ extern uint8_t __config_end; #include "rx/rx.h" #include "rx/spektrum.h" -#include "rx/frsky_d.h" +#include "../rx/cc2500_frsky_common.h" +#include "../rx/cc2500_frsky_x.h" #include "scheduler/scheduler.h" @@ -2112,12 +2113,24 @@ static void cliBeeper(char *cmdline) } #endif -#ifdef USE_RX_FRSKY_D void cliFrSkyBind(char *cmdline){ UNUSED(cmdline); - frSkyDBind(); -} + switch (rxConfig()->rx_spi_protocol) { +#ifdef USE_RX_FRSKY_SPI + case RX_SPI_FRSKY_D: + case RX_SPI_FRSKY_X: + frSkyBind(); + + cliPrint("Binding..."); + + break; #endif + default: + cliPrint("Not supported."); + + break; + } +} static void printMap(uint8_t dumpMask, const rxConfig_t *rxConfig, const rxConfig_t *defaultRxConfig) { @@ -3641,8 +3654,8 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("flash_write", NULL, "
", cliFlashWrite), #endif #endif -#ifdef USE_RX_FRSKY_D - CLI_COMMAND_DEF("frsky_bind", "initiate binding for FrSky RX", NULL, cliFrSkyBind), +#ifdef USE_RX_FRSKY_SPI + CLI_COMMAND_DEF("frsky_bind", "initiate binding for FrSky SPI RX", NULL, cliFrSkyBind), #endif CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet), #ifdef USE_GPS diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index e8ce4c5792..84a86e375d 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -65,8 +65,8 @@ #include "io/vtx_rtc6705.h" #include "rx/rx.h" +#include "rx/cc2500_frsky_common.h" #include "rx/spektrum.h" -#include "rx/frsky_d.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" @@ -195,6 +195,7 @@ static const char * const lookupTableRxSpi[] = { "H8_3D", "INAV", "FRSKY_D", + "FRSKY_X", "FLYSKY", "FLYSKY_2A" }; @@ -658,13 +659,13 @@ const clivalue_t valueTable[] = { { "frsky_unit", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_UNIT }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, frsky_unit) }, #endif { "frsky_vfas_precision", VAR_UINT8 | MASTER_VALUE, .config.minmax = { FRSKY_VFAS_PRECISION_LOW, FRSKY_VFAS_PRECISION_HIGH }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, frsky_vfas_precision) }, -#endif +#endif // USE_TELEMETRY_FRSKY { "hott_alarm_int", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 120 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, hottAlarmSoundInterval) }, { "pid_in_tlm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, pidValuesAsTelemetry) }, #if defined(USE_TELEMETRY_IBUS) { "report_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, report_cell_voltage) }, #endif -#endif +#endif // USE_TELEMETRY // PG_LED_STRIP_CONFIG #ifdef USE_LED_STRIP @@ -804,11 +805,12 @@ const clivalue_t valueTable[] = { { "esc_sensor_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, halfDuplex) }, #endif -#ifdef USE_RX_FRSKY_D - { "frsky_d_autobind", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_FRSKY_D_CONFIG, offsetof(frSkyDConfig_t, autoBind) }, - { "frsky_d_tx_id", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 2, PG_FRSKY_D_CONFIG, offsetof(frSkyDConfig_t, bindTxId) }, - { "frsky_d_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -127, 127 }, PG_FRSKY_D_CONFIG, offsetof(frSkyDConfig_t, bindOffset) }, - { "frsky_d_bind_hop_data", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 50, PG_FRSKY_D_CONFIG, offsetof(frSkyDConfig_t, bindHopData) }, +#ifdef USE_RX_FRSKY_SPI + { "frsky_spi_autobind", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_t, autoBind) }, + { "frsky_spi_tx_id", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 2, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_t, bindTxId) }, + { "frsky_spi_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -127, 127 }, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_t, bindOffset) }, + { "frsky_spi_bind_hop_data", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 50, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_t, bindHopData) }, + { "frsky_x_rx_num", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 255 }, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_t, rxNum) }, #endif { "led_inversion", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, ((1 << STATUS_LED_NUMBER) - 1) }, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, inversion) }, #ifdef USE_DASHBOARD diff --git a/src/main/rx/cc2500_frsky_common.h b/src/main/rx/cc2500_frsky_common.h new file mode 100644 index 0000000000..716aeff0f0 --- /dev/null +++ b/src/main/rx/cc2500_frsky_common.h @@ -0,0 +1,36 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +//#include +//#include + +//#include "rx.h" +//#include "rx_spi.h" + +typedef struct rxFrSkySpiConfig_s { + bool autoBind; + uint8_t bindTxId[2]; + int8_t bindOffset; + uint8_t bindHopData[50]; + uint8_t rxNum; +} rxFrSkySpiConfig_t; + +PG_DECLARE(rxFrSkySpiConfig_t, rxFrSkySpiConfig); + +void frSkyBind(void); diff --git a/src/main/rx/cc2500_frsky_d.c b/src/main/rx/cc2500_frsky_d.c new file mode 100644 index 0000000000..70be7eaf4e --- /dev/null +++ b/src/main/rx/cc2500_frsky_d.c @@ -0,0 +1,396 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef USE_RX_FRSKY_SPI_D + +#include "build/build_config.h" +#include "build/debug.h" + +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/cc2500.h" +#include "drivers/io.h" +#include "drivers/system.h" +#include "drivers/time.h" + +#include "fc/config.h" + +#include "config/feature.h" +#include "config/parameter_group_ids.h" + +#include "rx/rx.h" +#include "rx/rx_spi.h" +#include "rx/cc2500_frsky_common.h" +#include "rx/cc2500_frsky_shared.h" +#include "rx/cc2500_frsky_d.h" + +#include "sensors/battery.h" + +#include "telemetry/frsky.h" + +#define RC_CHANNEL_COUNT 8 +#define MAX_MISSING_PKT 100 + +#define DEBUG_DATA_ERROR_COUNT 0 + +#define SYNC 9000 +#define FS_THR 960 + +static uint32_t missingPackets; +static uint8_t calData[255][3]; +static uint8_t cnt; +static int32_t t_out; +static timeUs_t lastPacketReceivedTime; +static uint8_t protocolState; +static uint32_t start_time; +static uint16_t dataErrorCount = 0; + +#if defined(USE_RX_FRSKY_SPI_PA_LNA) +static uint8_t pass; +#endif + +#ifdef USE_RX_FRSKY_SPI_TELEMETRY +static uint8_t frame[20]; +static uint8_t telemetry_id; +static uint32_t telemetryTime; + +#if defined(USE_TELEMETRY_FRSKY) +#define MAX_SERIAL_BYTES 64 +static uint8_t hub_index; +static uint8_t idxx = 0; +static uint8_t idx_ok = 0; +static uint8_t telemetry_expected_id = 0; +static uint8_t srx_data[MAX_SERIAL_BYTES]; // buffer for telemetry serial data +#endif +#endif + +#if defined(USE_RX_FRSKY_SPI_TELEMETRY) +#if defined(USE_TELEMETRY_FRSKY) +static uint8_t frsky_append_hub_data(uint8_t *buf) +{ + if (telemetry_id == telemetry_expected_id) + idx_ok = idxx; + else // rx re-requests last packet + idxx = idx_ok; + + telemetry_expected_id = (telemetry_id + 1) & 0x1F; + uint8_t index = 0; + for (uint8_t i = 0; i < 10; i++) { + if (idxx == hub_index) { + break; + } + buf[i] = srx_data[idxx]; + idxx = (idxx + 1) & (MAX_SERIAL_BYTES - 1); + index++; + } + return index; +} + +static void frSkyTelemetryInitFrameSpi(void) +{ + hub_index = 0; + idxx = 0; +} + +static void frSkyTelemetryWriteSpi(uint8_t ch) +{ + if (hub_index < MAX_SERIAL_BYTES) { + srx_data[hub_index++] = ch; + } +} +#endif + +static void telemetry_build_frame(uint8_t *packet) +{ + const uint16_t adcExternal1Sample = adcGetChannel(ADC_EXTERNAL1); + const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); + uint8_t bytes_used = 0; + telemetry_id = packet[4]; + frame[0] = 0x11; // length + frame[1] = rxFrSkySpiConfig()->bindTxId[0]; + frame[2] = rxFrSkySpiConfig()->bindTxId[1]; + frame[3] = (uint8_t)((adcExternal1Sample & 0xff0) >> 4); // A1 + frame[4] = (uint8_t)((adcRssiSample & 0xff0) >> 4); // A2 + frame[5] = (uint8_t)RSSI_dBm; +#if defined(USE_TELEMETRY_FRSKY) + bytes_used = frsky_append_hub_data(&frame[8]); +#endif + frame[6] = bytes_used; + frame[7] = telemetry_id; +} + +#endif // USE_RX_FRSKY_SPI_TELEMETRY + +static void initialize(void) +{ + cc2500Reset(); + cc2500WriteReg(CC2500_02_IOCFG0, 0x01); + cc2500WriteReg(CC2500_17_MCSM1, 0x0C); + cc2500WriteReg(CC2500_18_MCSM0, 0x18); + cc2500WriteReg(CC2500_06_PKTLEN, 0x19); + cc2500WriteReg(CC2500_08_PKTCTRL0, 0x05); + cc2500WriteReg(CC2500_3E_PATABLE, 0xFF); + cc2500WriteReg(CC2500_0B_FSCTRL1, 0x08); + cc2500WriteReg(CC2500_0C_FSCTRL0, 0x00); + cc2500WriteReg(CC2500_0D_FREQ2, 0x5C); + cc2500WriteReg(CC2500_0E_FREQ1, 0x76); + cc2500WriteReg(CC2500_0F_FREQ0, 0x27); + cc2500WriteReg(CC2500_10_MDMCFG4, 0xAA); + cc2500WriteReg(CC2500_11_MDMCFG3, 0x39); + cc2500WriteReg(CC2500_12_MDMCFG2, 0x11); + cc2500WriteReg(CC2500_13_MDMCFG1, 0x23); + cc2500WriteReg(CC2500_14_MDMCFG0, 0x7A); + cc2500WriteReg(CC2500_15_DEVIATN, 0x42); + cc2500WriteReg(CC2500_19_FOCCFG, 0x16); + cc2500WriteReg(CC2500_1A_BSCFG, 0x6C); + cc2500WriteReg(CC2500_1B_AGCCTRL2, 0x03); + cc2500WriteReg(CC2500_1C_AGCCTRL1, 0x40); + cc2500WriteReg(CC2500_1D_AGCCTRL0, 0x91); + cc2500WriteReg(CC2500_21_FREND1, 0x56); + cc2500WriteReg(CC2500_22_FREND0, 0x10); + cc2500WriteReg(CC2500_23_FSCAL3, 0xA9); + cc2500WriteReg(CC2500_24_FSCAL2, 0x0A); + cc2500WriteReg(CC2500_25_FSCAL1, 0x00); + cc2500WriteReg(CC2500_26_FSCAL0, 0x11); + cc2500WriteReg(CC2500_29_FSTEST, 0x59); + cc2500WriteReg(CC2500_2C_TEST2, 0x88); + cc2500WriteReg(CC2500_2D_TEST1, 0x31); + cc2500WriteReg(CC2500_2E_TEST0, 0x0B); + cc2500WriteReg(CC2500_03_FIFOTHR, 0x07); + cc2500WriteReg(CC2500_09_ADDR, 0x00); + cc2500Strobe(CC2500_SIDLE); + + cc2500WriteReg(CC2500_07_PKTCTRL1, 0x04); + cc2500WriteReg(CC2500_0C_FSCTRL0, 0); + for (uint8_t c = 0; c < 0xFF; c++) { + cc2500Strobe(CC2500_SIDLE); + cc2500WriteReg(CC2500_0A_CHANNR, c); + cc2500Strobe(CC2500_SCAL); + delayMicroseconds(900); + calData[c][0] = cc2500ReadReg(CC2500_23_FSCAL3); + calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2); + calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1); + } +} + + +#define FRSKY_D_CHANNEL_SCALING (2.0f / 3) + +static void decodeChannelPair(uint16_t *channels, const uint8_t *packet, const uint8_t highNibbleOffset) { + channels[0] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf) << 8 | packet[0]); + channels[1] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf0) << 4 | packet[1]); +} + +void frSkyDSetRcData(uint16_t *rcData, const uint8_t *packet) +{ + uint16_t channels[RC_CHANNEL_COUNT]; + bool dataError = false; + + decodeChannelPair(channels, packet + 6, 4); + decodeChannelPair(channels + 2, packet + 8, 3); + decodeChannelPair(channels + 4, packet + 12, 4); + decodeChannelPair(channels + 6, packet + 14, 3); + + for (int i = 0; i < RC_CHANNEL_COUNT; i++) { + if ((channels[i] < 800) || (channels[i] > 2200)) { + dataError = true; + + break; + } + } + + if (!dataError) { + for (int i = 0; i < RC_CHANNEL_COUNT; i++) { + rcData[i] = channels[i]; + } + } else { + DEBUG_SET(DEBUG_RX_FRSKY_SPI, DEBUG_DATA_ERROR_COUNT, ++dataErrorCount); + } +} + +rx_spi_received_e frSkyDDataReceived(uint8_t *packet) +{ + const timeUs_t currentPacketReceivedTime = micros(); + + rx_spi_received_e ret = RX_SPI_RECEIVED_NONE; + + switch (protocolState) { + case STATE_INIT: + if ((millis() - start_time) > 10) { + initialize(); + + protocolState = STATE_BIND; + } + + break; + case STATE_BIND: + case STATE_BIND_TUNING: + case STATE_BIND_BINDING1: + case STATE_BIND_BINDING2: + case STATE_BIND_COMPLETE: + handleBinding(protocolState, packet); + + break; + case STATE_STARTING: + listLength = 47; + initialiseData(0); + protocolState = STATE_UPDATE; + nextChannel(1, true); // + cc2500Strobe(CC2500_SRX); + ret = RX_SPI_RECEIVED_BIND; + + break; + case STATE_UPDATE: + lastPacketReceivedTime = currentPacketReceivedTime; + protocolState = STATE_DATA; + + if (checkBindRequested(false)) { + lastPacketReceivedTime = 0; + t_out = 50; + missingPackets = 0; + + protocolState = STATE_INIT; + + break; + } + // here FS code could be + case STATE_DATA: + if (IORead(gdoPin)) { + uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; + if (ccLen >= 20) { + cc2500ReadFifo(packet, 20); + if (packet[19] & 0x80) { + missingPackets = 0; + t_out = 1; + if (packet[0] == 0x11) { + if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) && + (packet[2] == rxFrSkySpiConfig()->bindTxId[1])) { + IOHi(frSkyLedPin); + nextChannel(1, true); +#if defined(USE_RX_FRSKY_SPI_TELEMETRY) + if ((packet[3] % 4) == 2) { + telemetryTime = micros(); + setRssiDbm(packet[18]); + telemetry_build_frame(packet); + protocolState = STATE_TELEMETRY; + } else +#endif + { + cc2500Strobe(CC2500_SRX); + protocolState = STATE_UPDATE; + } + ret = RX_SPI_RECEIVED_DATA; + lastPacketReceivedTime = currentPacketReceivedTime; + } + } + } + } + } + + if (cmpTimeUs(currentPacketReceivedTime, lastPacketReceivedTime) > (t_out * SYNC)) { +#if defined(USE_RX_FRSKY_SPI_PA_LNA) + RxEnable(); +#endif + if (t_out == 1) { +#if defined(USE_RX_FRSKY_SPI_PA_LNA) && defined(USE_RX_FRSKY_SPI_DIVERSITY) // SE4311 chip + if (missingPackets >= 2) { + if (pass & 0x01) { + IOHi(antSelPin); + } else { + IOLo(antSelPin); + } + pass++; + } +#endif + + if (missingPackets > MAX_MISSING_PKT) { + t_out = 50; + +#if defined(USE_RX_FRSKY_SPI_TELEMETRY) + setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL); +#endif + } + + missingPackets++; + nextChannel(1, true); + } else { + if (cnt++ & 0x01) { + IOLo(frSkyLedPin); + } else { + IOHi(frSkyLedPin); + } + +#if defined(USE_RX_FRSKY_SPI_TELEMETRY) + setRssiUnfiltered(0, RSSI_SOURCE_RX_PROTOCOL); +#endif + nextChannel(13, true); + } + + cc2500Strobe(CC2500_SRX); + protocolState = STATE_UPDATE; + } + break; +#if defined(USE_RX_FRSKY_SPI_TELEMETRY) + case STATE_TELEMETRY: + if ((micros() - telemetryTime) >= 1380) { + cc2500Strobe(CC2500_SIDLE); + cc2500SetPower(6); + cc2500Strobe(CC2500_SFRX); +#if defined(USE_RX_FRSKY_SPI_PA_LNA) + TxEnable(); +#endif + cc2500Strobe(CC2500_SIDLE); + cc2500WriteFifo(frame, frame[0] + 1); + protocolState = STATE_DATA; + ret = RX_SPI_RECEIVED_DATA; + lastPacketReceivedTime = currentPacketReceivedTime; + } + + break; + +#endif + } + return ret; +} + +void frSkyDInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +{ + UNUSED(rxConfig); + + rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT; + + frskySpiRxSetup(); + + lastPacketReceivedTime = 0; + +#if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_FRSKY) + initFrSkyExternalTelemetry(&frSkyTelemetryInitFrameSpi, + &frSkyTelemetryWriteSpi); +#endif + if (rssiSource == RSSI_SOURCE_NONE) { + rssiSource = RSSI_SOURCE_RX_PROTOCOL; + } +} +#endif diff --git a/src/main/rx/frsky_d.h b/src/main/rx/cc2500_frsky_d.h similarity index 75% rename from src/main/rx/frsky_d.h rename to src/main/rx/cc2500_frsky_d.h index c565f8943f..8b66dfeba9 100644 --- a/src/main/rx/frsky_d.h +++ b/src/main/rx/cc2500_frsky_d.h @@ -17,23 +17,10 @@ #pragma once -#include -#include - +#include "rx.h" #include "rx_spi.h" -typedef struct frSkyDConfig_s { - bool autoBind; - uint8_t bindTxId[2]; - int8_t bindOffset; - uint8_t bindHopData[50]; -} frSkyDConfig_t; - -PG_DECLARE(frSkyDConfig_t, frSkyDConfig); - -struct rxConfig_s; -struct rxRuntimeConfig_s; void frSkyDInit(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); void frSkyDSetRcData(uint16_t *rcData, const uint8_t *payload); rx_spi_received_e frSkyDDataReceived(uint8_t *payload); -void frSkyDBind(void); +void frSkyBind(void); diff --git a/src/main/rx/cc2500_frsky_shared.c b/src/main/rx/cc2500_frsky_shared.c new file mode 100644 index 0000000000..30ac91f090 --- /dev/null +++ b/src/main/rx/cc2500_frsky_shared.c @@ -0,0 +1,408 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include "platform.h" + +#ifdef USE_RX_FRSKY_SPI + +#include "common/maths.h" + +#include "drivers/cc2500.h" +#include "drivers/io.h" +#include "drivers/time.h" + +#include "fc/config.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "rx/rx.h" + +#include "rx/cc2500_frsky_common.h" + +#include "cc2500_frsky_shared.h" + +static uint32_t missingPackets; +static uint8_t calData[255][3]; +static timeMs_t timeTunedMs; +uint8_t listLength; +static uint8_t bindIdx; +static uint8_t Lqi; +static uint8_t protocolState; +static timeMs_t timeStartedMs; +static int8_t bindOffset; +static bool lastBindPinStatus; +bool bindRequested = false; + +IO_t gdoPin; +static IO_t bindPin = DEFIO_IO(NONE); +IO_t frSkyLedPin; + +#if defined(USE_RX_FRSKY_SPI_PA_LNA) +static IO_t txEnPin; +static IO_t rxLnaEnPin; +IO_t antSelPin; +#endif + +#ifdef USE_RX_FRSKY_SPI_TELEMETRY +int16_t RSSI_dBm; +#endif + +PG_REGISTER_WITH_RESET_TEMPLATE(rxFrSkySpiConfig_t, rxFrSkySpiConfig, PG_RX_FRSKY_SPI_CONFIG, 0); + +PG_RESET_TEMPLATE(rxFrSkySpiConfig_t, rxFrSkySpiConfig, + .autoBind = false, + .bindTxId = {0, 0}, + .bindOffset = 0, + .bindHopData = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + .rxNum = 0, +); + +#if defined(USE_RX_FRSKY_SPI_TELEMETRY) +void setRssiDbm(uint8_t value) +{ + if (value >= 128) { + RSSI_dBm = ((((uint16_t)value) * 18) >> 5) - 82; + } else { + RSSI_dBm = ((((uint16_t)value) * 18) >> 5) + 65; + } + + setRssiUnfiltered(constrain(RSSI_dBm << 3, 0, 1023), RSSI_SOURCE_RX_PROTOCOL); +} +#endif // USE_RX_FRSKY_SPI_TELEMETRY + +#if defined(USE_RX_FRSKY_SPI_PA_LNA) +void RxEnable(void) +{ + IOLo(txEnPin); +} + +void TxEnable(void) +{ + IOHi(txEnPin); +} +#endif + +void frSkyBind(void) +{ + bindRequested = true; +} + +void initialiseData(uint8_t adr) +{ + cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)rxFrSkySpiConfig()->bindOffset); + cc2500WriteReg(CC2500_18_MCSM0, 0x8); + cc2500WriteReg(CC2500_09_ADDR, adr ? 0x03 : rxFrSkySpiConfig()->bindTxId[0]); + cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0D); + cc2500WriteReg(CC2500_19_FOCCFG, 0x16); + delay(10); +} + +static void initTuneRx(void) +{ + cc2500WriteReg(CC2500_19_FOCCFG, 0x14); + timeTunedMs = millis(); + bindOffset = -126; + cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset); + cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0C); + cc2500WriteReg(CC2500_18_MCSM0, 0x8); + + cc2500Strobe(CC2500_SIDLE); + cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]); + cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]); + cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]); + cc2500WriteReg(CC2500_0A_CHANNR, 0); + cc2500Strobe(CC2500_SFRX); + cc2500Strobe(CC2500_SRX); +} + +static bool tuneRx(uint8_t *packet) +{ + if (bindOffset >= 126) { + bindOffset = -126; + } + if ((millis() - timeTunedMs) > 50) { + timeTunedMs = millis(); + bindOffset += 5; + cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset); + } + if (IORead(gdoPin)) { + uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; + if (ccLen) { + cc2500ReadFifo(packet, ccLen); + if (packet[ccLen - 1] & 0x80) { + if (packet[2] == 0x01) { + Lqi = packet[ccLen - 1] & 0x7F; + if (Lqi < 50) { + rxFrSkySpiConfigMutable()->bindOffset = bindOffset; + + return true; + } + } + } + } + } + + return false; +} + +static void initGetBind(void) +{ + cc2500Strobe(CC2500_SIDLE); + cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]); + cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]); + cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]); + cc2500WriteReg(CC2500_0A_CHANNR, 0); + cc2500Strobe(CC2500_SFRX); + delayMicroseconds(20); // waiting flush FIFO + + cc2500Strobe(CC2500_SRX); + listLength = 0; + bindIdx = 0x05; +} + +static bool getBind1(uint8_t *packet) +{ + // len|bind |tx + // id|03|01|idx|h0|h1|h2|h3|h4|00|00|00|00|00|00|00|00|00|00|00|00|00|00|00|CHK1|CHK2|RSSI|LQI/CRC| + // Start by getting bind packet 0 and the txid + if (IORead(gdoPin)) { + uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; + if (ccLen) { + cc2500ReadFifo(packet, ccLen); + if (packet[ccLen - 1] & 0x80) { + if (packet[2] == 0x01) { + if (packet[5] == 0x00) { + rxFrSkySpiConfigMutable()->bindTxId[0] = packet[3]; + rxFrSkySpiConfigMutable()->bindTxId[1] = packet[4]; + for (uint8_t n = 0; n < 5; n++) { + rxFrSkySpiConfigMutable()->bindHopData[packet[5] + n] = + packet[6 + n]; + } + + return true; + } + } + } + } + } + + return false; +} + +static bool getBind2(uint8_t *packet) +{ + if (bindIdx <= 120) { + if (IORead(gdoPin)) { + uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; + if (ccLen) { + cc2500ReadFifo(packet, ccLen); + if (packet[ccLen - 1] & 0x80) { + if (packet[2] == 0x01) { + if ((packet[3] == rxFrSkySpiConfig()->bindTxId[0]) && + (packet[4] == rxFrSkySpiConfig()->bindTxId[1])) { + if (packet[5] == bindIdx) { +#if defined(DJTS) + if (packet[5] == 0x2D) { + for (uint8_t i = 0; i < 2; i++) { + rxFrSkySpiConfigMutable()->bindHopData[packet[5] + i] = packet[6 + i]; + } + listLength = 47; + + return true; + } +#endif + for (uint8_t n = 0; n < 5; n++) { + if (packet[6 + n] == packet[ccLen - 3] || (packet[6 + n] == 0)) { + if (bindIdx >= 0x2D) { + listLength = packet[5] + n; + + return true; + } + } + + rxFrSkySpiConfigMutable()->bindHopData[packet[5] + n] = packet[6 + n]; + } + + bindIdx = bindIdx + 5; + + return false; + } + } + } + } + } + } + + return false; + } else { + return true; + } +} + +bool checkBindRequested(bool reset) +{ + if (bindPin) { + bool bindPinStatus = IORead(bindPin); + if (lastBindPinStatus && !bindPinStatus) { + bindRequested = true; + } + lastBindPinStatus = bindPinStatus; + } + + if (!bindRequested) { + return false; + } else { + if (reset) { + bindRequested = false; + } + + return true; + } +} + +void handleBinding(uint8_t protocolState, uint8_t *packet) +{ + switch (protocolState) { + case STATE_BIND: + if (checkBindRequested(true) || rxFrSkySpiConfig()->autoBind) { + IOHi(frSkyLedPin); + initTuneRx(); + + protocolState = STATE_BIND_TUNING; + } else { + protocolState = STATE_STARTING; + } + + break; + case STATE_BIND_TUNING: + if (tuneRx(packet)) { + initGetBind(); + initialiseData(1); + + protocolState = STATE_BIND_BINDING1; + } + + break; + case STATE_BIND_BINDING1: + if (getBind1(packet)) { + protocolState = STATE_BIND_BINDING2; + } + + break; + case STATE_BIND_BINDING2: + if (getBind2(packet)) { + cc2500Strobe(CC2500_SIDLE); + + protocolState = STATE_BIND_COMPLETE; + } + + break; + case STATE_BIND_COMPLETE: + if (!rxFrSkySpiConfig()->autoBind) { + writeEEPROM(); + } else { + uint8_t ctr = 40; + while (ctr--) { + IOHi(frSkyLedPin); + delay(50); + IOLo(frSkyLedPin); + delay(50); + } + } + + protocolState = STATE_STARTING; + + break; + } +} + +void nextChannel(uint8_t skip, bool sendStrobe) +{ + static uint8_t channr = 0; + + channr += skip; + while (channr >= listLength) { + channr -= listLength; + } + cc2500Strobe(CC2500_SIDLE); + cc2500WriteReg(CC2500_23_FSCAL3, + calData[rxFrSkySpiConfig()->bindHopData[channr]][0]); + cc2500WriteReg(CC2500_24_FSCAL2, + calData[rxFrSkySpiConfig()->bindHopData[channr]][1]); + cc2500WriteReg(CC2500_25_FSCAL1, + calData[rxFrSkySpiConfig()->bindHopData[channr]][2]); + cc2500WriteReg(CC2500_0A_CHANNR, rxFrSkySpiConfig()->bindHopData[channr]); + if (sendStrobe) { + cc2500Strobe(CC2500_SFRX); + } +} + +void frskySpiRxSetup() +{ + // gpio init here + gdoPin = IOGetByTag(IO_TAG(RX_FRSKY_SPI_GDO_0_PIN)); + IOInit(gdoPin, OWNER_RX_BIND, 0); + IOConfigGPIO(gdoPin, IOCFG_IN_FLOATING); + frSkyLedPin = IOGetByTag(IO_TAG(RX_FRSKY_SPI_LED_PIN)); + IOInit(frSkyLedPin, OWNER_LED, 0); + IOConfigGPIO(frSkyLedPin, IOCFG_OUT_PP); +#if defined(USE_RX_FRSKY_SPI_PA_LNA) + rxLnaEnPin = IOGetByTag(IO_TAG(RX_FRSKY_SPI_LNA_EN_PIN)); + IOInit(rxLnaEnPin, OWNER_RX_BIND, 0); + IOConfigGPIO(rxLnaEnPin, IOCFG_OUT_PP); + IOHi(rxLnaEnPin); // always on at the moment + txEnPin = IOGetByTag(IO_TAG(RX_FRSKY_SPI_TX_EN_PIN)); + IOInit(txEnPin, OWNER_RX_BIND, 0); + IOConfigGPIO(txEnPin, IOCFG_OUT_PP); +#if defined(USE_RX_FRSKY_SPI_DIVERSITY) + antSelPin = IOGetByTag(IO_TAG(RX_FRSKY_SPI_ANT_SEL_PIN)); + IOInit(antSelPin, OWNER_RX_BIND, 0); + IOConfigGPIO(antSelPin, IOCFG_OUT_PP); +#endif +#endif // USE_RX_FRSKY_SPI_PA_LNA +#if defined(BINDPLUG_PIN) + bindPin = IOGetByTag(IO_TAG(BINDPLUG_PIN)); + IOInit(bindPin, OWNER_RX_BIND, 0); + IOConfigGPIO(bindPin, IOCFG_IPU); + + lastBindPinStatus = IORead(bindPin); +#endif + + timeStartedMs = millis(); + missingPackets = 0; + protocolState = STATE_INIT; +#if defined(USE_RX_FRSKY_SPI_PA_LNA) +#if defined(USE_RX_FRSKY_SPI_DIVERSITY) + IOHi(antSelPin); +#endif + RxEnable(); +#endif // USE_RX_FRSKY_SPI_PA_LNA + + if (rssiSource == RSSI_SOURCE_NONE) { + rssiSource = RSSI_SOURCE_RX_PROTOCOL; + } + + // if(!frSkySpiDetect())//detect spi working routine + // return; +} +#endif diff --git a/src/main/rx/cc2500_frsky_shared.h b/src/main/rx/cc2500_frsky_shared.h new file mode 100644 index 0000000000..3325132171 --- /dev/null +++ b/src/main/rx/cc2500_frsky_shared.h @@ -0,0 +1,61 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define MAX_MISSING_PKT 100 + +#define DEBUG_DATA_ERROR_COUNT 0 + +#define SYNC 9000 + +enum { + STATE_INIT = 0, + STATE_BIND, + STATE_BIND_TUNING, + STATE_BIND_BINDING1, + STATE_BIND_BINDING2, + STATE_BIND_COMPLETE, + STATE_STARTING, + STATE_UPDATE, + STATE_DATA, + STATE_TELEMETRY, + STATE_RESUME, +}; + +extern bool bindRequested; +extern uint8_t listLength; +extern int16_t RSSI_dBm; + +extern IO_t gdoPin; +extern IO_t frSkyLedPin; +extern IO_t antSelPin; + +void setRssiDbm(uint8_t value); + +void frskySpiRxSetup(); + +void RxEnable(void); +void TxEnable(void); + +void initialiseData(uint8_t adr); + +bool checkBindRequested(bool reset); + +void handleBinding(uint8_t protocolState, uint8_t *packet); + +void nextChannel(uint8_t skip, bool sendStrobe); diff --git a/src/main/rx/cc2500_frsky_x.c b/src/main/rx/cc2500_frsky_x.c new file mode 100644 index 0000000000..5298368e13 --- /dev/null +++ b/src/main/rx/cc2500_frsky_x.c @@ -0,0 +1,630 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_RX_FRSKY_SPI_X + +#include "build/build_config.h" +#include "build/debug.h" + +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/adc.h" +#include "drivers/cc2500.h" +#include "drivers/io.h" +#include "drivers/io_def.h" +#include "drivers/io_types.h" +#include "drivers/resource.h" +#include "drivers/system.h" +#include "drivers/time.h" + +#include "fc/config.h" + +#include "rx/rx.h" +#include "rx/rx_spi.h" +#include "rx/cc2500_frsky_common.h" +#include "rx/cc2500_frsky_shared.h" +#include "rx/cc2500_frsky_x.h" + +#include "sensors/battery.h" + +#include "telemetry/smartport.h" + +#define RC_CHANNEL_COUNT 16 + +const uint16_t CRCTable[] = { + 0x0000,0x1189,0x2312,0x329b,0x4624,0x57ad,0x6536,0x74bf, + 0x8c48,0x9dc1,0xaf5a,0xbed3,0xca6c,0xdbe5,0xe97e,0xf8f7, + 0x1081,0x0108,0x3393,0x221a,0x56a5,0x472c,0x75b7,0x643e, + 0x9cc9,0x8d40,0xbfdb,0xae52,0xdaed,0xcb64,0xf9ff,0xe876, + 0x2102,0x308b,0x0210,0x1399,0x6726,0x76af,0x4434,0x55bd, + 0xad4a,0xbcc3,0x8e58,0x9fd1,0xeb6e,0xfae7,0xc87c,0xd9f5, + 0x3183,0x200a,0x1291,0x0318,0x77a7,0x662e,0x54b5,0x453c, + 0xbdcb,0xac42,0x9ed9,0x8f50,0xfbef,0xea66,0xd8fd,0xc974, + 0x4204,0x538d,0x6116,0x709f,0x0420,0x15a9,0x2732,0x36bb, + 0xce4c,0xdfc5,0xed5e,0xfcd7,0x8868,0x99e1,0xab7a,0xbaf3, + 0x5285,0x430c,0x7197,0x601e,0x14a1,0x0528,0x37b3,0x263a, + 0xdecd,0xcf44,0xfddf,0xec56,0x98e9,0x8960,0xbbfb,0xaa72, + 0x6306,0x728f,0x4014,0x519d,0x2522,0x34ab,0x0630,0x17b9, + 0xef4e,0xfec7,0xcc5c,0xddd5,0xa96a,0xb8e3,0x8a78,0x9bf1, + 0x7387,0x620e,0x5095,0x411c,0x35a3,0x242a,0x16b1,0x0738, + 0xffcf,0xee46,0xdcdd,0xcd54,0xb9eb,0xa862,0x9af9,0x8b70, + 0x8408,0x9581,0xa71a,0xb693,0xc22c,0xd3a5,0xe13e,0xf0b7, + 0x0840,0x19c9,0x2b52,0x3adb,0x4e64,0x5fed,0x6d76,0x7cff, + 0x9489,0x8500,0xb79b,0xa612,0xd2ad,0xc324,0xf1bf,0xe036, + 0x18c1,0x0948,0x3bd3,0x2a5a,0x5ee5,0x4f6c,0x7df7,0x6c7e, + 0xa50a,0xb483,0x8618,0x9791,0xe32e,0xf2a7,0xc03c,0xd1b5, + 0x2942,0x38cb,0x0a50,0x1bd9,0x6f66,0x7eef,0x4c74,0x5dfd, + 0xb58b,0xa402,0x9699,0x8710,0xf3af,0xe226,0xd0bd,0xc134, + 0x39c3,0x284a,0x1ad1,0x0b58,0x7fe7,0x6e6e,0x5cf5,0x4d7c, + 0xc60c,0xd785,0xe51e,0xf497,0x8028,0x91a1,0xa33a,0xb2b3, + 0x4a44,0x5bcd,0x6956,0x78df,0x0c60,0x1de9,0x2f72,0x3efb, + 0xd68d,0xc704,0xf59f,0xe416,0x90a9,0x8120,0xb3bb,0xa232, + 0x5ac5,0x4b4c,0x79d7,0x685e,0x1ce1,0x0d68,0x3ff3,0x2e7a, + 0xe70e,0xf687,0xc41c,0xd595,0xa12a,0xb0a3,0x8238,0x93b1, + 0x6b46,0x7acf,0x4854,0x59dd,0x2d62,0x3ceb,0x0e70,0x1ff9, + 0xf78f,0xe606,0xd49d,0xc514,0xb1ab,0xa022,0x92b9,0x8330, + 0x7bc7,0x6a4e,0x58d5,0x495c,0x3de3,0x2c6a,0x1ef1,0x0f78 +}; + +#define TELEMETRY_OUT_BUFFER_SIZE 64 + +#define TELEMETRY_SEQUENCE_LENGTH 4 + +typedef struct telemetrySequenceMarkerData_s { + unsigned int packetSequenceId: 2; + unsigned int unused: 1; + unsigned int initRequest: 1; + unsigned int ackSequenceId: 2; + unsigned int retransmissionRequested: 1; + unsigned int initResponse: 1; +} __attribute__ ((__packed__)) telemetrySequenceMarkerData_t; + +typedef union telemetrySequenceMarker_s { + uint8_t raw; + telemetrySequenceMarkerData_t data; +} __attribute__ ((__packed__)) telemetrySequenceMarker_t; + +#define SEQUENCE_MARKER_REMOTE_PART 0xf0 + +#define TELEMETRY_DATA_SIZE 5 + +typedef struct telemetryData_s { + uint8_t dataLength; + uint8_t data[TELEMETRY_DATA_SIZE]; +} __attribute__ ((__packed__)) telemetryData_t; + +typedef struct telemetryBuffer_s { + telemetryData_t data; + uint8_t needsProcessing; +} telemetryBuffer_t; + +#define TELEMETRY_FRAME_SIZE sizeof(telemetryData_t) + +typedef struct telemetryPayload_s { + uint8_t packetConst; + uint8_t rssiA1; + telemetrySequenceMarker_t sequence; + telemetryData_t data; + uint8_t crc[2]; +} __attribute__ ((__packed__)) telemetryPayload_t; + +static telemetryBuffer_t telemetryRxBuffer[TELEMETRY_SEQUENCE_LENGTH]; +static telemetryData_t telemetryTxBuffer[TELEMETRY_SEQUENCE_LENGTH]; + +static uint8_t remoteProcessedId = 0; +static uint8_t remoteAckId = 0; + +static uint8_t remoteToProcessIndex = 0; + +static uint8_t localPacketId; + +static telemetrySequenceMarker_t responseToSend; + +static uint8_t ccLen; +static uint32_t missingPackets; +static uint8_t calData[255][3]; +static uint8_t cnt; +static timeDelta_t t_out; +static timeUs_t packet_timer; +static uint8_t protocolState; +static int16_t word_temp; +static uint32_t start_time; + +static bool frame_received; +static uint8_t one_time=1; +static uint8_t chanskip=1; + +#if defined(USE_RX_FRSKY_SPI_PA_LNA) +static uint8_t pass; +#endif +static timeDelta_t t_received; +#ifdef USE_RX_FRSKY_SPI_TELEMETRY +static uint8_t frame[20]; +static uint8_t telemetryRX; +#if defined(USE_TELEMETRY_SMARTPORT) +static uint8_t telemetryOutReader = 0; +static uint8_t telemetryOutWriter; + +static uint8_t telemetryOutBuffer[TELEMETRY_OUT_BUFFER_SIZE]; + +static bool telemetryEnabled = false; +#endif +#endif + + +bool frskySpiDetect(void)//debug CC2500 spi +{ + uint8_t tmp[2]; + tmp[0] = cc2500ReadReg(CC2500_30_PARTNUM | CC2500_READ_BURST);//Cc2500 read registers chip part num + tmp[1] = cc2500ReadReg(CC2500_31_VERSION | CC2500_READ_BURST);//Cc2500 read registers chip version + if (tmp[0] == 0x80 && tmp[1]==0x03){ + return true; + } + return false; +} + + +static uint16_t crc(uint8_t *data, uint8_t len) { + uint16_t crc = 0; + for(uint8_t i=0; i < len; i++) + crc = (crc<<8) ^ (CRCTable[((uint8_t)(crc>>8) ^ *data++) & 0xFF]); + return crc; +} + +#if defined(USE_TELEMETRY_SMARTPORT) +static uint8_t frsky_append_sport_data(uint8_t *buf) +{ + uint8_t index; + for (index = 0; index < TELEMETRY_DATA_SIZE; index++) { //max 5 bytes in a frame + if(telemetryOutReader == telemetryOutWriter){ //no new data + break; + } + buf[index] = telemetryOutBuffer[telemetryOutReader]; + telemetryOutReader = (telemetryOutReader + 1) % TELEMETRY_OUT_BUFFER_SIZE; + } + + return index; +} +#endif + +#if defined(USE_RX_FRSKY_SPI_TELEMETRY) +static void telemetry_build_frame(uint8_t *packet) +{ + frame[0]=0x0E;//length + frame[1]=rxFrSkySpiConfig()->bindTxId[0]; + frame[2]=rxFrSkySpiConfig()->bindTxId[1]; + frame[3]=packet[3]; + + static bool evenRun = false; + if (evenRun) { + frame[4]=(uint8_t)RSSI_dBm|0x80; + } else { + const uint16_t adcExternal1Sample = adcGetChannel(ADC_EXTERNAL1); + frame[4]=(uint8_t)((adcExternal1Sample & 0xfe0) >> 5); // A1; + } + evenRun = !evenRun; + + telemetrySequenceMarker_t *inFrameMarker = (telemetrySequenceMarker_t *)&packet[21]; + telemetrySequenceMarker_t *outFrameMarker = (telemetrySequenceMarker_t *)&frame[5]; + if (inFrameMarker->data.initRequest) {//check syncronization at startup ok if not no sport telemetry + outFrameMarker-> raw = 0; + outFrameMarker->data.initRequest = 1; + outFrameMarker->data.initResponse = 1; + + localPacketId = 0; + } else { + if (inFrameMarker->data.retransmissionRequested) { + uint8_t retransmissionFrameId = inFrameMarker->data.ackSequenceId; + outFrameMarker->raw = responseToSend.raw & SEQUENCE_MARKER_REMOTE_PART; + outFrameMarker->data.packetSequenceId = retransmissionFrameId; + + memcpy(&frame[6], &telemetryTxBuffer[retransmissionFrameId], TELEMETRY_FRAME_SIZE); + } else { + uint8_t localAckId = inFrameMarker->data.ackSequenceId; + if (localPacketId != (localAckId + 1) % TELEMETRY_SEQUENCE_LENGTH) { + outFrameMarker->raw = responseToSend.raw & SEQUENCE_MARKER_REMOTE_PART; + outFrameMarker->data.packetSequenceId = localPacketId; +\ + frame[6] = frsky_append_sport_data(&frame[7]); + memcpy(&telemetryTxBuffer[localPacketId], &frame[6], TELEMETRY_FRAME_SIZE); + + localPacketId = (localPacketId + 1) % TELEMETRY_SEQUENCE_LENGTH; + } + } + } + + uint16_t lcrc = crc(&frame[3], 10); + frame[13]=lcrc>>8; + frame[14]=lcrc; +} + +static bool frSkyXCheckQueueEmpty(void) +{ + return true; +} + +#if defined(USE_TELEMETRY_SMARTPORT) +static void frSkyXTelemetrySendByte(uint8_t c) { + if (c == FSSP_DLE || c == FSSP_START_STOP) { + telemetryOutBuffer[telemetryOutWriter] = FSSP_DLE; + telemetryOutWriter = (telemetryOutWriter + 1) % TELEMETRY_OUT_BUFFER_SIZE; + telemetryOutBuffer[telemetryOutWriter] = c ^ FSSP_DLE_XOR; + telemetryOutWriter = (telemetryOutWriter + 1) % TELEMETRY_OUT_BUFFER_SIZE; + } else { + telemetryOutBuffer[telemetryOutWriter] = c; + telemetryOutWriter = (telemetryOutWriter + 1) % TELEMETRY_OUT_BUFFER_SIZE; + } +} + +static void frSkyXTelemetryWriteFrame(const smartPortPayload_t *payload) +{ + telemetryOutBuffer[telemetryOutWriter] = FSSP_START_STOP; + telemetryOutWriter = (telemetryOutWriter + 1) % TELEMETRY_OUT_BUFFER_SIZE; + telemetryOutBuffer[telemetryOutWriter] = FSSP_SENSOR_ID1 & 0x1f; + telemetryOutWriter = (telemetryOutWriter + 1) % TELEMETRY_OUT_BUFFER_SIZE; + uint8_t *data = (uint8_t *)payload; + for (unsigned i = 0; i < sizeof(smartPortPayload_t); i++) { + frSkyXTelemetrySendByte(*data++); + } +} +#endif +#endif // USE_RX_FRSKY_SPI_TELEMETRY + + +static void initialize() { + cc2500Reset(); + cc2500WriteReg(CC2500_02_IOCFG0, 0x01); + cc2500WriteReg(CC2500_17_MCSM1, 0x0C); + cc2500WriteReg(CC2500_18_MCSM0, 0x18); + cc2500WriteReg(CC2500_06_PKTLEN, 0x1E); + cc2500WriteReg(CC2500_07_PKTCTRL1, 0x04); + cc2500WriteReg(CC2500_08_PKTCTRL0, 0x01); + cc2500WriteReg(CC2500_3E_PATABLE, 0xFF); + cc2500WriteReg(CC2500_0B_FSCTRL1, 0x0A); + cc2500WriteReg(CC2500_0C_FSCTRL0, 0x00); + cc2500WriteReg(CC2500_0D_FREQ2, 0x5C); + cc2500WriteReg(CC2500_0E_FREQ1, 0x76); + cc2500WriteReg(CC2500_0F_FREQ0, 0x27); + cc2500WriteReg(CC2500_10_MDMCFG4, 0x7B); + cc2500WriteReg(CC2500_11_MDMCFG3, 0x61); + cc2500WriteReg(CC2500_12_MDMCFG2, 0x13); + cc2500WriteReg(CC2500_13_MDMCFG1, 0x23); + cc2500WriteReg(CC2500_14_MDMCFG0, 0x7A); + cc2500WriteReg(CC2500_15_DEVIATN, 0x51); + cc2500WriteReg(CC2500_19_FOCCFG, 0x16); + cc2500WriteReg(CC2500_1A_BSCFG, 0x6C); + cc2500WriteReg(CC2500_1B_AGCCTRL2, 0x03); + cc2500WriteReg(CC2500_1C_AGCCTRL1, 0x40); + cc2500WriteReg(CC2500_1D_AGCCTRL0, 0x91); + cc2500WriteReg(CC2500_21_FREND1, 0x56); + cc2500WriteReg(CC2500_22_FREND0, 0x10); + cc2500WriteReg(CC2500_23_FSCAL3, 0xA9); + cc2500WriteReg(CC2500_24_FSCAL2, 0x0A); + cc2500WriteReg(CC2500_25_FSCAL1, 0x00); + cc2500WriteReg(CC2500_26_FSCAL0, 0x11); + cc2500WriteReg(CC2500_29_FSTEST, 0x59); + cc2500WriteReg(CC2500_2C_TEST2, 0x88); + cc2500WriteReg(CC2500_2D_TEST1, 0x31); + cc2500WriteReg(CC2500_2E_TEST0, 0x0B); + cc2500WriteReg(CC2500_03_FIFOTHR, 0x07); + cc2500WriteReg(CC2500_09_ADDR, 0x00); + cc2500Strobe(CC2500_SIDLE); // Go to idle... + + for(uint8_t c=0;c<0xFF;c++) + {//calibrate all channels + cc2500Strobe(CC2500_SIDLE); + cc2500WriteReg(CC2500_0A_CHANNR, c); + cc2500Strobe(CC2500_SCAL); + delayMicroseconds(900); // + calData[c][0] = cc2500ReadReg(CC2500_23_FSCAL3); + calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2); + calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1); + } + //#######END INIT######## +} + + +void frSkyXSetRcData(uint16_t *rcData, const uint8_t *packet) +{ + uint16_t c[8]; + + c[0] = (uint16_t)((packet[10] <<8)& 0xF00) | packet[9]; + c[1] = (uint16_t)((packet[11]<<4)&0xFF0) | (packet[10]>>4); + c[2] = (uint16_t)((packet[13] <<8)& 0xF00) | packet[12]; + c[3] = (uint16_t)((packet[14]<<4)&0xFF0) | (packet[13]>>4); + c[4] = (uint16_t)((packet[16] <<8)& 0xF00) | packet[15]; + c[5] = (uint16_t)((packet[17]<<4)&0xFF0) | (packet[16]>>4); + c[6] = (uint16_t)((packet[19] <<8)& 0xF00) | packet[18]; + c[7] = (uint16_t)((packet[20]<<4)&0xFF0) | (packet[19]>>4); + + uint8_t j; + for(uint8_t i=0;i<8;i++) { + if(c[i] > 2047) { + j = 8; + c[i] = c[i] - 2048; + } else { + j = 0; + } + word_temp = (((c[i]-64)<<1)/3+860); + if ((word_temp > 800) && (word_temp < 2200)) + rcData[i+j] = word_temp; + } +} + +rx_spi_received_e frSkyXDataReceived(uint8_t *packet) +{ + static unsigned receiveTelemetryRetryCount = 0; + static uint32_t polling_time=0; + + rx_spi_received_e ret = RX_SPI_RECEIVED_NONE; + + switch (protocolState) { + case STATE_INIT: + if ((millis() - start_time) > 10) { + initialize(); + + protocolState = STATE_BIND; + } + + break; + case STATE_BIND: + case STATE_BIND_TUNING: + case STATE_BIND_BINDING1: + case STATE_BIND_BINDING2: + case STATE_BIND_COMPLETE: + handleBinding(protocolState, packet); + + break; + case STATE_STARTING: + listLength = 47; + initialiseData(0); + protocolState = STATE_UPDATE; + nextChannel(1, false); // + cc2500Strobe(CC2500_SRX); + ret = RX_SPI_RECEIVED_BIND; + break; + case STATE_UPDATE: + packet_timer = micros(); + protocolState = STATE_DATA; + frame_received=false;//again set for receive + t_received = 5300; + if (checkBindRequested(false)) { + packet_timer = 0; + t_out = 50; + missingPackets = 0; + protocolState = STATE_INIT; + break; + } + // here FS code could be + case STATE_DATA: + if ((IORead(gdoPin)) &&(frame_received==false)){ + ccLen =cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; + ccLen =cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;//read 2 times to avoid reading errors + if (ccLen > 32) + ccLen = 32; + if (ccLen) { + cc2500ReadFifo(packet, ccLen); + uint16_t lcrc= crc(&packet[3],(ccLen-7)); + if((lcrc >>8)==packet[ccLen-4]&&(lcrc&0x00FF)==packet[ccLen-3]){//check crc + if (packet[0] == 0x1D) { + if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) && + (packet[2] == rxFrSkySpiConfig()->bindTxId[1]) && + (packet[6]==rxFrSkySpiConfig()->rxNum)) { + missingPackets = 0; + t_out = 1; + t_received = 0; + IOHi(frSkyLedPin); + if(one_time){ + chanskip=packet[5]<<2; + if(packet[4]data.packetSequenceId; + memcpy(&telemetryRxBuffer[remoteNewPacketId].data, &packet[22], TELEMETRY_FRAME_SIZE); + telemetryRxBuffer[remoteNewPacketId].needsProcessing = true; + + responseToSend.raw = 0; + uint8_t remoteToAckId = (remoteAckId + 1) % TELEMETRY_SEQUENCE_LENGTH; + if (remoteNewPacketId != remoteToAckId) { + while (remoteToAckId != remoteNewPacketId) { + if (!telemetryRxBuffer[remoteToAckId].needsProcessing) { + responseToSend.data.ackSequenceId = remoteToAckId; + responseToSend.data.retransmissionRequested = 1; + + receiveTelemetryRetryCount++; + + break; + } + + remoteToAckId = (remoteToAckId + 1) % TELEMETRY_SEQUENCE_LENGTH; + } + } + + if (!responseToSend.data.retransmissionRequested) { + receiveTelemetryRetryCount = 0; + + remoteToAckId = (remoteAckId + 1) % TELEMETRY_SEQUENCE_LENGTH; + uint8_t remoteNextAckId; + while (telemetryRxBuffer[remoteToAckId].needsProcessing && remoteToAckId != remoteAckId) { + remoteNextAckId = remoteToAckId; + remoteToAckId = (remoteToAckId + 1) % TELEMETRY_SEQUENCE_LENGTH; + } + remoteAckId = remoteNextAckId; + responseToSend.data.ackSequenceId = remoteAckId; + } + + if (receiveTelemetryRetryCount >= 5) { + remoteProcessedId = TELEMETRY_SEQUENCE_LENGTH - 1; + remoteAckId = TELEMETRY_SEQUENCE_LENGTH - 1; + for (unsigned i = 0; i < TELEMETRY_SEQUENCE_LENGTH; i++) { + telemetryRxBuffer[i].needsProcessing = false; + } + + receiveTelemetryRetryCount = 0; + } + + packet_timer=micros(); + frame_received=true;//no need to process frame again. + } + } + } + } + } + if (telemetryRX) { + if(cmpTimeUs(micros(), packet_timer) > t_received) { // if received or not received in this time sent telemetry data + protocolState=STATE_TELEMETRY; + telemetry_build_frame(packet); + } + } + if (cmpTimeUs(micros(), packet_timer) > t_out * SYNC) { + if (cnt++ & 0x01) { + IOLo(frSkyLedPin); + } else { + IOHi(frSkyLedPin); + } + //telemetryTime=micros(); +#if defined(USE_RX_FRSKY_SPI_TELEMETRY) + setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL); +#endif + nextChannel(1, false); + cc2500Strobe(CC2500_SRX); + protocolState = STATE_UPDATE; + } + break; +#ifdef USE_RX_FRSKY_SPI_TELEMETRY + case STATE_TELEMETRY: + if(cmpTimeUs(micros(), packet_timer) >= t_received + 400) { // if received or not received in this time sent telemetry data + cc2500Strobe(CC2500_SIDLE); + cc2500SetPower(6); + cc2500Strobe(CC2500_SFRX); + delayMicroseconds(30); +#if defined(USE_RX_FRSKY_SPI_PA_LNA) + TxEnable(); +#endif + cc2500Strobe(CC2500_SIDLE); + cc2500WriteFifo(frame, frame[0] + 1); + +#if defined(USE_TELEMETRY_SMARTPORT) + if (telemetryEnabled) { + bool clearToSend = false; + uint32_t now = millis(); + smartPortPayload_t *payload = NULL; + if ((now - polling_time) > 24) { + polling_time=now; + + clearToSend = true; + } else { + uint8_t remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH; + while (telemetryRxBuffer[remoteToProcessId].needsProcessing && !payload) { + while (remoteToProcessIndex < telemetryRxBuffer[remoteToProcessId].data.dataLength && !payload) { + payload = smartPortDataReceive(telemetryRxBuffer[remoteToProcessId].data.data[remoteToProcessIndex], &clearToSend, frSkyXCheckQueueEmpty, false); + remoteToProcessIndex = remoteToProcessIndex + 1; + } + + if (remoteToProcessIndex == telemetryRxBuffer[remoteToProcessId].data.dataLength) { + remoteToProcessIndex = 0; + telemetryRxBuffer[remoteToProcessId].needsProcessing = false; + remoteProcessedId = remoteToProcessId; + remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH; + } + } + } + processSmartPortTelemetry(payload, &clearToSend, NULL); + } +#endif + protocolState = STATE_RESUME; + ret = RX_SPI_RECEIVED_DATA; + } + + break; +#endif // USE_RX_FRSKY_SPI_TELEMETRY + case STATE_RESUME: + if (cmpTimeUs(micros(), packet_timer) > t_received + 3700) { + packet_timer = micros(); + t_received=5300; + frame_received=false;//again set for receive + nextChannel(chanskip, false); + cc2500Strobe(CC2500_SRX); +#ifdef USE_RX_FRSKY_SPI_PA_LNA + RxEnable(); +#ifdef USE_RX_FRSKY_SPI_DIVERSITY // SE4311 chip + if (missingPackets >= 2) { + if (pass & 0x01) + { + IOHi(antSelPin); + } + else + { + IOLo(antSelPin); + } + pass++; + } +#endif +#endif // USE_RX_FRSKY_SPI_PA_LNA + if (missingPackets > MAX_MISSING_PKT) + { + t_out = 50; + one_time=1; + telemetryRX=0; + protocolState = STATE_UPDATE; + break; + } + missingPackets++; + protocolState = STATE_DATA; + } + break; + } + return ret; +} + +void frSkyXInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +{ + UNUSED(rxConfig); + + rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT; + + frskySpiRxSetup(); + +#if defined(USE_TELEMETRY_SMARTPORT) + telemetryEnabled = initSmartPortTelemetryExternal(frSkyXTelemetryWriteFrame); +#endif +} + +#endif diff --git a/src/main/rx/cc2500_frsky_x.h b/src/main/rx/cc2500_frsky_x.h new file mode 100644 index 0000000000..b71ac85211 --- /dev/null +++ b/src/main/rx/cc2500_frsky_x.h @@ -0,0 +1,30 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include + +#include "rx_spi.h" + +struct rxConfig_s; +struct rxRuntimeConfig_s; +void frSkyXInit(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); +void frSkyXSetRcData(uint16_t *rcData, const uint8_t *payload); +rx_spi_received_e frSkyXDataReceived(uint8_t *payload); +void frSkyXBind(); diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c index 05d0939a69..bc05af05f3 100644 --- a/src/main/rx/fport.c +++ b/src/main/rx/fport.c @@ -310,7 +310,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) #if defined(USE_TELEMETRY_SMARTPORT) } else { timeUs_t currentTimeUs = micros(); - if (clearToSend && cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) { + if (telemetryEnabled && clearToSend && cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) { if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) { clearToSend = false; } diff --git a/src/main/rx/frsky_d.c b/src/main/rx/frsky_d.c deleted file mode 100644 index 61eb79b567..0000000000 --- a/src/main/rx/frsky_d.c +++ /dev/null @@ -1,738 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include -#include -#include - -#include "platform.h" - -#ifdef USE_RX_FRSKY_D - -#include "build/build_config.h" -#include "build/debug.h" - -#include "common/maths.h" -#include "common/utils.h" - -#include "drivers/cc2500.h" -#include "drivers/io.h" -#include "drivers/system.h" -#include "drivers/time.h" - -#include "fc/config.h" - -#include "config/feature.h" -#include "config/parameter_group_ids.h" - -#include "rx/rx.h" -#include "rx/rx_spi.h" -#include "rx/frsky_d.h" - -#include "sensors/battery.h" - -#include "telemetry/frsky.h" - -#define RC_CHANNEL_COUNT 8 -#define MAX_MISSING_PKT 100 - -#define DEBUG_DATA_ERROR_COUNT 0 - -#define SYNC 9000 -#define FS_THR 960 - -static uint32_t missingPackets; -static uint8_t calData[255][3]; -static uint32_t time_tune; -static uint8_t listLength; -static uint8_t bindIdx; -static uint8_t cnt; -static int32_t t_out; -static uint8_t Lqi; -static timeUs_t lastPacketReceivedTime; -static uint8_t protocolState; -static uint32_t start_time; -static int8_t bindOffset; -static uint16_t dataErrorCount = 0; - -static IO_t gdoPin; -static IO_t bindPin = DEFIO_IO(NONE); -static bool lastBindPinStatus; -static IO_t frSkyLedPin; -#if defined(USE_FRSKY_RX_PA_LNA) -static IO_t txEnPin; -static IO_t rxEnPin; -static IO_t antSelPin; -static uint8_t pass; -#endif -bool bindRequested = false; - -#ifdef USE_FRSKY_D_TELEMETRY -static uint8_t frame[20]; -static int16_t RSSI_dBm; -static uint8_t telemetry_id; -static uint32_t telemetryTime; - -#if defined(USE_TELEMETRY_FRSKY) -#define MAX_SERIAL_BYTES 64 -static uint8_t hub_index; -static uint8_t idxx = 0; -static uint8_t idx_ok = 0; -static uint8_t telemetry_expected_id = 0; -static uint8_t srx_data[MAX_SERIAL_BYTES]; // buffer for telemetry serial data -#endif -#endif - -PG_REGISTER_WITH_RESET_TEMPLATE(frSkyDConfig_t, frSkyDConfig, PG_FRSKY_D_CONFIG, - 0); - -PG_RESET_TEMPLATE(frSkyDConfig_t, frSkyDConfig, - .autoBind = false, - .bindTxId = {0, 0}, - .bindOffset = 0, - .bindHopData = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} -); - -enum { - STATE_INIT = 0, - STATE_BIND, - STATE_BIND_TUNING, - STATE_BIND_BINDING1, - STATE_BIND_BINDING2, - STATE_BIND_COMPLETE, - STATE_STARTING, - STATE_UPDATE, - STATE_DATA, - STATE_TELEMETRY -}; - -#if defined(USE_FRSKY_D_TELEMETRY) -static void compute_RSSIdbm(uint8_t *packet) -{ - if (packet[18] >= 128) { - RSSI_dBm = ((((uint16_t)packet[18]) * 18) >> 5) - 82; - } else { - RSSI_dBm = ((((uint16_t)packet[18]) * 18) >> 5) + 65; - } - - setRssiUnfiltered(constrain(RSSI_dBm << 3, 0, 1024), RSSI_SOURCE_RX_PROTOCOL); -} - -#if defined(USE_TELEMETRY_FRSKY) -static uint8_t frsky_append_hub_data(uint8_t *buf) -{ - if (telemetry_id == telemetry_expected_id) - idx_ok = idxx; - else // rx re-requests last packet - idxx = idx_ok; - - telemetry_expected_id = (telemetry_id + 1) & 0x1F; - uint8_t index = 0; - for (uint8_t i = 0; i < 10; i++) { - if (idxx == hub_index) { - break; - } - buf[i] = srx_data[idxx]; - idxx = (idxx + 1) & (MAX_SERIAL_BYTES - 1); - index++; - } - return index; -} - -static void frSkyTelemetryInitFrameSpi(void) -{ - hub_index = 0; - idxx = 0; -} - -static void frSkyTelemetryWriteSpi(uint8_t ch) -{ - if (hub_index < MAX_SERIAL_BYTES) { - srx_data[hub_index++] = ch; - } -} -#endif - -static void telemetry_build_frame(uint8_t *packet) -{ - const uint16_t adcExternal1Sample = adcGetChannel(ADC_EXTERNAL1); - const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); - uint8_t bytes_used = 0; - telemetry_id = packet[4]; - frame[0] = 0x11; // length - frame[1] = frSkyDConfig()->bindTxId[0]; - frame[2] = frSkyDConfig()->bindTxId[1]; - frame[3] = (uint8_t)((adcExternal1Sample & 0xff0) >> 4); // A1 - frame[4] = (uint8_t)((adcRssiSample & 0xff0) >> 4); // A2 - frame[5] = (uint8_t)RSSI_dBm; -#if defined(USE_TELEMETRY_FRSKY) - bytes_used = frsky_append_hub_data(&frame[8]); -#endif - frame[6] = bytes_used; - frame[7] = telemetry_id; -} - -#endif - -#if defined(USE_FRSKY_RX_PA_LNA) -static void RX_enable(void) -{ - IOLo(txEnPin); - IOHi(rxEnPin); -} - -static void TX_enable(void) -{ - IOLo(rxEnPin); - IOHi(txEnPin); -} -#endif - -void frSkyDBind(void) -{ - bindRequested = true; -} - -static void initialize(void) -{ - cc2500Reset(); - cc2500WriteReg(CC2500_02_IOCFG0, 0x01); - cc2500WriteReg(CC2500_17_MCSM1, 0x0C); - cc2500WriteReg(CC2500_18_MCSM0, 0x18); - cc2500WriteReg(CC2500_06_PKTLEN, 0x19); - cc2500WriteReg(CC2500_08_PKTCTRL0, 0x05); - cc2500WriteReg(CC2500_3E_PATABLE, 0xFF); - cc2500WriteReg(CC2500_0B_FSCTRL1, 0x08); - cc2500WriteReg(CC2500_0C_FSCTRL0, 0x00); - cc2500WriteReg(CC2500_0D_FREQ2, 0x5C); - cc2500WriteReg(CC2500_0E_FREQ1, 0x76); - cc2500WriteReg(CC2500_0F_FREQ0, 0x27); - cc2500WriteReg(CC2500_10_MDMCFG4, 0xAA); - cc2500WriteReg(CC2500_11_MDMCFG3, 0x39); - cc2500WriteReg(CC2500_12_MDMCFG2, 0x11); - cc2500WriteReg(CC2500_13_MDMCFG1, 0x23); - cc2500WriteReg(CC2500_14_MDMCFG0, 0x7A); - cc2500WriteReg(CC2500_15_DEVIATN, 0x42); - cc2500WriteReg(CC2500_19_FOCCFG, 0x16); - cc2500WriteReg(CC2500_1A_BSCFG, 0x6C); - cc2500WriteReg(CC2500_1B_AGCCTRL2, 0x03); - cc2500WriteReg(CC2500_1C_AGCCTRL1, 0x40); - cc2500WriteReg(CC2500_1D_AGCCTRL0, 0x91); - cc2500WriteReg(CC2500_21_FREND1, 0x56); - cc2500WriteReg(CC2500_22_FREND0, 0x10); - cc2500WriteReg(CC2500_23_FSCAL3, 0xA9); - cc2500WriteReg(CC2500_24_FSCAL2, 0x0A); - cc2500WriteReg(CC2500_25_FSCAL1, 0x00); - cc2500WriteReg(CC2500_26_FSCAL0, 0x11); - cc2500WriteReg(CC2500_29_FSTEST, 0x59); - cc2500WriteReg(CC2500_2C_TEST2, 0x88); - cc2500WriteReg(CC2500_2D_TEST1, 0x31); - cc2500WriteReg(CC2500_2E_TEST0, 0x0B); - cc2500WriteReg(CC2500_03_FIFOTHR, 0x07); - cc2500WriteReg(CC2500_09_ADDR, 0x00); - cc2500Strobe(CC2500_SIDLE); - - cc2500WriteReg(CC2500_07_PKTCTRL1, 0x04); - cc2500WriteReg(CC2500_0C_FSCTRL0, 0); - for (uint8_t c = 0; c < 0xFF; c++) { - cc2500Strobe(CC2500_SIDLE); - cc2500WriteReg(CC2500_0A_CHANNR, c); - cc2500Strobe(CC2500_SCAL); - delayMicroseconds(900); - calData[c][0] = cc2500ReadReg(CC2500_23_FSCAL3); - calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2); - calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1); - } -} - -static void initialize_data(uint8_t adr) -{ - cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)frSkyDConfig()->bindOffset); - cc2500WriteReg(CC2500_18_MCSM0, 0x8); - cc2500WriteReg(CC2500_09_ADDR, adr ? 0x03 : frSkyDConfig()->bindTxId[0]); - cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0D); - cc2500WriteReg(CC2500_19_FOCCFG, 0x16); - delay(10); -} - -static void initTuneRx(void) -{ - cc2500WriteReg(CC2500_19_FOCCFG, 0x14); - time_tune = millis(); - bindOffset = -126; - cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset); - cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0C); - cc2500WriteReg(CC2500_18_MCSM0, 0x8); - - cc2500Strobe(CC2500_SIDLE); - cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]); - cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]); - cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]); - cc2500WriteReg(CC2500_0A_CHANNR, 0); - cc2500Strobe(CC2500_SFRX); - cc2500Strobe(CC2500_SRX); -} - -static bool tuneRx(uint8_t *packet) -{ - if (bindOffset >= 126) { - bindOffset = -126; - } - if ((millis() - time_tune) > 50) { - time_tune = millis(); - bindOffset += 5; - cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset); - } - if (IORead(gdoPin)) { - uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; - if (ccLen) { - cc2500ReadFifo(packet, ccLen); - if (packet[ccLen - 1] & 0x80) { - if (packet[2] == 0x01) { - Lqi = packet[ccLen - 1] & 0x7F; - if (Lqi < 50) { - frSkyDConfigMutable()->bindOffset = bindOffset; - - return true; - } - } - } - } - } - - return false; -} - -static void initGetBind(void) -{ - cc2500Strobe(CC2500_SIDLE); - cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]); - cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]); - cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]); - cc2500WriteReg(CC2500_0A_CHANNR, 0); - cc2500Strobe(CC2500_SFRX); - delayMicroseconds(20); // waiting flush FIFO - - cc2500Strobe(CC2500_SRX); - listLength = 0; - bindIdx = 0x05; -} - -static bool getBind1(uint8_t *packet) -{ - // len|bind |tx - // id|03|01|idx|h0|h1|h2|h3|h4|00|00|00|00|00|00|00|00|00|00|00|00|00|00|00|CHK1|CHK2|RSSI|LQI/CRC| - // Start by getting bind packet 0 and the txid - if (IORead(gdoPin)) { - uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; - if (ccLen) { - cc2500ReadFifo(packet, ccLen); - if (packet[ccLen - 1] & 0x80) { - if (packet[2] == 0x01) { - if (packet[5] == 0x00) { - frSkyDConfigMutable()->bindTxId[0] = packet[3]; - frSkyDConfigMutable()->bindTxId[1] = packet[4]; - for (uint8_t n = 0; n < 5; n++) { - frSkyDConfigMutable()->bindHopData[packet[5] + n] = - packet[6 + n]; - } - - return true; - } - } - } - } - } - - return false; -} - -static bool getBind2(uint8_t *packet) -{ - if (bindIdx <= 120) { - if (IORead(gdoPin)) { - uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; - if (ccLen) { - cc2500ReadFifo(packet, ccLen); - if (packet[ccLen - 1] & 0x80) { - if (packet[2] == 0x01) { - if ((packet[3] == frSkyDConfig()->bindTxId[0]) && - (packet[4] == frSkyDConfig()->bindTxId[1])) { - if (packet[5] == bindIdx) { -#if defined(DJTS) - if (packet[5] == 0x2D) { - for (uint8_t i = 0; i < 2; i++) { - frSkyDConfigMutable() - ->bindHopData[packet[5] + i] = - packet[6 + i]; - } - listLength = 47; - - return true; - } -#endif - for (uint8_t n = 0; n < 5; n++) { - if (packet[6 + n] == packet[ccLen - 3] || - (packet[6 + n] == 0)) { - if (bindIdx >= 0x2D) { - listLength = packet[5] + n; - - return true; - } - } - - frSkyDConfigMutable()->bindHopData[packet[5] + n] = packet[6 + n]; - } - - bindIdx = bindIdx + 5; - - return false; - } - } - } - } - } - } - - return false; - } else { - return true; - } - -} - -static void nextChannel(uint8_t skip) -{ - static uint8_t channr = 0; - - channr += skip; - while (channr >= listLength) { - channr -= listLength; - } - cc2500Strobe(CC2500_SIDLE); - cc2500WriteReg(CC2500_23_FSCAL3, - calData[frSkyDConfig()->bindHopData[channr]][0]); - cc2500WriteReg(CC2500_24_FSCAL2, - calData[frSkyDConfig()->bindHopData[channr]][1]); - cc2500WriteReg(CC2500_25_FSCAL1, - calData[frSkyDConfig()->bindHopData[channr]][2]); - cc2500WriteReg(CC2500_0A_CHANNR, frSkyDConfig()->bindHopData[channr]); - cc2500Strobe(CC2500_SFRX); -} - -static bool checkBindRequested(bool reset) -{ - if (bindPin) { - bool bindPinStatus = IORead(bindPin); - if (lastBindPinStatus && !bindPinStatus) { - bindRequested = true; - } - lastBindPinStatus = bindPinStatus; - } - - if (!bindRequested) { - return false; - } else { - if (reset) { - bindRequested = false; - } - - return true; - } -} - -#define FRSKY_D_CHANNEL_SCALING (2.0f / 3) - -static void decodeChannelPair(uint16_t *channels, const uint8_t *packet, const uint8_t highNibbleOffset) { - channels[0] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf) << 8 | packet[0]); - channels[1] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf0) << 4 | packet[1]); -} - -void frSkyDSetRcData(uint16_t *rcData, const uint8_t *packet) -{ - uint16_t channels[RC_CHANNEL_COUNT]; - bool dataError = false; - - decodeChannelPair(channels, packet + 6, 4); - decodeChannelPair(channels + 2, packet + 8, 3); - decodeChannelPair(channels + 4, packet + 12, 4); - decodeChannelPair(channels + 6, packet + 14, 3); - - for (int i = 0; i < RC_CHANNEL_COUNT; i++) { - if ((channels[i] < 800) || (channels[i] > 2200)) { - dataError = true; - - break; - } - } - - if (!dataError) { - for (int i = 0; i < RC_CHANNEL_COUNT; i++) { - rcData[i] = channels[i]; - } - } else { - DEBUG_SET(DEBUG_FRSKY_D_RX, DEBUG_DATA_ERROR_COUNT, ++dataErrorCount); - } -} - -rx_spi_received_e frSkyDDataReceived(uint8_t *packet) -{ - const timeUs_t currentPacketReceivedTime = micros(); - - rx_spi_received_e ret = RX_SPI_RECEIVED_NONE; - - switch (protocolState) { - case STATE_INIT: - if ((millis() - start_time) > 10) { - initialize(); - - protocolState = STATE_BIND; - } - - break; - case STATE_BIND: - if (checkBindRequested(true) || frSkyDConfig()->autoBind) { - IOHi(frSkyLedPin); - initTuneRx(); - - protocolState = STATE_BIND_TUNING; - } else { - protocolState = STATE_STARTING; - } - - break; - case STATE_BIND_TUNING: - if (tuneRx(packet)) { - initGetBind(); - initialize_data(1); - - protocolState = STATE_BIND_BINDING1; - } - - break; - case STATE_BIND_BINDING1: - if (getBind1(packet)) { - protocolState = STATE_BIND_BINDING2; - } - - break; - case STATE_BIND_BINDING2: - if (getBind2(packet)) { - cc2500Strobe(CC2500_SIDLE); - - protocolState = STATE_BIND_COMPLETE; - } - - break; - case STATE_BIND_COMPLETE: - if (!frSkyDConfig()->autoBind) { - writeEEPROM(); - } else { - uint8_t ctr = 40; - while (ctr--) { - IOHi(frSkyLedPin); - delay(50); - IOLo(frSkyLedPin); - delay(50); - } - } - - protocolState = STATE_STARTING; - - break; - case STATE_STARTING: - listLength = 47; - initialize_data(0); - protocolState = STATE_UPDATE; - nextChannel(1); // - cc2500Strobe(CC2500_SRX); - ret = RX_SPI_RECEIVED_BIND; - - break; - case STATE_UPDATE: - lastPacketReceivedTime = currentPacketReceivedTime; - protocolState = STATE_DATA; - - if (checkBindRequested(false)) { - lastPacketReceivedTime = 0; - t_out = 50; - missingPackets = 0; - - protocolState = STATE_INIT; - - break; - } - // here FS code could be - case STATE_DATA: - if (IORead(gdoPin)) { - uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; - if (ccLen >= 20) { - cc2500ReadFifo(packet, 20); - if (packet[19] & 0x80) { - missingPackets = 0; - t_out = 1; - if (packet[0] == 0x11) { - if ((packet[1] == frSkyDConfig()->bindTxId[0]) && - (packet[2] == frSkyDConfig()->bindTxId[1])) { - IOHi(frSkyLedPin); - nextChannel(1); -#ifdef USE_FRSKY_D_TELEMETRY - if ((packet[3] % 4) == 2) { - telemetryTime = micros(); - compute_RSSIdbm(packet); - telemetry_build_frame(packet); - protocolState = STATE_TELEMETRY; - } else -#endif - { - cc2500Strobe(CC2500_SRX); - protocolState = STATE_UPDATE; - } - ret = RX_SPI_RECEIVED_DATA; - lastPacketReceivedTime = currentPacketReceivedTime; - } - } - } - } - } - - if (cmpTimeUs(currentPacketReceivedTime, lastPacketReceivedTime) > (t_out * SYNC)) { -#ifdef USE_FRSKY_RX_PA_LNA - RX_enable(); -#endif - if (t_out == 1) { -#ifdef USE_FRSKY_RX_DIVERSITY // SE4311 chip - if (missingPackets >= 2) { - if (pass & 0x01) { - IOHi(antSelPin); - } else { - IOLo(antSelPin); - } - pass++; - } -#endif - - if (missingPackets > MAX_MISSING_PKT) - t_out = 50; - - missingPackets++; - nextChannel(1); - } else { - if (cnt++ & 0x01) { - IOLo(frSkyLedPin); - } else - IOHi(frSkyLedPin); - - nextChannel(13); - } - - cc2500Strobe(CC2500_SRX); - protocolState = STATE_UPDATE; - } - break; -#ifdef USE_FRSKY_D_TELEMETRY - case STATE_TELEMETRY: - if ((micros() - telemetryTime) >= 1380) { - cc2500Strobe(CC2500_SIDLE); - cc2500SetPower(6); - cc2500Strobe(CC2500_SFRX); -#if defined(USE_FRSKY_RX_PA_LNA) - TX_enable(); -#endif - cc2500Strobe(CC2500_SIDLE); - cc2500WriteFifo(frame, frame[0] + 1); - protocolState = STATE_DATA; - ret = RX_SPI_RECEIVED_DATA; - lastPacketReceivedTime = currentPacketReceivedTime; - } - - break; - -#endif - } - return ret; -} - -static void frskyD_Rx_Setup(rx_spi_protocol_e protocol) -{ - UNUSED(protocol); - // gpio init here - gdoPin = IOGetByTag(IO_TAG(FRSKY_RX_GDO_0_PIN)); - IOInit(gdoPin, OWNER_RX_BIND, 0); - IOConfigGPIO(gdoPin, IOCFG_IN_FLOATING); - frSkyLedPin = IOGetByTag(IO_TAG(FRSKY_RX_LED_PIN)); - IOInit(frSkyLedPin, OWNER_LED, 0); - IOConfigGPIO(frSkyLedPin, IOCFG_OUT_PP); -#if defined(USE_FRSKY_RX_PA_LNA) - rxEnPin = IOGetByTag(IO_TAG(FRSKY_RX_RX_EN_PIN)); - IOInit(rxEnPin, OWNER_RX_BIND, 0); - IOConfigGPIO(rxEnPin, IOCFG_OUT_PP); - txEnPin = IOGetByTag(IO_TAG(FRSKY_RX_TX_EN_PIN)); - IOInit(txEnPin, OWNER_RX_BIND, 0); - IOConfigGPIO(txEnPin, IOCFG_OUT_PP); -#endif -#if defined(USE_FRSKY_RX_DIVERSITY) - antSelPin = IOGetByTag(IO_TAG(FRSKY_RX_ANT_SEL_PIN)); - IOInit(antSelPin, OWNER_RX_BIND, 0); - IOConfigGPIO(antSelPin, IOCFG_OUT_PP); -#endif -#if defined(BINDPLUG_PIN) - bindPin = IOGetByTag(IO_TAG(BINDPLUG_PIN)); - IOInit(bindPin, OWNER_RX_BIND, 0); - IOConfigGPIO(bindPin, IOCFG_IPU); - - lastBindPinStatus = IORead(bindPin); -#endif - - start_time = millis(); - lastPacketReceivedTime = 0; - t_out = 50; - missingPackets = 0; - protocolState = STATE_INIT; -#if defined(USE_FRSKY_RX_DIVERSITY) - IOHi(antSelPin); -#endif -#if defined(USE_FRSKY_RX_PA_LNA) - RX_enable(); -#endif - -#if defined(USE_FRSKY_D_TELEMETRY) -#if defined(USE_TELEMETRY_FRSKY) - initFrSkyExternalTelemetry(&frSkyTelemetryInitFrameSpi, - &frSkyTelemetryWriteSpi); -#endif - - if (rssiSource == RSSI_SOURCE_NONE) { - rssiSource = RSSI_SOURCE_RX_PROTOCOL; - } -#endif - - // if(!frSkySpiDetect())//detect spi working routine - // return; -} - -void frSkyDInit(const rxConfig_t *rxConfig, - rxRuntimeConfig_t *rxRuntimeConfig) -{ - rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT; - frskyD_Rx_Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol); -} -#endif diff --git a/src/main/rx/rx_spi.c b/src/main/rx/rx_spi.c index a03e1aa2df..492aed20a6 100644 --- a/src/main/rx/rx_spi.c +++ b/src/main/rx/rx_spi.c @@ -35,7 +35,8 @@ #include "rx/rx.h" #include "rx/rx_spi.h" -#include "rx/frsky_d.h" +#include "cc2500_frsky_d.h" +#include "cc2500_frsky_x.h" #include "rx/nrf24_cx10.h" #include "rx/nrf24_syma.h" #include "rx/nrf24_v202.h" @@ -113,13 +114,20 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol) protocolSetRcDataFromPayload = inavNrf24SetRcDataFromPayload; break; #endif -#ifdef USE_RX_FRSKY_D +#ifdef USE_RX_FRSKY_SPI_D case RX_SPI_FRSKY_D: protocolInit = frSkyDInit; protocolDataReceived = frSkyDDataReceived; protocolSetRcDataFromPayload = frSkyDSetRcData; break; #endif +#ifdef USE_RX_FRSKY_SPI_X + case RX_SPI_FRSKY_X: + protocolInit = frSkyXInit; + protocolDataReceived = frSkyXDataReceived; + protocolSetRcDataFromPayload = frSkyXSetRcData; + break; +#endif #ifdef USE_RX_FLYSKY case RX_SPI_A7105_FLYSKY: case RX_SPI_A7105_FLYSKY_2A: diff --git a/src/main/rx/rx_spi.h b/src/main/rx/rx_spi.h index 1d33dd7b5e..fe9a014245 100644 --- a/src/main/rx/rx_spi.h +++ b/src/main/rx/rx_spi.h @@ -30,6 +30,7 @@ typedef enum { RX_SPI_NRF24_H8_3D, RX_SPI_NRF24_INAV, RX_SPI_FRSKY_D, + RX_SPI_FRSKY_X, RX_SPI_A7105_FLYSKY, RX_SPI_A7105_FLYSKY_2A, RX_SPI_PROTOCOL_COUNT diff --git a/src/main/target/MIDELICF3/target.h b/src/main/target/MIDELICF3/target.h index 99a69a231d..fd4c832dd8 100644 --- a/src/main/target/MIDELICF3/target.h +++ b/src/main/target/MIDELICF3/target.h @@ -99,27 +99,35 @@ #define RX_SPI_INSTANCE SPI1 #define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA -#define USE_RX_FRSKY_D -#define DEFAULT_RX_FEATURE FEATURE_RX_SPI -#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_FRSKY_D -#define USE_FRSKY_D_TELEMETRY +#define USE_RX_FRSKY_SPI_D +#define USE_RX_FRSKY_SPI_X +#define DEFAULT_RX_FEATURE FEATURE_RX_SPI +#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_FRSKY_X +#define USE_RX_FRSKY_SPI_TELEMETRY -#define USE_FRSKY_RX_PA_LNA -#define USE_FRSKY_RX_DIVERSITY +#define RX_NSS_PIN SPI1_NSS_PIN +#define RX_SCK_PIN SPI1_SCK_PIN +#define RX_MISO_PIN SPI1_MISO_PIN +#define RX_MOSI_PIN SPI1_MOSI_PIN -#define RX_NSS_PIN SPI1_NSS_PIN -#define RX_SCK_PIN SPI1_SCK_PIN -#define RX_MISO_PIN SPI1_MISO_PIN -#define RX_MOSI_PIN SPI1_MOSI_PIN +#define RX_FRSKY_SPI_GDO_0_PIN PB0 -#define FRSKY_RX_GDO_0_PIN PB0 -#define FRSKY_RX_ANT_SEL_PIN PB2 -#define FRSKY_RX_TX_EN_PIN PB1 -#define FRSKY_RX_RX_EN_PIN PB11 -#define FRSKY_RX_LED_PIN PB6 +#define RX_FRSKY_SPI_LED_PIN PB6 -#define BINDPLUG_PIN PC13 + +#define USE_RX_FRSKY_SPI_PA_LNA + +#define RX_FRSKY_SPI_TX_EN_PIN PB1 +#define RX_FRSKY_SPI_LNA_EN_PIN PB11 + + +#define USE_RX_FRSKY_SPI_DIVERSITY + +#define RX_FRSKY_SPI_ANT_SEL_PIN PB2 + + +#define BINDPLUG_PIN PC13 #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_ESCSERIAL diff --git a/src/main/target/MIDELICF3/target.mk b/src/main/target/MIDELICF3/target.mk index dfdcf239e4..fd5cb6e26a 100644 --- a/src/main/target/MIDELICF3/target.mk +++ b/src/main/target/MIDELICF3/target.mk @@ -6,4 +6,6 @@ TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_mpu6050.c \ drivers/cc2500.c \ - rx/frsky_d.c + rx/cc2500_frsky_shared.c \ + rx/cc2500_frsky_d.c \ + rx/cc2500_frsky_x.c diff --git a/src/main/target/common_fc_post.h b/src/main/target/common_fc_post.h index ac7cb26ebb..aa9bb466e4 100644 --- a/src/main/target/common_fc_post.h +++ b/src/main/target/common_fc_post.h @@ -73,3 +73,8 @@ #undef VTX_TRAMP #undef VTX_SMARTAUDIO #endif + +#if defined(USE_RX_FRSKY_SPI_D) || defined(USE_RX_FRSKY_SPI_X) +#define USE_RX_FRSKY_SPI +#endif + diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c index 135cc91722..db4eab7757 100644 --- a/src/main/telemetry/msp_shared.c +++ b/src/main/telemetry/msp_shared.c @@ -132,6 +132,7 @@ bool handleMspFrame(uint8_t *frameStart, int frameLength) sbufAdvance(frameBuf, frameBytesRemaining); sbufWriteData(rxBuf, payload, frameBytesRemaining); lastSeq = seqNumber; + return false; } else { sbufReadData(frameBuf, payload, bufferBytesRemaining); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index dea2d4aa4c..828db52785 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -10,7 +10,7 @@ #include "platform.h" -#ifdef USE_TELEMETRY +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT) #include "common/axis.h" #include "common/color.h" @@ -65,27 +65,6 @@ enum SPSTATE_INITIALIZED_EXTERNAL, }; -enum -{ - FSSP_START_STOP = 0x7E, - - FSSP_DLE = 0x7D, - FSSP_DLE_XOR = 0x20, - - FSSP_DATA_FRAME = 0x10, - FSSP_MSPC_FRAME_SMARTPORT = 0x30, // MSP client frame - FSSP_MSPC_FRAME_FPORT = 0x31, // MSP client frame - FSSP_MSPS_FRAME = 0x32, // MSP server frame - - // ID of sensor. Must be something that is polled by FrSky RX - FSSP_SENSOR_ID1 = 0x1B, - FSSP_SENSOR_ID2 = 0x0D, - FSSP_SENSOR_ID3 = 0x34, - FSSP_SENSOR_ID4 = 0x67 - // there are 32 ID's polled by smartport master - // remaining 3 bits are crc (according to comments in openTx code) -}; - // these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky.h enum { @@ -166,7 +145,7 @@ static smartPortWriteFrameFn *smartPortWriteFrame; static bool smartPortMspReplyPending = false; #endif -static smartPortPayload_t *smartPortDataReceiveSerial(uint16_t c, bool *clearToSend) +smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool useChecksum) { static uint8_t rxBuffer[sizeof(smartPortPayload_t)]; static uint8_t smartPortRxBytes = 0; @@ -188,7 +167,7 @@ static smartPortPayload_t *smartPortDataReceiveSerial(uint16_t c, bool *clearToS if (awaitingSensorId) { awaitingSensorId = false; - if ((c == FSSP_SENSOR_ID1) && (serialRxBytesWaiting(smartPortSerialPort) == 0)) { + if ((c == FSSP_SENSOR_ID1) && checkQueueEmpty()) { // our slot is starting, no need to decode more *clearToSend = true; skipUntilStart = true; @@ -210,6 +189,12 @@ static smartPortPayload_t *smartPortDataReceiveSerial(uint16_t c, bool *clearToS if (smartPortRxBytes < sizeof(smartPortPayload_t)) { rxBuffer[smartPortRxBytes++] = (uint8_t)c; checksum += c; + + if (!useChecksum && (smartPortRxBytes == sizeof(smartPortPayload_t))) { + skipUntilStart = true; + + return (smartPortPayload_t *)&rxBuffer; + } } else { skipUntilStart = true; @@ -386,17 +371,6 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear static uint8_t t2Cnt = 0; switch (id) { -#ifdef USE_GPS - case FSSP_DATAID_SPEED : - if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { - //convert to knots: 1cm/s = 0.0194384449 knots - //Speed should be sent in knots/1000 (GPS speed is in cm/s) - uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100; - smartPortSendPackage(id, tmpui); - *clearToSend = false; - } - break; -#endif case FSSP_DATAID_VFAS : if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && getBatteryCellCount() > 0) { uint16_t vfasVoltage; @@ -430,28 +404,6 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear break; //case FSSP_DATAID_ADC1 : //case FSSP_DATAID_ADC2 : -#ifdef USE_GPS - case FSSP_DATAID_LATLONG : - if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { - uint32_t tmpui = 0; - // the same ID is sent twice, one for longitude, one for latitude - // the MSB of the sent uint32_t helps FrSky keep track - // the even/odd bit of our counter helps us keep track - if (smartPortIdCnt & 1) { - tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare - tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast - if (gpsSol.llh.lon < 0) tmpui |= 0x40000000; - } - else { - tmpui = abs(gpsSol.llh.lat); // now we have unsigned value and one bit to spare - tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast - if (gpsSol.llh.lat < 0) tmpui |= 0x40000000; - } - smartPortSendPackage(id, tmpui); - *clearToSend = false; - } - break; -#endif //case FSSP_DATAID_CAP_USED : case FSSP_DATAID_VARIO : if (sensors(SENSOR_BARO)) { @@ -564,6 +516,35 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear } break; #ifdef USE_GPS + case FSSP_DATAID_SPEED : + if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { + //convert to knots: 1cm/s = 0.0194384449 knots + //Speed should be sent in knots/1000 (GPS speed is in cm/s) + uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100; + smartPortSendPackage(id, tmpui); + *clearToSend = false; + } + break; + case FSSP_DATAID_LATLONG : + if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { + uint32_t tmpui = 0; + // the same ID is sent twice, one for longitude, one for latitude + // the MSB of the sent uint32_t helps FrSky keep track + // the even/odd bit of our counter helps us keep track + if (smartPortIdCnt & 1) { + tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare + tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast + if (gpsSol.llh.lon < 0) tmpui |= 0x40000000; + } + else { + tmpui = abs(gpsSol.llh.lat); // now we have unsigned value and one bit to spare + tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast + if (gpsSol.llh.lat < 0) tmpui |= 0x40000000; + } + smartPortSendPackage(id, tmpui); + *clearToSend = false; + } + break; case FSSP_DATAID_GPS_ALT : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { smartPortSendPackage(id, gpsSol.llh.alt * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7) @@ -584,6 +565,11 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear } } +static bool serialCheckQueueEmpty(void) +{ + return (serialRxBytesWaiting(smartPortSerialPort) == 0); +} + void handleSmartPortTelemetry(void) { static bool clearToSend = false; @@ -597,7 +583,7 @@ void handleSmartPortTelemetry(void) smartPortPayload_t *payload = NULL; while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) { uint8_t c = serialRead(smartPortSerialPort); - payload = smartPortDataReceiveSerial(c, &clearToSend); + payload = smartPortDataReceive(c, &clearToSend, serialCheckQueueEmpty, true); } processSmartPortTelemetry(payload, &clearToSend, &requestTimeout); diff --git a/src/main/telemetry/smartport.h b/src/main/telemetry/smartport.h index 0b258efeba..a5e6370337 100644 --- a/src/main/telemetry/smartport.h +++ b/src/main/telemetry/smartport.h @@ -13,6 +13,27 @@ #define SMARTPORT_MSP_TX_BUF_SIZE 256 #define SMARTPORT_MSP_RX_BUF_SIZE 64 +enum +{ + FSSP_START_STOP = 0x7E, + + FSSP_DLE = 0x7D, + FSSP_DLE_XOR = 0x20, + + FSSP_DATA_FRAME = 0x10, + FSSP_MSPC_FRAME_SMARTPORT = 0x30, // MSP client frame + FSSP_MSPC_FRAME_FPORT = 0x31, // MSP client frame + FSSP_MSPS_FRAME = 0x32, // MSP server frame + + // ID of sensor. Must be something that is polled by FrSky RX + FSSP_SENSOR_ID1 = 0x1B, + FSSP_SENSOR_ID2 = 0x0D, + FSSP_SENSOR_ID3 = 0x34, + FSSP_SENSOR_ID4 = 0x67 + // there are 32 ID's polled by smartport master + // remaining 3 bits are crc (according to comments in openTx code) +}; + typedef struct smartPortPayload_s { uint8_t frameId; uint16_t valueId; @@ -20,6 +41,7 @@ typedef struct smartPortPayload_s { } __attribute__((packed)) smartPortPayload_t; typedef void smartPortWriteFrameFn(const smartPortPayload_t *payload); +typedef bool smartPortCheckQueueEmptyFn(void); bool initSmartPortTelemetry(void); void checkSmartPortTelemetryState(void); @@ -28,6 +50,8 @@ bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameEx void handleSmartPortTelemetry(void); void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *hasRequest, const uint32_t *requestTimeout); +smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool withChecksum); + struct serialPort_s; void smartPortWriteFrameSerial(const smartPortPayload_t *payload, struct serialPort_s *port, uint16_t checksum); void smartPortSendByte(uint8_t c, uint16_t *checksum, struct serialPort_s *port);