xyFC/src/rx.c

/*
 * FrSky RX 2-way protocol
 */

#include <stdint.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/eeprom.h>

#include "cc2500.h"
#include "spi.h"
#include "timer.h"


// ----------------------------------------------------------------------------


//#define DEBUG
//#define DEBUG_RSSI
//#define DEBUG0
//#define DEBUG1
//#define DEBUG2
//#define DEBUG3
//#define DEBUG4
//#define DEBUG5
//#define RSSI_OVER_PPM 7

#define FAILSAFE

// Used for RSSI_OVER_PPM
int rssi;
const int rssi_offset = 71;
const int rssi_min = -103;
const int rssi_max = -96;

#define chanel_number 8  //set the number of chanels
#define SEEK_CHANSKIP   13
#define MAX_MISSING_PKT 20
#define FAILSAFE_MISSING_PKT 170
#define PPM_FrLen 22500
#define PPM_PulseLen 300
#define default_servo_value 1500
#define onState 0  //set polarity of the pulses: 1 is positive, 0 is negative
#define sigPin 10

#define bind_pin A0     //C0 bind plug also servo8

#define TRUE 1
#define FALSE 0

// Globals:
static uint8_t ccData[27];
static uint8_t ccLen;
static uint8_t packet = FALSE;
static uint8_t channr = 0;
static uint8_t missingPackets = 0;
uint8_t hopData[60];
uint8_t listLength;
uint8_t txid[2];
static uint8_t counter = 0;
volatile uint16_t Servo_data[10] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};
volatile int ppm[chanel_number];
volatile uint8_t failed = FALSE;
int count = 0;
uint16_t c[8];


void getBind(void);
void Store_bind(void);
void updateRSSI(void);
void loop(void);
void nextChannel(uint8_t skip);
void tuning(void);


// ----------------------------------------------------------------------------

static void initialize(uint8_t bind);
static void binding(void);

void rxInit(void) {
    initialize(1); // binding
    binding();

    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,    0x0C);
    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);
    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,   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);

    cc2500WriteReg(CC2500_09_ADDR, bind ? 0x03 : txid[0]);

    cc2500Strobe(CC2500_SIDLE); // Go to idle...

    // 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 : count); // Frequency offset hack

    cc2500WriteReg(CC2500_0A_CHANNR, 0x00);
}


static void binding(void) {
    uint8_t jumper2 = 0; //bind_jumper();

    while (1) {
        if (jumper2 == 0) {
            // bind complete or no bind
            uint8_t adr = 100;
            for (uint8_t i = 0; i < 2; i++) {
                txid[i] = eeprom_read_byte(adr + i);
            }
            if ((txid[0] == 0xff) && (txid[1] == 0xff)) {
                // No valid txid, forcing bind
                jumper2 = 1;
                continue;
            }
            for (uint8_t i = 0; i < sizeof(hopData); i++) {
                hopData[i] = eeprom_read_byte(adr + 10 + i);
            }
            listLength = eeprom_read_byte(adr + 100);
            count = eeprom_read_byte(adr + 101);
            break;
        } else {
            tuning();
            //count=0xC8;//for test
            cc2500WriteReg(CC2500_0C_FSCTRL0, count);
            int adr = 100;
            eeprom_write_byte(adr + 101, count);
            getBind();
            // TODO reset?!
            while (1) { }
        }
    }
}


// ----------------------------------------------------------------------------



// Receives complete bind setup
void getBind(void)
{
    cc2500Strobe(CC2500_SRX);//enter in rx mode
    listLength = 0;
    uint8_t eol = FALSE;
    //           len|bind |tx id|idx|h0|h1|h2|h3|h4|00|00|00|00|00|00|01
    // Start by getting bind packet 0 and the txid
    //        0  1   2  txid0(3) txid1()4    5  6  7   8  9 10 11 12 13 14 15 16 17
    //ccdata    //11 03 01  d7       2d          00 00 1e 3c 5b 78 00 00 00 00 00 00 01
    //11 03 01  19       3e          00 02 8e 2f bb 5c 00 00 00 00 00 00 01
    while (1) {
        if (GDO_1) {
            ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
            if (ccLen) {
                cc2500ReadFifo((uint8_t *)ccData, ccLen);
                if (ccData[ccLen - 1] & 0x80) {
                    if (ccData[2] == 0x01) {
                        if (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;
                        }
                    }
                }
            }
        }
    }

    #if defined(DEBUG)
        Serial.print(txid[0], HEX);
        Serial.println(txid[1], HEX);
    #endif

    for (uint8_t bindIdx = 0x05; bindIdx <= 120; bindIdx += 5) {
        while (1) {
            if (GDO_1) {
                ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
                if (ccLen) {
                    cc2500ReadFifo((uint8_t *)ccData, ccLen);
                    if (ccData[ccLen - 1] & 0x80) {
                        if (ccData[2] == 0x01) {
#if DEBUG3
                            Serial.print("ccLen = ");
                            Serial.println(ccLen);
#endif
                            if ((ccData[3] == txid[0]) && (ccData[4] == txid[1])) {
#if DEBUG3
                                Serial.print("ccData[5] = ");
                                Serial.println(ccData[5]);
                                Serial.print("bindIdx = ");
                                Serial.println(bindIdx);
#endif
                                if (ccData[5] == bindIdx) {
                                    for (uint8_t n = 0; n < 5; n++) {
#if DEBUG3
                                        Serial.print("ccData[6 + n] = ");
                                        Serial.println(ccData[6 + n]);
                                        Serial.print("ccData[ccLen - 3] = ");
                                        Serial.println(ccData[ccLen - 3]);
#endif
                                        //if (ccData[6 + n] == ccData[ccLen - 3]) {
                                          if (ccData[6 + n] <= 3) {
                                            eol = TRUE;
                                            #if defined(DEBUG)
                                                Serial.print("listLength: ");
                                                Serial.println(listLength);
                                            #endif
                                            listLength = ccData[5] + n;
                                            break;
                                        }
                                        hopData[ccData[5] + n] = ccData[6 + n];
                                    }
                                    break;
                                }
                            }
                        }
                    }
                }
            }
        }
        #if defined(DEBUG)
                Serial.println(bindIdx / 5);
        #endif
        if (eol) break; // End of list found, stop!
    }

    #if defined(DEBUG)
        listLength = 47;
        Serial.println("jumpIdx list: ");
        for (uint8_t jumpIdx = 0; jumpIdx < (listLength); jumpIdx++) {
            Serial.print(" ");
            Serial.print(hopData[jumpIdx], HEX);
            Serial.print(" ");
        }
        Serial.println(" ");
    #endif

    Store_bind();
    cc2500Strobe(CC2500_SIDLE);    // Back to idle
}





