WebSVN - FlightCtrl - Diff - Rev 1171 and 1224


#include "main.h"
 
volatile unsigned int CountMilliseconds = 0;
volatile static unsigned int tim_main;
volatile unsigned char UpdateMotor = 0;
volatile unsigned int cntKompass = 0;
volatile unsigned int beeptime = 0;
volatile unsigned char SendSPI = 0, ServoActive = 0;
 
unsigned int BeepMuster = 0xffff;
int ServoValue = 0;
 
volatile int16_t        ServoNickValue = 0;
volatile int16_t        ServoRollValue = 0;
 
 
enum {
  STOP             = 0,
  CK               = 1,
  CK8              = 2,
  CK64             = 3,
  CK256            = 4,
  CK1024           = 5,
  T0_FALLING_EDGE  = 6,
  T0_RISING_EDGE   = 7
};
 
 
SIGNAL (SIG_OVERFLOW0)    // 8kHz
{
    static unsigned char cnt_1ms = 1,cnt = 0;
    unsigned char pieper_ein = 0;
//    TCNT0 -= 250;//TIMER_RELOAD_VALUE;
   if(SendSPI) SendSPI--;
   if(!cnt--)
    {
     cnt = 9;
     cnt_1ms++;
     cnt_1ms %= 2;
     if(!cnt_1ms) UpdateMotor = 1;
     CountMilliseconds++;
    }
 
     if(beeptime > 1)
        {
        beeptime--;
        if(beeptime & BeepMuster)
         {
          pieper_ein = 1;
         }
         else pieper_ein = 0;
        }
     else
      {
       pieper_ein = 0;
       BeepMuster = 0xffff;
      }
 
 
     if(pieper_ein)
        {
          if(PlatinenVersion == 10) PORTD |= (1<<2); // Speaker an PORTD.2
          else                      PORTC |= (1<<7); // Speaker an PORTC.7
        }
     else
        {
         if(PlatinenVersion == 10) PORTD &= ~(1<<2);
         else                      PORTC &= ~(1<<7);
        }
 
 if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
 {
  if(PINC & 0x10)
   {
    cntKompass++;
   }
  else
   {
    if((cntKompass) && (cntKompass < 362))
    {
     cntKompass += cntKompass / 41;
     if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;
    }
//     if(cntKompass < 10) cntKompass = 10;
//     KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
     KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
    cntKompass = 0;
   }
 }
}
 
 
// -----------------------------------------------------------------------
 
unsigned int SetDelay (unsigned int t)
{
//  TIMSK0 &= ~_BV(TOIE0);
  return(CountMilliseconds + t + 1);
//  TIMSK0 |= _BV(TOIE0);
}
 
// -----------------------------------------------------------------------
char CheckDelay(unsigned int t)
{
//  TIMSK0 &= ~_BV(TOIE0);
  return(((t - CountMilliseconds) & 0x8000) >> 9);
//  TIMSK0 |= _BV(TOIE0);
}
 
// -----------------------------------------------------------------------
void Delay_ms(unsigned int w)
{
 unsigned int akt;
 akt = SetDelay(w);
 while (!CheckDelay(akt));
}
 
void Delay_ms_Mess(unsigned int w)
{
 unsigned int akt;
 akt = SetDelay(w);
 while (!CheckDelay(akt)) if(AdReady) {AdReady = 0; ANALOG_ON;}
}
 
/*****************************************************/
/*              Initialize Timer 2                   */
/*****************************************************/
// The timer 2 is used to generate the PWM at PD7 (J7)
// to control a camera servo for nick compensation.
void TIMER2_Init(void)
{
        uint8_t sreg = SREG;
 
        // disable all interrupts before reconfiguration
        cli();
 
        // set PD7 as output of the PWM for nick servo
        DDRD  |= (1<<DDD7);
        PORTD &= ~(1<<PORTD7);  // set PD7 to low
 
        DDRC  |= (1<<DDC6);     // set PC6 as output (Reset for HEF4017)
    HEF4017R_ON;
        // Timer/Counter 2 Control Register A
 
        // Timer Mode is FastPWM with timer reload at OCR2A (Bits: WGM22 = 1, WGM21 = 1, WGM20 = 1)
    // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)
    // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
        TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0));
    TCCR2A |= (1<<WGM21)|(1<<WGM20);
 
    // Timer/Counter 2 Control Register B
 
        // Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz
        // The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us
        // hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms
 
    // divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1)
        TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS22));
    TCCR2B |= (1<<CS21)|(1<<CS20)|(1<<WGM22);
 
        // Initialize the Timer/Counter 2 Register
    TCNT2 = 0;
 
        // Initialize the Output Compare Register A used for PWM generation on port PD7.
        OCR2A = 255;
        TCCR2A |= (1<<COM2A1); // set or clear at compare match depends on value of COM2A0
 
        // Timer/Counter 2 Interrupt Mask Register
        // Enable timer output compare match A Interrupt only
        TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2));
        TIMSK2 |= (1<<OCIE2A);
 
    SREG = sreg;
}
 
