Subversion Repositories FlightCtrl

#include "dsl.h"
#include "rc.h"

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

// dsl protocol parser state machine
#define DSLSTATE_SYNC1  0
#define DSLSTATE_SYNC2  1
#define DSLSTATE_HDR    2
#define DSLSTATE_DATA   3
#define DSLSTATE_CRC    4

#define DSL_SYNC1_CHAR 0xFF
#define DSL_SYNC2_CHAR 0xFF


uint8_t dsl_RSSI = 0;
uint8_t dsl_Battery = 0;
uint8_t dsl_Allocation = 0;
uint8_t PacketBuffer[6];

typedef union
{
        int16_t Servo[2];
        uint8_t  byte[4];
} ServoPair_t;

ServoPair_t ServoPair;


// 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)
{
    int16_t tmp;
    uint8_t index = channel + 1; // mk channels start with 1


    // Signal vom  ACTDSL-Empfänger liegt zwischen
    // 7373 (1ms) und 14745 (2ms).
    signal-= 11059;     // shift to neutral
    signal/= 24;        // scale to mk rc resolution

        // calculate exponential history for signal
        tmp = (3 * (PPM_in[index]) + signal) / 4;
        if(tmp > signal+1) tmp--; else
        if(tmp < signal-1) tmp++;
        // calculate signal difference on good signal level
        if(RC_Quality >= 195)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for noise reduction
        else PPM_diff[index] = 0;
        PPM_in[index] = tmp; // update channel value

    if(index == 4) NewPpmData = 0;
}

// This function is called within dsl_parser(), when a complete
// data packet with valid checksum has been received.
void dsl_decode_packet(void)
{
    uint8_t  i;

    if(PacketBuffer[0] == 0x1F) // separate status frame
    {
        dsl_Allocation  = PacketBuffer[1]; // Get frequency allocation
        // ??                   = PacketBuffer[2];
        dsl_RSSI                = PacketBuffer[3]; // Get signal quality
        RC_Quality              = dsl_RSSI; // have to be scaled
        dsl_Battery     = PacketBuffer[4]; // Get voltage of battery supply
        // ??                   = PacketBuffer[5];
        }
    else // analyze servo pair frame
    {
                i = PacketBuffer[0] & 0x0F;   // last 4 bits of the header indicates servo pair
                if(i < 10)
                {
                        // big to little endian
                        ServoPair.byte[1] = PacketBuffer[1];
                        ServoPair.byte[0] = PacketBuffer[2];
                        ServoPair.byte[3] = PacketBuffer[3];
            ServoPair.byte[2] = PacketBuffer[4];
                        dsl_new_signal(i,  ServoPair.Servo[0]);
                        dsl_new_signal(i+1,ServoPair.Servo[0]);
                }
        }
}


// 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)
//
// Servo Pair:   |0xFF|0xFF|0x1X|D0|D1|D2|D3|CRC|
// X is channel index of 1 servo value
// D0, D1 is servo channel X
// D2, D3 is servo channel X+1

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

        static uint8_t dslstate = DSLSTATE_SYNC1;

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

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

                case DSLSTATE_HDR: // check header byte
                        if ((c & 0xF0) != 0x10)
                        {
                                dslstate = DSLSTATE_SYNC1;
                        }
                        else
                        {
                                cnt = 0;
                                PacketBuffer[cnt++] = c;
                                crc = c;
                                if(c == 0x1F)  packet_len = 5;
                                else           packet_len = 4;
                                dslstate = DSLSTATE_DATA;
                        }
                        break;

                case DSLSTATE_DATA:
                        PacketBuffer[cnt++] = c;
                        crc += c;
                        if(cnt > packet_len) dslstate = DSLSTATE_CRC;
                        break;

                case DSLSTATE_CRC:
                        // Calculate checksum
                        crc = ~crc;
                        if (crc == 0xFF) crc = 0xFE;
                        // if crc matches
                if (c == crc) dsl_decode_packet();
                dslstate = DSLSTATE_SYNC1;
                        break;

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