Subversion Repositories FlightCtrl

#include "dsl.h"

/*
1. GND (at channel 4 side)
2. TxD
3. RxD
4. 5V
*/

// dsl protocol parser state machine
#define DSLSTATE_IDLE   0
#define DLSSTATE_SYNC   1
#define DSLSTATE_HDR    2
#define DSLSTATE_DATA   3

#define DSL_SYNC1_CHAR 0xFF
#define DSL_SYNC2_CHAR 0xFF


volatile uint8_t rcdsl_RSSI = 0;
uint8_t dsl_battery = 0;
uint8_t dsl_allocation = 0;
uint8_t data[6];

typedef union {
        uint16_t pos[2];
        uint8_t dat[4];
} servos_t;


// This function is called, when a new servo signal is properly received.
// Parameters: servo  - servo number (0-9)
//             signal - servo signal between 7373 (1ms) and 14745 (2ms)
void dsl_new_signal(uint8_t channel, int16_t signal)
{
    volatile signed int tmp;
    uint8_t index = channel+1; // Der MK fängt bei 1 an zu zählen


    // Signal vom  ACTDSL-Empfänger liegt zwischen
    // 7373 (1ms) und 14745 (2ms).
    signal-= 11059;     // Neutralstellung zur Null verschieben
    signal/= 24;        // Auf MK Skalierung umrechnen

    // Stabiles Signal
    //if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10;}

    tmp = (3 * (PPM_in[index]) + signal) / 4;
    if(tmp > signal+1) tmp--; else
    if(tmp < signal-1) tmp++;
    if(SenderOkay >= 105)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3;
    else PPM_diff[index] = 0;
    PPM_in[index] = tmp;
    //NewPpmData = 0;
    //NewActData = 1;
    if(index == 4)  {
        //NewActData = 1;
        NewPpmData = 0;
        //PORTC = (PORTC&(~(1<<PC4))) | ((~PORTC)&(1<<PC4));
        //PINC = (1<<PC4);
    }
    //PPM_in[index] = signal;

}

// This function is called within dsl_parser(), when a complete
// data packet with valid checksum has been received.
void dsl_incoming_paket(void)
{
    uint8_t  i;
    static servos_t servos;
    // Look for status headers
    if ((data[0])==0x1F) {
        // Get frequency allocation
        dsl_allocation = data[0+1];
        // Get signal quality
        dsl_RSSI = data[2+1];
        // Get voltage of battery supply
        dsl_battery = data[3+1];
    }

    // Look for signal headers
    if ((data[0]&0xF0)==0x10) {

        i = data[0]&0x0F;   // Last 4 bits of the header indicates servo pair

        if (i<10) {
            // Convert byte array to two uint16
            servos.dat[1] = data[1];
            servos.dat[0] = data[2];
            servos.dat[3] = data[3];
            servos.dat[2] = data[4];

            rcdsl_new_signal(i  ,  (int16_t)servos.pos[0]);
            rcdsl_new_signal(i+1,  (int16_t)servos.pos[1]);
        }
    }
}


// Status Frame: |0xFF|0xFF|0x1F|FREQALLOC|??|RSSI|VBAT|CRC
// FREQ ALLOC = 35, 40, 72
// RSSI = 0.. 255                               // Received signal strength indicator
// VBAT = 0...255                               // supply voltage (0.0V.. 8.2V)
//
//|0xFF|0xFF|CMD|D0|D1|D2|D3

// this function should be called within the UART RX ISR
void dsl_parser(uint8_t c)
{
        static uint8_t crc      = 0;
        static uint8_t packet_len       = 0;
        static uint8_t data_counter = 0;

        static uint8_t dslstate = DSLSTATE_IDLE;

        switch(dslstate)
        {
                case DSLSTATE_IDLE: // check 1st sync byte
                        if (c == DSL_SYNC1_CHAR) dslstate = DSLSTATE_SYNC;
                        else dslstate = UBXSTATE_IDLE; // out of synchronization
                        break;

                case DSLSTATE_SYNC: // check 2nd sync byte
                        if (c == DSL_SYNC2_CHAR) dslstate = DSLSTATE_CMD; // snchrinization complete trigger to cmd
                        else dslstate = DSLSTATE_IDLE; // out of synchronization
                        break;

                case DSLSTATE_HDR: // check cmd byte
                        if (c == 0x1F) packet_len = 5;
                        else           packet_len = 4;
                        dcnt = 1;
                crc = 0;
                dslstate = DSLSTATE_DATA;
                        break;

                case DSLSTATE_DATA:
                        data[dcnt++] = c;
                        crc += c;
                        if(dcnt > packetlen) dslstate = DSLSTATE_CRC;
                        break;

                case DSLSTATE_CRC:
                        // Calculate checksum
                        crc = ~crc;
                        if (crc == 0xFF) crc = 0xFE;
                        // If it match the received one, then apply data
                if (c == crc) rcdsl_incoming_paket();
                dslstate = DSLSTATE_IDLE;
                        break;

                default: // unknown ubx state
                        dslstate = DSLSTATE_IDLE;
                        break;
        }
}