123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477 |
- /*
- * FrSky RX 2-way protocol
- */
-
- #include <stdint.h>
- #include <avr/io.h>
- #include <avr/interrupt.h>
- #include <avr/eeprom.h>
- #include <avr/wdt.h>
-
- #include "cc2500.h"
- #include "cppm.h"
- #include "spi.h"
- #include "timer.h"
- #include "rx.h"
- #include "main.h"
-
- #define CHANNELS 8
- #define PPM_MIN 1000
- #define PPM_MAX 2000
- #define HOP_DATA_LENGTH 60
- #define EEPROM_BASE_ADDRESS 100
- #define MISSING_PACKET_DELAY 9
- #define SEEK_CHANNEL_SKIP 13
- #define MAX_MISSING_PACKET 20
- #define FAILSAFE_MISSING_PACKET 170
- #define RSSI_OVER_PPM 7
- #define RSSI_OFFSET 71
- #define RSSI_MIN -103
- #define RSSI_MAX -96
- #define PPM_SCALE 0.67
-
- uint8_t ccData[27];
- uint8_t ccLen;
- uint8_t packet = 0;
- uint8_t channr = 0;
- uint8_t missingPackets = 0;
- uint8_t hopData[HOP_DATA_LENGTH];
- uint8_t listLength;
- uint8_t txid[2];
- uint8_t counter = 0;
- uint8_t failed = 0;
- uint8_t frequencyOffsetHack = 0;
- uint16_t c[8];
- int rssi;
-
- // original 0x5C 0x76 0x27
- uint8_t freq2 = 0x5C, freq1 = 0x70, freq0 = 0x00;
-
- static uint16_t ppmBuffer[CHANNELS];
-
- static long map(long x, long in_min, long in_max, long out_min, long out_max);
- static long constrain(long x, long min, long max);
-
- static void initialize(uint8_t bind);
- static void binding(void);
- static void tuning(void);
- static void performBind(void);
- static void nextChannel(uint8_t skip);
- static void readBindingData(void);
- static void writeBindingData(void);
-
- void rxInit(void) {
- GDO_dir;
-
- debugWrite("CC2500: initializing\n");
-
- initialize(1); // binding
-
- debugWrite("CC2500: binding\n");
-
- binding();
-
- debugWrite("CC2500: receiving\n");
-
- initialize(0); // data
- cc2500WriteReg(CC2500_0A_CHANNR, hopData[channr]);// 0A - hop
- cc2500WriteReg(CC2500_23_FSCAL3, 0x89); // 23 - 89
- cc2500Strobe(CC2500_SRX);
- }
-
- static void initialize(uint8_t bind) {
- cc2500ResetChip();
- cc2500WriteReg(CC2500_02_IOCFG0, 0x01); // RX complete interrupt(GDO0)
- cc2500WriteReg(CC2500_17_MCSM1, 0x0F);
- cc2500WriteReg(CC2500_18_MCSM0, 0x18);
- cc2500WriteReg(CC2500_06_PKTLEN, 0x19); // Leave room for appended status bytes
- cc2500WriteReg(CC2500_08_PKTCTRL0, 0x05);
- cc2500WriteReg(CC2500_3E_PATABLE, 0xFF);
- cc2500WriteReg(CC2500_0B_FSCTRL1, 0x08);
- cc2500WriteReg(CC2500_0C_FSCTRL0, 0x00);
-
- #ifdef FREQ_ORIGINAL
- /*
- * According to the internet, these are the default FrSky
- * FREQ values (0x5C7627) which should equal 2.404GHz.
- * The datasheet mentions Fcarrier = Fxosc * FREQ / 2^16
- * Therefore, FrSky seems to be using 26MHz too...?
- */
- cc2500WriteReg(CC2500_0D_FREQ2, 0x5C);
- cc2500WriteReg(CC2500_0E_FREQ1, 0x76);
- cc2500WriteReg(CC2500_0F_FREQ0, 0x27);
- #else
- cc2500WriteReg(CC2500_0D_FREQ2, 0x5B); // freq2
- cc2500WriteReg(CC2500_0E_FREQ1, 0x00); // freq1
- cc2500WriteReg(CC2500_0F_FREQ0, 0x00); // freq0
- #endif
-
- 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, 0x05);
- 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, 0x0F); // 0x07?
-
- cc2500WriteReg(CC2500_09_ADDR, bind ? 0x03 : txid[0]);
- //cc2500WriteReg(CC2500_09_ADDR, bind ? 0x00 : txid[0]);
-
- debugWrite("CC2500: Entering IDLE mode...\n");
-
- cc2500Strobe(CC2500_SIDLE); // Go to idle...
-
- #ifdef DEBUG
- uint8_t part = cc2500ReadReg(CC2500_30_PARTNUM);
- uint8_t version = cc2500ReadReg(CC2500_31_VERSION);
-
- debugWrite("CC2500: Part Number: 0x");
- debugHex(part);
- debugWrite("\nCC2500: Version: 0x");
- debugHex(version);
- debugWrite("\n");
-
- if ((part != 0x80) || (version != 0x03)) {
- debugWrite("INVALID CC2500 STATUS! IT MAY BE DEAD!\n");
- }
- #endif
-
- // hack: Append status, filter by address, auto-flush on bad crc, PQT=0
- cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0D);
-
- //cc2500WriteReg(CC2500_0C_FSCTRL0, 0); // Frequency offset
- cc2500WriteReg(CC2500_0C_FSCTRL0, bind ? 0x00 : frequencyOffsetHack);
-
- cc2500WriteReg(CC2500_0A_CHANNR, 0x00);
- }
-
- static void binding(void) {
- readBindingData();
- if ((txid[0] != 0xff) || (txid[1] != 0xff)) {
- // valid binding data found
- debugWrite("RX: found data in EEPROM!\n");
- return;
- }
-
- debugWrite("RX: no stored data, tuning...\n");
-
- // No valid txid, forcing bind
- tuning();
-
- //frequencyOffsetHack = 0xC8; // for test
- cc2500WriteReg(CC2500_0C_FSCTRL0, frequencyOffsetHack);
- eeprom_write_byte(EEPROM_BASE_ADDRESS + 101, frequencyOffsetHack);
-
- debugWrite("RX: tuned, binding...\n");
-
- performBind();
- }
-
- static void tuning() {
- cc2500Strobe(CC2500_SRX); // enter in rx mode
-
- uint16_t frequencyOffsetTimer = 0;
- while (1) {
- frequencyOffsetTimer++;
- if (frequencyOffsetTimer > 3000) {
- frequencyOffsetTimer = 0;
- cc2500WriteReg(CC2500_0C_FSCTRL0, frequencyOffsetHack);
- if (frequencyOffsetHack <= 250) {
- frequencyOffsetHack += 5;
- } else {
- frequencyOffsetHack = 0;
-
- if (freq0 < 255) {
- freq0++;
- } else {
- freq0 = 0;
- if (freq1 < 255) {
- freq1++;
- } else {
- freq1 = 0;
- if (freq2 < 255) {
- freq2++;
- } else {
- freq2 = 0;
- }
- }
- }
-
- debugWrite("0x");
- debugHex(freq2);
- debugHex(freq1);
- debugHex(freq0);
- debugWrite("\n");
-
- cc2500WriteReg(CC2500_0D_FREQ2, freq2);
- cc2500WriteReg(CC2500_0E_FREQ1, freq1);
- cc2500WriteReg(CC2500_0F_FREQ0, freq0);
- }
- //cc2500Strobe(CC2500_SRX); // enter in rx mode
- }
-
- if (GDO_1) {
- debugWrite("Tuning: data flag has been set!\n");
-
- //ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
- ccLen = cc2500ReadReg(CC2500_3B_RXBYTES) & 0x7F;
- if (ccLen) {
- debugWrite("Tuning: got data!\n");
- cc2500ReadFifo(ccData, ccLen);
- if ((ccData[ccLen - 1] & 0x80)
- && (ccData[2] == 0x01)
- && (ccData[5] == 0x00)) {
- break;
- }
- } else {
- debugWrite("Tuning: no data?!\n");
- }
- }
-
- wdt_reset();
-
- #ifdef DEBUG
- uartMenu();
- #endif
- }
- }
-
- static void performBind(void) {
- cc2500Strobe(CC2500_SRX); // enter in rx mode
-
- while (1) {
- if (GDO_1) {
- //ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
- ccLen = cc2500ReadReg(CC2500_3B_RXBYTES) & 0x7F;
- if (ccLen) {
- cc2500ReadFifo(ccData, ccLen);
- if ((ccData[ccLen - 1] & 0x80)
- && (ccData[2] == 0x01)
- && (ccData[5] == 0x00)) {
- txid[0] = ccData[3];
- txid[1] = ccData[4];
- for (uint8_t n = 0; n < 5; n++) {
- hopData[ccData[5] + n] = ccData[6 + n];
- }
- break;
- }
- }
- }
-
- wdt_reset();
-
- #ifdef DEBUG
- uartMenu();
- #endif
- }
-
- debugWrite("RX: got hop data, reading list...\n");
-
- listLength = 0;
- uint8_t eol = 0;
- for (uint8_t bindIdx = 0x05; bindIdx <= 120; bindIdx += 5) {
- while (1) {
- if (GDO_1) {
- //ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
- ccLen = cc2500ReadReg(CC2500_3B_RXBYTES) & 0x7F;
- if (ccLen) {
- cc2500ReadFifo(ccData, ccLen);
- if ((ccData[ccLen - 1] & 0x80)
- && (ccData[2] == 0x01)
- && (ccData[3] == txid[0])
- && (ccData[4] == txid[1])
- && (ccData[5] == bindIdx)) {
- for (uint8_t n = 0; n < 5; n++) {
- //if (ccData[6 + n] == ccData[ccLen - 3]) {
- if (ccData[6 + n] <= 3) {
- eol = 1;
- listLength = ccData[5] + n;
- break;
- }
- hopData[ccData[5] + n] = ccData[6 + n];
- }
- break;
- }
- }
- }
-
- wdt_reset();
-
- #ifdef DEBUG
- uartMenu();
- #endif
- }
-
- if (eol) {
- break; // End of list found, stop!
- }
- }
-
- debugWrite("RX: binding finished!\n");
-
- writeBindingData();
- cc2500Strobe(CC2500_SIDLE); // Back to idle
- }
-
- static void nextChannel(uint8_t skip) {
- channr += skip;
- if (channr >= listLength) {
- channr -= listLength;
- }
-
- cc2500WriteReg(CC2500_0A_CHANNR, hopData[channr]);
- cc2500WriteReg(CC2500_23_FSCAL3, 0x89);
- }
-
- static void readBindingData() {
- for (uint8_t i = 0; i < 2; i++) {
- txid[i] = eeprom_read_byte(EEPROM_BASE_ADDRESS + i);
- }
-
- for (uint8_t i = 0; i < HOP_DATA_LENGTH; i++) {
- hopData[i] = eeprom_read_byte(EEPROM_BASE_ADDRESS + 10 + i);
- }
-
- listLength = eeprom_read_byte(EEPROM_BASE_ADDRESS + 100);
- frequencyOffsetHack = eeprom_read_byte(EEPROM_BASE_ADDRESS + 101);
- }
-
- static void writeBindingData() {
- for (uint8_t i = 0; i < 2; i++) {
- eeprom_write_byte(EEPROM_BASE_ADDRESS + i, txid[i]);
- }
-
- for (uint8_t i = 0; i < HOP_DATA_LENGTH; i++) {
- eeprom_write_byte(EEPROM_BASE_ADDRESS + 10 + i, hopData[i]);
- }
-
- eeprom_write_byte(EEPROM_BASE_ADDRESS + 100, listLength);
- }
-
- void rxReceivePacket() {
- time_t time = timerGet();
-
- if (missingPackets > FAILSAFE_MISSING_PACKET) {
- failed = 1;
- missingPackets = 0;
- }
-
- while (1) {
- if ((timerGet() - time) > MISSING_PACKET_DELAY) {
- missingPackets++;
- cc2500Strobe(CC2500_SIDLE);
- if (missingPackets > MAX_MISSING_PACKET) {
- nextChannel(SEEK_CHANNEL_SKIP);
- counter++;
-
- if (counter > (MAX_MISSING_PACKET << 1)) {
- debugWrite("RX: missing packet notification!\n");
- }
-
- if (counter == (MAX_MISSING_PACKET << 2)) counter = 0;
- break;
- } else
- nextChannel(1);
- break;
- }
-
- if (GDO_1) {
- //ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
- ccLen = cc2500ReadReg(CC2500_3B_RXBYTES) & 0x7F;
- if (ccLen > 20) {
- ccLen = 20;
- }
- if (ccLen) {
- cc2500ReadFifo((uint8_t *)ccData, ccLen);
- if (ccData[ccLen - 1] & 0x80) { // Only if correct CRC
- missingPackets = 0;
- if ((ccData[0] == 0x11) // Correct length
- && (ccData[1] == txid[0]) // Correct txid
- && (ccData[2] == txid[1])) {
- packet = 1;
-
- #ifdef RSSI_OVER_PPM
- //int rssi_dec = cc2500ReadReg(CC2500_34_RSSI | CC2500_READ_BURST);
- int rssi_dec = cc2500ReadReg(CC2500_34_RSSI);
- if (rssi_dec < 128) {
- rssi = ((rssi_dec / 2) - RSSI_OFFSET) & 0x7f;
- } else {
- rssi = (((rssi_dec - 256) / 2)) - RSSI_OFFSET;
- }
- rssi = constrain(rssi, RSSI_MIN, RSSI_MAX);
- #endif
-
- cc2500Strobe(CC2500_SIDLE);
- nextChannel(1);
- failed = 0;
- break;
- }
- }
- }
- }
-
- wdt_reset();
-
- #ifdef DEBUG
- uartMenu();
- #endif
- }
-
- if (packet != 0) {
- packet = 0;
- c[0] = (uint16_t)(ccData[10] & 0x0F) << 8 | ccData[6];
- c[1] = (uint16_t)(ccData[10] & 0xF0) << 4 | ccData[7];
- c[2] = (uint16_t)(ccData[11] & 0x0F) << 8 | ccData[8];
- c[3] = (uint16_t)(ccData[11] & 0xF0) << 4 | ccData[9];
- c[4] = (uint16_t)(ccData[16] & 0x0F) << 8 | ccData[12];
- c[5] = (uint16_t)(ccData[16] & 0xF0) << 4 | ccData[13];
- c[6] = (uint16_t)(ccData[17] & 0x0F) << 8 | ccData[14];
- c[7] = (uint16_t)(ccData[17] & 0xF0) << 4 | ccData[15];
-
- for (int i = 0; i < CHANNELS; i++) {
- ppmBuffer[i] = PPM_SCALE * c[i];
- if ((ppmBuffer[i] < 900) || (ppmBuffer[i] > 2100)) {
- ppmBuffer[i] = 850;
- }
- }
-
- #ifdef RSSI_OVER_PPM
- ppmBuffer[RSSI_OVER_PPM] = map(rssi, RSSI_MIN, RSSI_MAX, PPM_MIN, PPM_MAX);
- #endif
-
- debugWrite("RX: packet received, sending CPPM!\n");
- cppmCopy(ppmBuffer);
- }
-
- cc2500Strobe(CC2500_SRX);
- }
-
- static long map(long x, long in_min, long in_max, long out_min, long out_max) {
- return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
- }
-
- static long constrain(long x, long min, long max) {
- if (x < min) {
- x = min;
- }
- if (x > max) {
- x = max;
- }
- return x;
- }
|