Subversion Repositories FlightCtrl

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// This code has been derived from the implementation of Stefan Engelke.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
    Copyright (c) 2008 Stefan Engelke <stefan@tinkerer.eu>

    Permission is hereby granted, free of charge, to any person
    obtaining a copy of this software and associated documentation
    files (the "Software"), to deal in the Software without
    restriction, including without limitation the rights to use, copy,
    modify, merge, publish, distribute, sublicense, and/or sell copies
    of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be
    included in all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
    MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
    DEALINGS IN THE SOFTWARE.

    $Id: rcdsl.c 60 2008-08-21 07:50:48Z taser $

    RCDSL.H and RCDSL.C is an INOFFICIAL implementation of the
    communication protocol used by DSL receivers of Act Europe.
    The DSL receivers have a serial communication port to connect
    two receivers in diversity mode. Each receiver is sending the
    received servo signals periodically over this port. This fact
    can be used to connect the receiver to the control unit of the
    model via UART instead of evaluating the PPM signal.

    If you have any questions, fell free to send me an e-mail.

*/


/*
Connection of DSL to SV1 of FC:
( DSL Pin1 is on side of channel 4 )

1. GND <--> pin 7 (GND)
2. TXD <--> pin 3 (RXD1 Atmega644p)
3. RXD <--> pin 4 (TXD1 Atmega644p) optional
4. 5V  <--> pin 2 (5V)

If a receiver is connected via PPM input at the same time, the PPM input will be disabled
if a stable signal can be captured by the uart.

Data are send at every 20 ms @ 38400 Baud 8-N-1

Data Frame: |0xFF|0xFF|0x1F|FREQALLOC|??|RSSI|VBAT|??|CRC|10|CH0D1|CH0D0|CH1D1|CH1D0|CRC| ...etc

FREQALLOC = 35, 40, 72
RSSI = 0.. 255 // Received signal strength indicator
VBAT = 0...255 // supply voltage (0.0V.. 7.8V)

Servo Pair:   |0x1X|CHXD1|CHXD0|CHX+1D1|CHX+1D0|CRC|
X is channel index of 1 servo value
D1D0 is servo value as u16 in range of 7373 (1ms) to 14745 (2ms)
there are 8 channels submitted, i.e 4 servo pairs


Frame examples with signel received

FFFF 1F23F079A304AD 1036012B1E6F 122AFB2AECB2 142B4D2B4404 1636872B33CE
FFFF 1F23F079A304AD 1036002B1F6F 122AFE2AEBB0 142B4B2B4406 1636872B33CE
FFFF 1F23F079A304AD 1035FF2B226E 122AFC2AEAB3 142B4E2B4304 1636882B33CD
FFFF 1F23F079A304AD 1036022B1E6E 122AFB2AEEB0 142B4A2B4506 1636872B33CE
FFFF 1F23F079A304AD 1036022B1E6E 122AFE2AEBB0 142B4B2B4406 1636882B33CD
FFFF 1F23F079A304AD 1036012B1E6F 122AFD2AEAB2 142B4E2B4403 1636862B33CF
FFFF 1F23F079A304AD 1036032B1D6E 122AFD2AEBB1 142B4C2B4504 1636862B33CF

Frame examples with no signal received

FFFF 1F23F000A30426
FFFF 1F23F000A30426
FFFF 1F23F000A30426
FFFF 1F23F000A30426
FFFF 1F23F000A30426
FFFF 1F23F000A30426
FFFF 1F23F000A30426
*/

#include <stdlib.h>
#include "dsl.h"
#include "rc.h"
#include "eeprom.h"

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];
} ChannelPair_t;

ChannelPair_t ChannelPair;


// 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

        if(index > 0  &&  index < MAX_CHANNELS)
        {
                if(RC_Channels < index)  RC_Channels = index;

                // signal from  DSL-receiver is between 7373 (1ms) und 14745 (2ms).
                signal-= 11059;     // shift to neutral
                signal/= 24;        // scale to mk rc resolution

                if(abs(signal - PPM_in[index]) < 6)
                {
                        if(RC_Quality < 200)
                        {
                                RC_Quality += 10;
                        }
                        else
                        {
                                RC_Quality = 200;
                                PPM_INPUT_OFF; // disable PPM input at ICP
                        }
                }

                // 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 >= 180)  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;

        // check for header condition
        if((PacketBuffer[0] & 0xF0) == 0x10)
        {
                if(PacketBuffer[0] == 0x1F) // separate status frame
                {
                        dsl_Allocation  = PacketBuffer[1]; // Get frequency allocation
                        // ??                   = PacketBuffer[2];
                        dsl_RSSI                = PacketBuffer[3]; // Get signal quality
                        dsl_Battery     = PacketBuffer[4]; // Get voltage of battery supply
                        // ??                   = PacketBuffer[5];
                        RC_RSSI                 = dsl_RSSI;
                        PPM_in[0] = RC_RSSI;
                        if(RC_RSSI == 0)
                        {   // set to neutral attitude commands
                                PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] = 0;
                                PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
                                PPM_diff[ParamSet.ChannelAssignment[CH_YAW]]  = 0;
                                PPM_in[ParamSet.ChannelAssignment[CH_NICK]] = 0;
                                PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
                                PPM_in[ParamSet.ChannelAssignment[CH_YAW]]  = 0;
                        }
                }
                else // probably a channel pair
                {
                        i = PacketBuffer[0] & 0x0F;   // last 4 bits of the header indicates the channel pair
                        if(i < 10)// maximum 12 channels
                        {
                                // big to little endian
                                ChannelPair.byte[1] = PacketBuffer[1];
                                ChannelPair.byte[0] = PacketBuffer[2];
                                ChannelPair.byte[3] = PacketBuffer[3];
                                ChannelPair.byte[2] = PacketBuffer[4];
                                dsl_new_signal(i,  ChannelPair.Servo[0]);
                                dsl_new_signal(i+1,ChannelPair.Servo[1]);
                        }
                }
        } // EOF header condition
}


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

        // check for sync condition
        if ((c==0xFF) && (last_c==0xFF))
        {
                cnt = 0; // reset byte counter
                crc = 0; // reset checksum
                return;
    }

        if(cnt == 0) // begin of a packet
        {
                if(c == 0x1F)   packet_len = 5; // a status packet has 5 bytes + crc
                else                    packet_len = 4; // a channel pair packet has 4 bytes + crc
        }
        if(cnt > packet_len) // packet complete, crc byte received
        {
                // calculate checksum
                crc = ~crc;
                if (crc == 0xFF) crc = 0xFE;
                // if crc matches decode the packet
        if (c == crc) dsl_decode_packet();
        // handle next packet
        cnt = 0;
        crc = 0;
        }
        else // collect channel data bytes
        {
                PacketBuffer[cnt++] = c;
                crc += c;
        }
        // store last byte for sync check
        last_c = c;
}