Subversion Repositories FlightCtrl

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// This code (protocol) is inspired by the RCDSL 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.

*/

/***********************************************************************************************************

* FC_JN_RECEIVER by DLP

************************************************************************************************************

Connection of receiver to SV1 of FC:

TXD <--> pin 3 (RXD1 Atmega644p)

RXD <--> pin 4 (TXD1 Atmega644p)

Data is sent at every 20 ms @ 57600 Baud 8-N-1

************************************************************************************************************

Data Frame Jennic: |0xFF|0xFF|STATUS|CHN|CH0D1|CH0D0|CH1D1|CH1D0|CRC|

STATUS = Jennic Network Status (0 = OK, 1 = Coordinator not available, 2 = General Error)

CHN = Channel Pair (0 = Channel 0 & 1 ...etc)

Channel Pair: |CHN|CHnD1|CHnD0|CHnD1|CHnD0|CRC|

CHN is the lower Channel index of the Channel Pair

n is the Channel

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 example with signal received:

FFFF 00 0036012B1E7f

FFFF 00 022AFB2AECC2

FFFF 00 042B4D2B4414

FFFF 00 0636872B33DE

Frame example with no signal received:

FFFF 01 (Network Coordinator not Accessible)

or

FFFF 02 (General Error)

***********************************************************************************************************/

#include <stdlib.h>

#include "jennic.h"

#include "rc.h"

#include "uart0.h"

uint8_t jen_status = 0;

uint8_t PacketBuffer[9];

typedef union

{

    int16_t Servo[2];

    uint8_t  byte[4];

} ChannelPair_t;

ChannelPair_t ChannelPair;

// to be called within the UART RX ISR

void jen_parser (uint8_t c)

{

          static uint8_t last_c = 0;

    static uint8_t crc  = 0;

    static uint8_t cnt = 0;

    uint8_t packet_len = 5;

    // sync condition

    if ((c==0xFF) && (last_c==0xFF))

    {

        cnt = 0; // reset counter

        crc = 0; // reset checksum

        return;

    }

    if (cnt > packet_len) // packet complete, crc received

    {

        // calculate checksum

        crc = ~crc;

        if (crc == 0xFF) crc = 0xFE;

        // decode packet if checksum correct

        if (c == crc) jen_decode_packet ();

        // handle next packet

        cnt = 0;

        crc = 0;

    }

    else // collect channel data bytes

    {

        PacketBuffer[cnt++] = c;

        crc += c;

    }

    last_c = c; // store last byte for sync check

}

void jen_decode_packet (void)

{

    uint8_t  i;

    jen_status = PacketBuffer[0];

    // header condition

    if (!(PacketBuffer[1] & 0xF0))

    {

        if (!jen_status) RC_Quality = 200;

        else if (jen_status)

        {

            RC_Quality = 0;

            for (i = 0; i<5; i++)

            {

                PPM_diff[i] = 0;

                PPM_in[i] = 0;

            }

        }

        i = PacketBuffer[1] & 0x0F;   // lower nibble header indicates the channel pair

        if (i < 10) // maximum 12 channels

        {

            // big to little endian

            ChannelPair.byte[1] = PacketBuffer[2];

            ChannelPair.byte[0] = PacketBuffer[3];

            ChannelPair.byte[3] = PacketBuffer[4];

            ChannelPair.byte[2] = PacketBuffer[5];

            // new servo signal properly received

            jen_new_signal (i,  ChannelPair.Servo[0]);

            jen_new_signal (i+1, ChannelPair.Servo[1]);

        }

    } // end of header condition

}

// Parameters: channel  - channel number (0-9)

//             signal     - signal between 7363 and 14875

void jen_new_signal (uint8_t channel, int16_t signal)

{

    int16_t tmp;

    uint8_t index = channel + 1; // mk channels start with 1

    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++;

    if (RC_Quality == 200)  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 < 5) DebugOut.Analog[(20+index)] = PPM_in[index]; // debug output is handy whilst developing

    if (index == 4) NewPpmData = 0;

}