Subversion Repositories FlightCtrl

/*#######################################################################################
Decodieren eines RC Summen Signals
#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + only for non-profit use
// + www.MikroKopter.com
// + see the File "License.txt" for further Informations
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

#include "rc.h"
#include "main.h"

volatile int PPM_in[11];
int RCQuality = 0;
volatile unsigned char NewPpmData = 1;

//############################################################################
//zum decodieren des PPM-Signals wird Timer1 mit seiner Input
//Capture Funktion benutzt:
void rc_sum_init (void)
//############################################################################
{
        TCCR1B=(1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1); //timer1 prescale 64
    TIMSK1 |= _BV(ICIE1);
    return;
}

//############################################################################
//Diese Routine startet und inizialisiert den Timer für RC
SIGNAL(SIG_INPUT_CAPTURE1)
//############################################################################
{
        static unsigned int AltICR=0;
    signed int signal = 0,tmp;
        static int index;              
        static float RC_Quality = 0.0F;        
    static int PPM_org[11];

        signal = (unsigned int) ICR1 - AltICR;         
        AltICR = ICR1; 
       
    //Syncronisationspause?
        if ((signal > 1500) && (signal < 8000))  
    {
        index = 1;             
        NewPpmData = 0;  // Null bedeutet: Neue Daten
    }
        else
    {
        if(index < 10)
        {
            if((signal > 250) && (signal < 687))
            {
                signal -= 466;
                // Stabiles Signal     
                                if(abs(signal - PPM_in[index]) < 6)
                                {
                                        if(SenderOkay < 200)
                                        {
                                                SenderOkay += 10;
                                        }
                                }
                                /* Give an estimate for the Signal Level based on the RC-Jitter */
                                if (abs(2 * (signal - PPM_org[index]) > (int) RC_Quality))
                                {
                                        RC_Quality = 0.99F * RC_Quality + 0.01F * (float) abs(2 * (signal - PPM_org[index])) ;                         
                                }
                                else
                                {
                                        RC_Quality = 0.998F * RC_Quality + 0.002F * (float) abs(2 * (signal - PPM_org[index])) ;
                                }              
                                tmp = (3 * (PPM_in[index]) + signal) / 4;  
                                PPM_in[index] = tmp;
                                PPM_org[index] = signal;
                        }        
                        else
                        {
                                        RC_Quality = 0.95F * RC_Quality + 0.05F * 100;         
                        }
                        RC_Quality = MIN(100.F, RC_Quality);
                        RCQuality = 100 - (int) RC_Quality;        
                        index++;  
                        if(index == 5) PORTD |= 0x20; else PORTD &= ~0x20;  // Servosignal an J3 anlegen
                        if(index == 6) PORTD |= 0x10; else PORTD &= ~0x10;  // Servosignal an J4 anlegen
                        if(index == 7) PORTD |= 0x08; else PORTD &= ~0x08;  // Servosignal an J5 anlegen
        }
        }
}