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"

int PPM_in[11] = {0,0,0,0,0,0,0,0,0,0,0};
int RCQuality = 0;
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 see
        http://forum.mikrokopter.de/topic-post44807.html#post44807*/
        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;
      DebugOut.Analog[12] = RCQuality;
      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
    }
  }
}