//----------------------------
void Timer_Init(void)
{
    tim_main = SetDelay(10);
    TCCR0B = CK8;
    TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM
    OCR0A =  0;
    OCR0B = 120;
    TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE;  // reload
    //OCR1  = 0x00;
 
    TIMSK0 |= _BV(TOIE0);
}
 
 
/*****************************************************/
/*              Control Servo Position               */
/*****************************************************/
 
ISR(TIMER2_COMPA_vect)
{
 
        // frame len 22.5 ms = 14063 * 1.6 us
        // stop pulse: 0.3 ms = 188 * 1.6 us
        // min servo pulse: 0.6 ms =  375 * 1.6 us
        // max servo pulse: 2.4 ms = 1500 * 1.6 us
        // resolution: 1500 - 375 = 1125 steps
 
        #define IRS_RUNTIME 127
        #define PPM_STOPPULSE 188
//      #define PPM_FRAMELEN (14063
    #define PPM_FRAMELEN (1757 * EE_Parameter.ServoNickRefresh)
        #define MINSERVOPULSE 375
        #define MAXSERVOPULSE 1500
        #define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE)
 
        static uint8_t  PulseOutput = 0;
        static uint16_t RemainingPulse = 0;
        static uint16_t ServoFrameTime = 0;
        static uint8_t  ServoIndex = 0;
 
        #define MULTIPLYER 4
        static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
        static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
 
        if(PlatinenVersion < 20)
        {
                //---------------------------
                // Nick servo state machine
                //---------------------------
                if(!PulseOutput) // pulse output complete
                {
                        if(TCCR2A & (1<<COM2A0)) // we had a low pulse
                        {
                                TCCR2A &= ~(1<<COM2A0);// make a high pulse
                                RemainingPulse  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
 
                                ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
                                ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
                                if(EE_Parameter.ServoNickCompInvert & 0x01)
                                {       // inverting movement of servo
                                        ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
                                }
                                else
                                {       // non inverting movement of servo
                                        ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
                                }
                                // limit servo value to its parameter range definition
                                if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
                                {
                                        ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
                                }
                                else
                                if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
                                {
                                        ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
                                }
 
                                RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
 
                                ServoNickValue /= MULTIPLYER;
                                DebugOut.Analog[20] = ServoNickValue;
 
                                // range servo pulse width
                                if(RemainingPulse > MAXSERVOPULSE )                     RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
                                else if(RemainingPulse < MINSERVOPULSE )        RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
                                // accumulate time for correct update rate
                                ServoFrameTime = RemainingPulse;
                        }
                        else // we had a high pulse
                        {
                                TCCR2A |= (1<<COM2A0); // make a low pulse
                                RemainingPulse = PPM_FRAMELEN - ServoFrameTime;
                        }
                        // set pulse output active
                        PulseOutput = 1;
                }
        } // EOF Nick servo state machine
        else
        {
                //-----------------------------------------------------
                // PPM state machine, onboard demultiplexed by HEF4017
                //-----------------------------------------------------
                if(!PulseOutput) // pulse output complete
                {
                        if(TCCR2A & (1<<COM2A0)) // we had a low pulse
                        {
                                TCCR2A &= ~(1<<COM2A0);// make a high pulse
 
                                if(ServoIndex == 0) // if we are at the sync gap
                                {
                                        RemainingPulse = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time
                                        ServoFrameTime = 0; // reset servo frame time
                                        HEF4017R_ON; // enable HEF4017 reset
                                }
                                else // servo channels
                                {
                                        RemainingPulse  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
                                        switch(ServoIndex) // map servo channels
                                        {
                                                case 1: // Nick Compensation Servo
                                                        ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
                                                        ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
                                                        if(EE_Parameter.ServoNickCompInvert & 0x01)
                                                        {       // inverting movement of servo
                                                                ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
                                                        }
                                                        else
                                                        {       // non inverting movement of servo
                                                                ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
                                                        }
                                                        // limit servo value to its parameter range definition
                                                        if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
                                                        {
                                                                ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
                                                        }
                                                        else
                                                        if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
                                                        {
                                                                ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
                                                        }
                                                        RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
                                                        ServoNickValue /= MULTIPLYER;
                                                        DebugOut.Analog[20] = ServoNickValue;
                                                        break;
                                         case 2: // Roll Compensation Servo
                                                        ServoRollOffset = (ServoRollOffset * 3 + (int16_t) 80 * MULTIPLYER) / 4; // lowpass offset
                                                        ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
                                                        //if(EE_Parameter.ServoRollCompInvert & 0x01)
                                                        {       // inverting movement of servo
                                                                ServoRollValue += (int16_t)( ( (int32_t) 50 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
                                                        }
/*                                                      else
                                                        {       // non inverting movement of servo
                                                                ServoRollValue -= (int16_t)( ( (int32_t) 40 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
                                                        }
*/                                                      // limit servo value to its parameter range definition
                                                        if(ServoRollValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
                                                        {
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
                                                        }
                                                        else
                                                        if(ServoRollValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
                                                        {
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
                                                        }
                                                        RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
                                                        ServoRollValue /= MULTIPLYER;
                                                        //DebugOut.Analog[20] = ServoRollValue;
 
/*                                                      ServoRollOffset = (ServoRollOffset * 3 + (int16_t)Parameter_ServoRollControl * MULTIPLYER) / 4; // lowpass offset
                                                        ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
                                                        if(EE_Parameter.ServoRollCompInvert & 0x01)
                                                        {       // inverting movement of servo
                                                                ServoRollValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
                                                        }
                                                        else
                                                        {       // non inverting movement of servo
                                                                ServoRollValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
                                                        }
                                                        // limit servo value to its parameter range definition
                                                        if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) )
                                                        {
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER;
                                                        }
                                                        else
                                                        if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) )
                                                        {
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER;
                                                        }
                                                        RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
                                                        ServoRollValue /= MULTIPLYER;
                                                        //DebugOut.Analog[20] = ServoRollValue;
*/                                                      break;
 
                                                default: // other servo channels
                                                        RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs
                                                        break;
                                        }
                                        // range servo pulse width
                                        if(RemainingPulse > MAXSERVOPULSE )                     RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
                                        else if(RemainingPulse < MINSERVOPULSE )        RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
                                        // substract stop pulse width
                                        RemainingPulse -= PPM_STOPPULSE;
                                        // accumulate time for correct sync gap
                                        ServoFrameTime += RemainingPulse;
                                }
                        }
                        else // we had a high pulse
                        {
                                TCCR2A |= (1<<COM2A0); // make a low pulse
                                // set pulsewidth to stop pulse width
                                RemainingPulse = PPM_STOPPULSE;
                                // accumulate time for correct sync gap
                                ServoFrameTime += RemainingPulse;
                                if(ServoActive && SenderOkay > 180) HEF4017R_OFF; // disable HEF4017 reset
                                ServoIndex++; // change to next servo channel
                                if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap
                        }
                        // set pulse output active
                        PulseOutput = 1;
                }
        } // EOF PPM state machine
 
        // General pulse output generator
        if(RemainingPulse > (255 + IRS_RUNTIME))
        {
                OCR2A = 255;
                RemainingPulse -= 255;
        }
        else
        {
                if(RemainingPulse > 255) // this is the 2nd last part
                {
                        if((RemainingPulse - 255) < IRS_RUNTIME)
                        {
                                OCR2A = 255 - IRS_RUNTIME;
                                RemainingPulse -= 255 - IRS_RUNTIME;
 
                        }
                        else // last part > ISR_RUNTIME
                        {
                                OCR2A = 255;
                                RemainingPulse -= 255;
                        }
                }
                else // this is the last part
                {
                        OCR2A = RemainingPulse;
                        RemainingPulse = 0;
                        PulseOutput = 0; // trigger to stop pulse
                }
        } // EOF general pulse output generator
}
 

Rev 1171	Rev 1224
1	#include "main.h"	1	#include "main.h"
2		2
3	volatile unsigned int CountMilliseconds = 0;	3	volatile unsigned int CountMilliseconds = 0;
4	volatile static unsigned int tim_main;	4	volatile static unsigned int tim_main;
5	volatile unsigned char UpdateMotor = 0;	5	volatile unsigned char UpdateMotor = 0;
6	volatile unsigned int cntKompass = 0;	6	volatile unsigned int cntKompass = 0;
7	volatile unsigned int beeptime = 0;	7	volatile unsigned int beeptime = 0;
8	volatile unsigned char SendSPI = 0, ServoActive = 0;	8	volatile unsigned char SendSPI = 0, ServoActive = 0;
9		9
10	unsigned int BeepMuster = 0xffff;	10	unsigned int BeepMuster = 0xffff;
11	int ServoValue = 0;	11	int ServoValue = 0;
12		12
13	volatile int16_t ServoNickValue = 0;	13	volatile int16_t ServoNickValue = 0;
14	volatile int16_t ServoRollValue = 0;	14	volatile int16_t ServoRollValue = 0;
15		15
16		16
17	enum {	17	enum {
18	STOP = 0,	18	STOP = 0,
19	CK = 1,	19	CK = 1,
20	CK8 = 2,	20	CK8 = 2,
21	CK64 = 3,	21	CK64 = 3,
22	CK256 = 4,	22	CK256 = 4,
23	CK1024 = 5,	23	CK1024 = 5,
24	T0_FALLING_EDGE = 6,	24	T0_FALLING_EDGE = 6,
25	T0_RISING_EDGE = 7	25	T0_RISING_EDGE = 7
26	};	26	};
27		27
28		28
29	SIGNAL (SIG_OVERFLOW0) // 8kHz	29	SIGNAL (SIG_OVERFLOW0) // 8kHz
30	{	30	{
31	static unsigned char cnt_1ms = 1,cnt = 0;	31	static unsigned char cnt_1ms = 1,cnt = 0;
32	unsigned char pieper_ein = 0;	32	unsigned char pieper_ein = 0;
33	// TCNT0 -= 250;//TIMER_RELOAD_VALUE;	33	// TCNT0 -= 250;//TIMER_RELOAD_VALUE;
34	if(SendSPI) SendSPI--;	34	if(SendSPI) SendSPI--;
35	if(!cnt--)	35	if(!cnt--)
36	{	36	{
37	cnt = 9;	37	cnt = 9;
38	cnt_1ms++;	38	cnt_1ms++;
39	cnt_1ms %= 2;	39	cnt_1ms %= 2;
40	if(!cnt_1ms) UpdateMotor = 1;	40	if(!cnt_1ms) UpdateMotor = 1;
41	CountMilliseconds++;	41	CountMilliseconds++;
42	}	42	}
43		43
44	if(beeptime > 1)	44	if(beeptime > 1)
45	{	45	{
46	beeptime--;	46	beeptime--;
47	if(beeptime & BeepMuster)	47	if(beeptime & BeepMuster)
48	{	48	{
49	pieper_ein = 1;	49	pieper_ein = 1;
50	}	50	}
51	else pieper_ein = 0;	51	else pieper_ein = 0;
52	}	52	}
53	else	53	else
54	{	54	{
55	pieper_ein = 0;	55	pieper_ein = 0;
56	BeepMuster = 0xffff;	56	BeepMuster = 0xffff;
57	}	57	}
58		58
59		59
60	if(pieper_ein)	60	if(pieper_ein)
61	{	61	{
62	if(PlatinenVersion == 10) PORTD \|= (1<<2); // Speaker an PORTD.2	62	if(PlatinenVersion == 10) PORTD \|= (1<<2); // Speaker an PORTD.2
63	else PORTC \|= (1<<7); // Speaker an PORTC.7	63	else PORTC \|= (1<<7); // Speaker an PORTC.7
64	}	64	}
65	else	65	else
66	{	66	{
67	if(PlatinenVersion == 10) PORTD &= ~(1<<2);	67	if(PlatinenVersion == 10) PORTD &= ~(1<<2);
68	else PORTC &= ~(1<<7);	68	else PORTC &= ~(1<<7);
69	}	69	}
70		70
71	if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)	71	if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
72	{	72	{
73	if(PINC & 0x10)	73	if(PINC & 0x10)
74	{	74	{
75	cntKompass++;	75	cntKompass++;
76	}	76	}
77	else	77	else
78	{	78	{
79	if((cntKompass) && (cntKompass < 362))	79	if((cntKompass) && (cntKompass < 362))
80	{	80	{
81	cntKompass += cntKompass / 41;	81	cntKompass += cntKompass / 41;
82	if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;	82	if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;
83	}	83	}
84	// if(cntKompass < 10) cntKompass = 10;	84	// if(cntKompass < 10) cntKompass = 10;
85	// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;	85	// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
86	KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;	86	KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
87	cntKompass = 0;	87	cntKompass = 0;
88	}	88	}
89	}	89	}
90	}	90	}
91		91
92		92
93	// -----------------------------------------------------------------------	93	// -----------------------------------------------------------------------
94		94
95	unsigned int SetDelay (unsigned int t)	95	unsigned int SetDelay (unsigned int t)
96	{	96	{
97	// TIMSK0 &= ~_BV(TOIE0);	97	// TIMSK0 &= ~_BV(TOIE0);
98	return(CountMilliseconds + t + 1);	98	return(CountMilliseconds + t + 1);
99	// TIMSK0 \|= _BV(TOIE0);	99	// TIMSK0 \|= _BV(TOIE0);
100	}	100	}
101		101
102	// -----------------------------------------------------------------------	102	// -----------------------------------------------------------------------
103	char CheckDelay(unsigned int t)	103	char CheckDelay(unsigned int t)
104	{	104	{
105	// TIMSK0 &= ~_BV(TOIE0);	105	// TIMSK0 &= ~_BV(TOIE0);
106	return(((t - CountMilliseconds) & 0x8000) >> 9);	106	return(((t - CountMilliseconds) & 0x8000) >> 9);
107	// TIMSK0 \|= _BV(TOIE0);	107	// TIMSK0 \|= _BV(TOIE0);
108	}	108	}
109		109
110	// -----------------------------------------------------------------------	110	// -----------------------------------------------------------------------
111	void Delay_ms(unsigned int w)	111	void Delay_ms(unsigned int w)
112	{	112	{
113	unsigned int akt;	113	unsigned int akt;
114	akt = SetDelay(w);	114	akt = SetDelay(w);
115	while (!CheckDelay(akt));	115	while (!CheckDelay(akt));
116	}	116	}
117		117
118	void Delay_ms_Mess(unsigned int w)	118	void Delay_ms_Mess(unsigned int w)
119	{	119	{
120	unsigned int akt;	120	unsigned int akt;
121	akt = SetDelay(w);	121	akt = SetDelay(w);
122	while (!CheckDelay(akt)) if(AdReady) {AdReady = 0; ANALOG_ON;}	122	while (!CheckDelay(akt)) if(AdReady) {AdReady = 0; ANALOG_ON;}
123	}	123	}
124		124
125	/*****************************************************/	125	/*****************************************************/
126	/* Initialize Timer 2 */	126	/* Initialize Timer 2 */
127	/*****************************************************/	127	/*****************************************************/
128	// The timer 2 is used to generate the PWM at PD7 (J7)	128	// The timer 2 is used to generate the PWM at PD7 (J7)
129	// to control a camera servo for nick compensation.	129	// to control a camera servo for nick compensation.
130	void TIMER2_Init(void)	130	void TIMER2_Init(void)
131	{	131	{
132	uint8_t sreg = SREG;	132	uint8_t sreg = SREG;
133		133
134	// disable all interrupts before reconfiguration	134	// disable all interrupts before reconfiguration
135	cli();	135	cli();
136		136
137	// set PD7 as output of the PWM for nick servo	137	// set PD7 as output of the PWM for nick servo
138	DDRD \|= (1<<DDD7);	138	DDRD \|= (1<<DDD7);
139	PORTD &= ~(1<<PORTD7); // set PD7 to low	139	PORTD &= ~(1<<PORTD7); // set PD7 to low
140		140
141	DDRC \|= (1<<DDC6); // set PC6 as output (Reset for HEF4017)	141	DDRC \|= (1<<DDC6); // set PC6 as output (Reset for HEF4017)
142	HEF4017R_ON;	142	HEF4017R_ON;
143	// Timer/Counter 2 Control Register A	143	// Timer/Counter 2 Control Register A
144		144
145	// Timer Mode is FastPWM with timer reload at OCR2A (Bits: WGM22 = 1, WGM21 = 1, WGM20 = 1)	145	// Timer Mode is FastPWM with timer reload at OCR2A (Bits: WGM22 = 1, WGM21 = 1, WGM20 = 1)
146	// PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)	146	// PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)
147	// PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)	147	// PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
148	TCCR2A &= ~((1<<COM2A1)\|(1<<COM2A0)\|(1<<COM2B1)\|(1<<COM2B0));	148	TCCR2A &= ~((1<<COM2A1)\|(1<<COM2A0)\|(1<<COM2B1)\|(1<<COM2B0));
149	TCCR2A \|= (1<<WGM21)\|(1<<WGM20);	149	TCCR2A \|= (1<<WGM21)\|(1<<WGM20);
150		150
151	// Timer/Counter 2 Control Register B	151	// Timer/Counter 2 Control Register B
152		152
153	// Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz	153	// Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz
154	// The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us	154	// The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us
155	// hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms	155	// hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms
156		156
157	// divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1)	157	// divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1)
158	TCCR2B &= ~((1<<FOC2A)\|(1<<FOC2B)\|(1<<CS22));	158	TCCR2B &= ~((1<<FOC2A)\|(1<<FOC2B)\|(1<<CS22));
159	TCCR2B \|= (1<<CS21)\|(1<<CS20)\|(1<<WGM22);	159	TCCR2B \|= (1<<CS21)\|(1<<CS20)\|(1<<WGM22);
160		160
161	// Initialize the Timer/Counter 2 Register	161	// Initialize the Timer/Counter 2 Register
162	TCNT2 = 0;	162	TCNT2 = 0;
163		163
164	// Initialize the Output Compare Register A used for PWM generation on port PD7.	164	// Initialize the Output Compare Register A used for PWM generation on port PD7.
165	OCR2A = 255;	165	OCR2A = 255;
166	TCCR2A \|= (1<<COM2A1); // set or clear at compare match depends on value of COM2A0	166	TCCR2A \|= (1<<COM2A1); // set or clear at compare match depends on value of COM2A0
167		167
168	// Timer/Counter 2 Interrupt Mask Register	168	// Timer/Counter 2 Interrupt Mask Register
169	// Enable timer output compare match A Interrupt only	169	// Enable timer output compare match A Interrupt only
170	TIMSK2 &= ~((1<<OCIE2B)\|(1<<TOIE2));	170	TIMSK2 &= ~((1<<OCIE2B)\|(1<<TOIE2));
171	TIMSK2 \|= (1<<OCIE2A);	171	TIMSK2 \|= (1<<OCIE2A);
172		172
173	SREG = sreg;	173	SREG = sreg;
174	}	174	}
175		175
176	//----------------------------	176	//----------------------------
177	void Timer_Init(void)	177	void Timer_Init(void)
178	{	178	{
179	tim_main = SetDelay(10);	179	tim_main = SetDelay(10);
180	TCCR0B = CK8;	180	TCCR0B = CK8;
181	TCCR0A = (1<<COM0A1)\|(1<<COM0B1)\|3;//fast PWM	181	TCCR0A = (1<<COM0A1)\|(1<<COM0B1)\|3;//fast PWM
182	OCR0A = 0;	182	OCR0A = 0;
183	OCR0B = 120;	183	OCR0B = 120;
184	TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE; // reload	184	TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE; // reload
185	//OCR1 = 0x00;	185	//OCR1 = 0x00;
186		186
187	TIMSK0 \|= _BV(TOIE0);	187	TIMSK0 \|= _BV(TOIE0);
188	}	188	}
189		189
190		190
191	/*****************************************************/	191	/*****************************************************/
192	/* Control Servo Position */	192	/* Control Servo Position */
193	/*****************************************************/	193	/*****************************************************/
194		194
195	ISR(TIMER2_COMPA_vect)	195	ISR(TIMER2_COMPA_vect)
196	{	196	{
197		197
198	// frame len 22.5 ms = 14063 * 1.6 us	198	// frame len 22.5 ms = 14063 * 1.6 us
199	// stop pulse: 0.3 ms = 188 * 1.6 us	199	// stop pulse: 0.3 ms = 188 * 1.6 us
200	// min servo pulse: 0.6 ms = 375 * 1.6 us	200	// min servo pulse: 0.6 ms = 375 * 1.6 us
201	// max servo pulse: 2.4 ms = 1500 * 1.6 us	201	// max servo pulse: 2.4 ms = 1500 * 1.6 us
202	// resolution: 1500 - 375 = 1125 steps	202	// resolution: 1500 - 375 = 1125 steps
203		203
204	#define IRS_RUNTIME 127	204	#define IRS_RUNTIME 127
205	#define PPM_STOPPULSE 188	205	#define PPM_STOPPULSE 188
206	// #define PPM_FRAMELEN (14063	206	// #define PPM_FRAMELEN (14063
207	#define PPM_FRAMELEN (1757 * EE_Parameter.ServoNickRefresh)	207	#define PPM_FRAMELEN (1757 * EE_Parameter.ServoNickRefresh)
208	#define MINSERVOPULSE 375	208	#define MINSERVOPULSE 375
209	#define MAXSERVOPULSE 1500	209	#define MAXSERVOPULSE 1500
210	#define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE)	210	#define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE)
211		211
212	static uint8_t PulseOutput = 0;	212	static uint8_t PulseOutput = 0;
213	static uint16_t RemainingPulse = 0;	213	static uint16_t RemainingPulse = 0;
214	static uint16_t ServoFrameTime = 0;	214	static uint16_t ServoFrameTime = 0;
215	static uint8_t ServoIndex = 0;	215	static uint8_t ServoIndex = 0;
216		216
217	#define MULTIPLYER 4	217	#define MULTIPLYER 4
218	static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon	218	static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
-		219	static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
219		220
220	if(PlatinenVersion < 20)	221	if(PlatinenVersion < 20)
221	{	222	{
222	//---------------------------	223	//---------------------------
223	// Nick servo state machine	224	// Nick servo state machine
224	//---------------------------	225	//---------------------------
225	if(!PulseOutput) // pulse output complete	226	if(!PulseOutput) // pulse output complete
226	{	227	{
227	if(TCCR2A & (1<<COM2A0)) // we had a low pulse	228	if(TCCR2A & (1<<COM2A0)) // we had a low pulse
228	{	229	{
229	TCCR2A &= ~(1<<COM2A0);// make a high pulse	230	TCCR2A &= ~(1<<COM2A0);// make a high pulse
230	RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms	231	RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
231		232
232	ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset	233	ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
233	ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)	234	ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
234	if(EE_Parameter.ServoNickCompInvert & 0x01)	235	if(EE_Parameter.ServoNickCompInvert & 0x01)
235	{ // inverting movement of servo	236	{ // inverting movement of servo
236	ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );	237	ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
237	}	238	}
238	else	239	else
239	{ // non inverting movement of servo	240	{ // non inverting movement of servo
240	ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );	241	ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
241	}	242	}
242	// limit servo value to its parameter range definition	243	// limit servo value to its parameter range definition
243	if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )	244	if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
244	{	245	{
245	ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;	246	ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
246	}	247	}
247	else	248	else
248	if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )	249	if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
249	{	250	{
250	ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;	251	ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
251	}	252	}
252		253
253	RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position	254	RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
254		255
255	ServoNickValue /= MULTIPLYER;	256	ServoNickValue /= MULTIPLYER;
256	DebugOut.Analog[20] = ServoNickValue;	257	DebugOut.Analog[20] = ServoNickValue;
257		258
258	// range servo pulse width	259	// range servo pulse width
259	if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit	260	if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
260	else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit	261	else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
261	// accumulate time for correct update rate	262	// accumulate time for correct update rate
262	ServoFrameTime = RemainingPulse;	263	ServoFrameTime = RemainingPulse;
263	}	264	}
264	else // we had a high pulse	265	else // we had a high pulse
265	{	266	{
266	TCCR2A \|= (1<<COM2A0); // make a low pulse	267	TCCR2A \|= (1<<COM2A0); // make a low pulse
267	RemainingPulse = PPM_FRAMELEN - ServoFrameTime;	268	RemainingPulse = PPM_FRAMELEN - ServoFrameTime;
268	}	269	}
269	// set pulse output active	270	// set pulse output active
270	PulseOutput = 1;	271	PulseOutput = 1;
271	}	272	}
272	} // EOF Nick servo state machine	273	} // EOF Nick servo state machine
273	else	274	else
274	{	275	{
275	//-----------------------------------------------------	276	//-----------------------------------------------------
276	// PPM state machine, onboard demultiplexed by HEF4017	277	// PPM state machine, onboard demultiplexed by HEF4017
277	//-----------------------------------------------------	278	//-----------------------------------------------------
278	if(!PulseOutput) // pulse output complete	279	if(!PulseOutput) // pulse output complete
279	{	280	{
280	if(TCCR2A & (1<<COM2A0)) // we had a low pulse	281	if(TCCR2A & (1<<COM2A0)) // we had a low pulse
281	{	282	{
282	TCCR2A &= ~(1<<COM2A0);// make a high pulse	283	TCCR2A &= ~(1<<COM2A0);// make a high pulse
283		284
284	if(ServoIndex == 0) // if we are at the sync gap	285	if(ServoIndex == 0) // if we are at the sync gap
285	{	286	{
286	RemainingPulse = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time	287	RemainingPulse = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time
287	ServoFrameTime = 0; // reset servo frame time	288	ServoFrameTime = 0; // reset servo frame time
288	HEF4017R_ON; // enable HEF4017 reset	289	HEF4017R_ON; // enable HEF4017 reset
289	}	290	}
290	else // servo channels	291	else // servo channels
291	{	292	{
292	RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms	293	RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
293	switch(ServoIndex) // map servo channels	294	switch(ServoIndex) // map servo channels
294	{	295	{
295	case 1: // Nick Compensation Servo	296	case 1: // Nick Compensation Servo
296	ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset	297	ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
297	ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)	298	ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
298	if(EE_Parameter.ServoNickCompInvert & 0x01)	299	if(EE_Parameter.ServoNickCompInvert & 0x01)
299	{ // inverting movement of servo	300	{ // inverting movement of servo
300	ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );	301	ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
301	}	302	}
302	else	303	else
303	{ // non inverting movement of servo	304	{ // non inverting movement of servo
304	ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );	305	ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
305	}	306	}
306	// limit servo value to its parameter range definition	307	// limit servo value to its parameter range definition
307	if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )	308	if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
308	{	309	{
309	ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;	310	ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
310	}	311	}
311	else	312	else
312	if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )	313	if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
313	{	314	{
314	ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;	315	ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
315	}	316	}
316	RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position	317	RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
317	ServoNickValue /= MULTIPLYER;	318	ServoNickValue /= MULTIPLYER;
318	DebugOut.Analog[20] = ServoNickValue;	319	DebugOut.Analog[20] = ServoNickValue;
319	break;	320	break;
-		321	case 2: // Roll Compensation Servo
-		322	ServoRollOffset = (ServoRollOffset * 3 + (int16_t) 80 * MULTIPLYER) / 4; // lowpass offset
-		323	ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
-		324	//if(EE_Parameter.ServoRollCompInvert & 0x01)
-		325	{ // inverting movement of servo
-		326	ServoRollValue += (int16_t)( ( (int32_t) 50 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
-		327	}
-		328	/* else
-		329	{ // non inverting movement of servo
-		330	ServoRollValue -= (int16_t)( ( (int32_t) 40 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
-		331	}
-		332	*/ // limit servo value to its parameter range definition
-		333	if(ServoRollValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
-		334	{
-		335	ServoRollValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
-		336	}
-		337	else
-		338	if(ServoRollValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
-		339	{
-		340	ServoRollValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
-		341	}
-		342	RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
-		343	ServoRollValue /= MULTIPLYER;
-		344	//DebugOut.Analog[20] = ServoRollValue;
-		345
-		346	/* ServoRollOffset = (ServoRollOffset * 3 + (int16_t)Parameter_ServoRollControl * MULTIPLYER) / 4; // lowpass offset
-		347	ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
-		348	if(EE_Parameter.ServoRollCompInvert & 0x01)
-		349	{ // inverting movement of servo
-		350	ServoRollValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
-		351	}
-		352	else
-		353	{ // non inverting movement of servo
-		354	ServoRollValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
-		355	}
-		356	// limit servo value to its parameter range definition
-		357	if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) )
-		358	{
-		359	ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER;
-		360	}
-		361	else
-		362	if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) )
-		363	{
-		364	ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER;
-		365	}
-		366	RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
-		367	ServoRollValue /= MULTIPLYER;
-		368	//DebugOut.Analog[20] = ServoRollValue;
-		369	*/ break;
320		370
321	default: // other servo channels	371	default: // other servo channels
322	RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs	372	RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs
323	break;	373	break;
324	}	374	}
325	// range servo pulse width	375	// range servo pulse width
326	if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit	376	if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
327	else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit	377	else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
328	// substract stop pulse width	378	// substract stop pulse width
329	RemainingPulse -= PPM_STOPPULSE;	379	RemainingPulse -= PPM_STOPPULSE;
330	// accumulate time for correct sync gap	380	// accumulate time for correct sync gap
331	ServoFrameTime += RemainingPulse;	381	ServoFrameTime += RemainingPulse;
332	}	382	}
333	}	383	}
334	else // we had a high pulse	384	else // we had a high pulse
335	{	385	{
336	TCCR2A \|= (1<<COM2A0); // make a low pulse	386	TCCR2A \|= (1<<COM2A0); // make a low pulse
337	// set pulsewidth to stop pulse width	387	// set pulsewidth to stop pulse width
338	RemainingPulse = PPM_STOPPULSE;	388	RemainingPulse = PPM_STOPPULSE;
339	// accumulate time for correct sync gap	389	// accumulate time for correct sync gap
340	ServoFrameTime += RemainingPulse;	390	ServoFrameTime += RemainingPulse;
341	if(ServoActive && SenderOkay > 180) HEF4017R_OFF; // disable HEF4017 reset	391	if(ServoActive && SenderOkay > 180) HEF4017R_OFF; // disable HEF4017 reset
342	ServoIndex++; // change to next servo channel	392	ServoIndex++; // change to next servo channel
343	if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap	393	if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap
344	}	394	}
345	// set pulse output active	395	// set pulse output active
346	PulseOutput = 1;	396	PulseOutput = 1;
347	}	397	}
348	} // EOF PPM state machine	398	} // EOF PPM state machine
349		399
350	// General pulse output generator	400	// General pulse output generator
351	if(RemainingPulse > (255 + IRS_RUNTIME))	401	if(RemainingPulse > (255 + IRS_RUNTIME))
352	{	402	{
353	OCR2A = 255;	403	OCR2A = 255;
354	RemainingPulse -= 255;	404	RemainingPulse -= 255;
355	}	405	}
356	else	406	else
357	{	407	{
358	if(RemainingPulse > 255) // this is the 2nd last part	408	if(RemainingPulse > 255) // this is the 2nd last part
359	{	409	{
360	if((RemainingPulse - 255) < IRS_RUNTIME)	410	if((RemainingPulse - 255) < IRS_RUNTIME)
361	{	411	{
362	OCR2A = 255 - IRS_RUNTIME;	412	OCR2A = 255 - IRS_RUNTIME;
363	RemainingPulse -= 255 - IRS_RUNTIME;	413	RemainingPulse -= 255 - IRS_RUNTIME;
364		414
365	}	415	}
366	else // last part > ISR_RUNTIME	416	else // last part > ISR_RUNTIME
367	{	417	{
368	OCR2A = 255;	418	OCR2A = 255;
369	RemainingPulse -= 255;	419	RemainingPulse -= 255;
370	}	420	}
371	}	421	}
372	else // this is the last part	422	else // this is the last part
373	{	423	{
374	OCR2A = RemainingPulse;	424	OCR2A = RemainingPulse;
375	RemainingPulse = 0;	425	RemainingPulse = 0;
376	PulseOutput = 0; // trigger to stop pulse	426	PulseOutput = 0; // trigger to stop pulse
377	}	427	}
378	} // EOF general pulse output generator	428	} // EOF general pulse output generator
379	}	429	}
380		430

Subversion Repositories FlightCtrl

(root)/tags/V2.06a/timer0.c @ 2564 - Rev 1171 → 1224