void Store_bind()
{
    uint8_t i;
    int adr = 100;
    for (i = 0; i < 2; i++) {
        eeprom_write_byte(adr + i, txid[i]);
    }
    for (i = 0; i < sizeof(hopData); i++) {
        eeprom_write_byte(adr + 10 + i, hopData[i]);
    }
    eeprom_write_byte(adr + 100, listLength);
}




void updateRSSI() {
#if defined(RSSI_OVER_PPM)
    int rssi_dec = cc2500ReadReg(CC2500_34_RSSI | CC2500_READ_BURST);
    if (rssi_dec < 128) {
        rssi = ((rssi_dec / 2) - rssi_offset) & 0x7f;
    } else {
        rssi = (((rssi_dec - 256) / 2)) - rssi_offset;
    }

  #if defined(DEBUG_RSSI2)
    Serial.print(millis());
    Serial.print("\t");
    Serial.println(rssi);
  #endif

    rssi = constrain(rssi, rssi_min, rssi_max);
#endif
}

void loop()
{
    time_t time = timerGet();

    #if defined(FAILSAFE)
        if (missingPackets > FAILSAFE_MISSING_PKT) {
            failed = TRUE;
            missingPackets = 0;
        }
    #endif

    while (1) {
        if ((timerGet() - time) > 9) {
            missingPackets++;
            cc2500Strobe(CC2500_SIDLE);
            if (missingPackets > MAX_MISSING_PKT) {
                nextChannel(SEEK_CHANSKIP);
                counter++;
                //if (counter > (MAX_MISSING_PKT << 1)) LED_ON;
                if (counter == (MAX_MISSING_PKT << 2)) counter = 0;
                break;
            } else
                nextChannel(1);
            break;
        }
        if (GDO_1) {
            ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 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
                        if ((ccData[1] == txid[0]) && (ccData[2] == txid[1])) { // Only if correct txid
                            packet = TRUE;
                            //sei();
                            updateRSSI();
                            cc2500Strobe(CC2500_SIDLE);
                            nextChannel(1);
                            failed = FALSE;
                            break;
                        }
                    }
                }

            }

        }
    }

    if (packet == TRUE) {
        packet = FALSE;
        //cli();
        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];
        //sei();
        for (int i = 0; i < 8; i++) {
            Servo_data[i] = 0.67 * c[i];
            if (Servo_data[i] < 900) { //added new
                Servo_data[i] = 1500; //added new
                Servo_data[2] = 1000;
            }
            ppm[i] = Servo_data[i];
        }
        #if defined(RSSI_OVER_PPM)
          ppm[RSSI_OVER_PPM] = map(rssi, rssi_min, rssi_max, 1000, 2000);
        #endif
        #if defined(DEBUG5)
                //Serial.println(rssi);
        #endif
        #if defined(DEBUG0)
                for (int i = 0; i < 8; i++) {
                    Serial.print(" ");
                    Serial.print(Servo_data[i]);
                    Serial.print(" ");
                }
                Serial.println(" ");
        #endif
    }

    cc2500Strobe(CC2500_SRX);
}




void nextChannel(uint8_t skip)
{
    channr += skip;//
    if (channr >= listLength) channr -= listLength;
    cc2500WriteReg(CC2500_0A_CHANNR, hopData[channr]);
    cc2500WriteReg(CC2500_23_FSCAL3, 0x89);

}



void tuning()
{
    cc2500Strobe(CC2500_SRX);//enter in rx mode
    int count1 = 0;
    while (1) {
        count1++;
        if (count >= 250) {
            count = 0;
        }
        if (count1 > 3000) {
            count1 = 0;
            cc2500WriteReg(CC2500_0C_FSCTRL0, count);  // Frequency offset hack
            count = count + 10;
            //cc2500Strobe(CC2500_SRX);//enter in rx mode
        }
        if (GDO_1) {
            ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
            if (ccLen) {
                cc2500ReadFifo((uint8_t *)ccData, ccLen);
                if (ccData[ccLen - 1] & 0x80) {
                    if (ccData[2] == 0x01) {
                        if (ccData[5] == 0x00) {
                            break;
                        }
                    }
                }
            }
        }
    }
    #if defined(DEBUG1)
        Serial.println(count, HEX);
    #endif
}