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