WebSVN - FlightCtrl - Diff - Rev 1111 and 1153 - /branches/V0.82a_ACC-HH


#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;
volatile unsigned int ServoState = 40;
 
unsigned int BeepMuster = 0xffff;
int ServoValue = 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;
   }
 }
}
 
//----------------------------
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;
 
    TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
//    TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22);    // clk/256
    TCCR2B=(0<<CS20)|(0<<CS21)|(1<<CS22);      // clk/64
 
 
  TIMSK2 |= _BV(OCIE2A);
 
    TIMSK0 |= _BV(TOIE0);
    OCR2A = 10;
    TCNT2 = 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)) ANALOG_ON;
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//  Servo ansteuern
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
SIGNAL(SIG_OVERFLOW2)
{
  if (ServoState > 0) PORTD |= 0x80;
  else PORTD &= ~0x80;
  TCCR2A =3;
  TIMSK2 &= ~_BV(TOIE2);
}
 
SIGNAL(SIG_OUTPUT_COMPARE2A)
{
  static unsigned char postPulse = 0x80;
  static int filterServo = 100;
  static unsigned char restPulse = 50;
  #define MULTIPLIER 4
  if(PlatinenVersion < 20)
  {
   if(ServoState == 4)
    {
     ServoValue = 0x0030;  // Offset Part1
     filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;
         ServoValue += filterServo;
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );
     else ServoValue -= (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );
     if((ServoValue) < ((int)EE_Parameter.ServoNickMin*3)) ServoValue = (int)EE_Parameter.ServoNickMin*3;
     else if((ServoValue) > ((int)EE_Parameter.ServoNickMax*3)) ServoValue = (int)EE_Parameter.ServoNickMax*3;
 
         DebugOut.Analog[20] = ServoValue;
         if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60;
         OCR2A = 255-(ServoValue % 256);
         TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
        }
    else if ((ServoState > 0) && (ServoState < 4))
    {
          if(ServoValue > 255)
          { PORTD |= 0x80;
                TCCR2A =3;
                ServoValue -= 255;
          }
          else
      {
             TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3;
                 OCR2A = postPulse; // Offset Part2
                 ServoState = 1;
      }
    }
    else if (ServoState == 0)
          {
             ServoState  = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER;
         PORTD&=~0x80;
                 TCCR2A = 3;
          }
  }
 else
  {
  if(ServoState == 4)
    {
     PORTD &= ~0x80;
     PORTC |= _BV(PC6);
     ServoValue = 0x00030;  // Offset Part1
     filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;
     ServoValue += filterServo;
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );
     else ServoValue -= (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );
     if((ServoValue) < ((int)EE_Parameter.ServoNickMin*3)) ServoValue = (int)EE_Parameter.ServoNickMin*3;
     else if((ServoValue) > ((int)EE_Parameter.ServoNickMax*3)) ServoValue = (int)EE_Parameter.ServoNickMax*3;
    PORTD |= 0x80;     // dummy clock to skip output Q0
  if ((ServoValue % 256) < 1)  ServoValue -=2;
  if ((ServoValue % 256) > 253)  ServoValue +=2;
  DebugOut.Analog[20] = ServoValue;
 
  OCR2A = 254-(ServoValue % 255);
  PORTD &= ~0x80;
 
  TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;    // set on match
  //ServoValue -= OCR2A;
  PORTC &= ~_BV(PC6);
    }
 else if ((ServoState > 0) && (ServoState < 4))
    {
   if(ServoValue > 255)
   {
    PORTD &= ~0x80;
    TCCR2A =3;
    ServoValue -= 255;
    OCR2A = postPulse; // Offset Part2
    //OCR2A = (ServoValue % 256);
     }
   else
      {
      OCR2A = postPulse; // Offset Part2
      //OCR2A = (ServoValue % 256);
      //TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3; 
      ServoState = 1;
      }
    }
  else if (ServoState == 0)
    {
     PORTD &= ~0x80;
     TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
     ServoState  = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER;
     TCCR2A =3;
    }
 
 }
 ServoState--;
}
 
 

Rev 1111	Rev 1153
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;	8	volatile unsigned char SendSPI = 0;
9	volatile unsigned int ServoState = 40;	9	volatile unsigned int ServoState = 40;
10		10
11	unsigned int BeepMuster = 0xffff;	11	unsigned int BeepMuster = 0xffff;
12	int ServoValue = 0;	12	int ServoValue = 0;
13		13
14	enum {	14	enum {
15	STOP = 0,	15	STOP = 0,
16	CK = 1,	16	CK = 1,
17	CK8 = 2,	17	CK8 = 2,
18	CK64 = 3,	18	CK64 = 3,
19	CK256 = 4,	19	CK256 = 4,
20	CK1024 = 5,	20	CK1024 = 5,
21	T0_FALLING_EDGE = 6,	21	T0_FALLING_EDGE = 6,
22	T0_RISING_EDGE = 7	22	T0_RISING_EDGE = 7
23	};	23	};
24		24
25		25
26	SIGNAL (SIG_OVERFLOW0) // 8kHz	26	SIGNAL (SIG_OVERFLOW0) // 8kHz
27	{	27	{
28	static unsigned char cnt_1ms = 1,cnt = 0;	28	static unsigned char cnt_1ms = 1,cnt = 0;
29	unsigned char pieper_ein = 0;	29	unsigned char pieper_ein = 0;
30	// TCNT0 -= 250;//TIMER_RELOAD_VALUE;	30	// TCNT0 -= 250;//TIMER_RELOAD_VALUE;
31	if(SendSPI) SendSPI--;	31	if(SendSPI) SendSPI--;
32	if(!cnt--)	32	if(!cnt--)
33	{	33	{
34	cnt = 9;	34	cnt = 9;
35	cnt_1ms++;	35	cnt_1ms++;
36	cnt_1ms %= 2;	36	cnt_1ms %= 2;
37	if(!cnt_1ms) UpdateMotor = 1;	37	if(!cnt_1ms) UpdateMotor = 1;
38	CountMilliseconds++;	38	CountMilliseconds++;
39	}	39	}
40		40
41	if(beeptime > 1)	41	if(beeptime > 1)
42	{	42	{
43	beeptime--;	43	beeptime--;
44	if(beeptime & BeepMuster)	44	if(beeptime & BeepMuster)
45	{	45	{
46	pieper_ein = 1;	46	pieper_ein = 1;
47	}	47	}
48	else pieper_ein = 0;	48	else pieper_ein = 0;
49	}	49	}
50	else	50	else
51	{	51	{
52	pieper_ein = 0;	52	pieper_ein = 0;
53	BeepMuster = 0xffff;	53	BeepMuster = 0xffff;
54	}	54	}
55		55
56		56
57	if(pieper_ein)	57	if(pieper_ein)
58	{	58	{
59	if(PlatinenVersion == 10) PORTD \|= (1<<2); // Speaker an PORTD.2	59	if(PlatinenVersion == 10) PORTD \|= (1<<2); // Speaker an PORTD.2
60	else PORTC \|= (1<<7); // Speaker an PORTC.7	60	else PORTC \|= (1<<7); // Speaker an PORTC.7
61	}	61	}
62	else	62	else
63	{	63	{
64	if(PlatinenVersion == 10) PORTD &= ~(1<<2);	64	if(PlatinenVersion == 10) PORTD &= ~(1<<2);
65	else PORTC &= ~(1<<7);	65	else PORTC &= ~(1<<7);
66	}	66	}
67		67
68	if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)	68	if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
69	{	69	{
70	if(PINC & 0x10)	70	if(PINC & 0x10)
71	{	71	{
72	cntKompass++;	72	cntKompass++;
73	}	73	}
74	else	74	else
75	{	75	{
76	if((cntKompass) && (cntKompass < 362))	76	if((cntKompass) && (cntKompass < 362))
77	{	77	{
78	cntKompass += cntKompass / 41;	78	cntKompass += cntKompass / 41;
79	if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;	79	if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;
80	}	80	}
81	// if(cntKompass < 10) cntKompass = 10;	81	// if(cntKompass < 10) cntKompass = 10;
82	// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;	82	// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
83	KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;	83	KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
84	cntKompass = 0;	84	cntKompass = 0;
85	}	85	}
86	}	86	}
87	}	87	}
88		88
89	//----------------------------	89	//----------------------------
90	void Timer_Init(void)	90	void Timer_Init(void)
91	{	91	{
92	tim_main = SetDelay(10);	92	tim_main = SetDelay(10);
93	TCCR0B = CK8;	93	TCCR0B = CK8;
94	TCCR0A = (1<<COM0A1)\|(1<<COM0B1)\|3;//fast PWM	94	TCCR0A = (1<<COM0A1)\|(1<<COM0B1)\|3;//fast PWM
95	OCR0A = 0;	95	OCR0A = 0;
96	OCR0B = 120;	96	OCR0B = 120;
97	TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE; // reload	97	TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE; // reload
98	//OCR1 = 0x00;	98	//OCR1 = 0x00;
99		99
100	TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|3;	100	TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|3;
101	// TCCR2B=(0<<CS20)\|(1<<CS21)\|(1<<CS22); // clk/256	101	// TCCR2B=(0<<CS20)\|(1<<CS21)\|(1<<CS22); // clk/256
102	TCCR2B=(0<<CS20)\|(0<<CS21)\|(1<<CS22); // clk/64	102	TCCR2B=(0<<CS20)\|(0<<CS21)\|(1<<CS22); // clk/64
103		103
104		104
105	TIMSK2 \|= _BV(OCIE2A);	105	TIMSK2 \|= _BV(OCIE2A);
106		106
107	TIMSK0 \|= _BV(TOIE0);	107	TIMSK0 \|= _BV(TOIE0);
108	OCR2A = 10;	108	OCR2A = 10;
109	TCNT2 = 0;	109	TCNT2 = 0;
110		110
111	}	111	}
112		112
113	// -----------------------------------------------------------------------	113	// -----------------------------------------------------------------------
114		114
115	unsigned int SetDelay (unsigned int t)	115	unsigned int SetDelay (unsigned int t)
116	{	116	{
117	// TIMSK0 &= ~_BV(TOIE0);	117	// TIMSK0 &= ~_BV(TOIE0);
118	return(CountMilliseconds + t + 1);	118	return(CountMilliseconds + t + 1);
119	// TIMSK0 \|= _BV(TOIE0);	119	// TIMSK0 \|= _BV(TOIE0);
120	}	120	}
121		121
122	// -----------------------------------------------------------------------	122	// -----------------------------------------------------------------------
123	char CheckDelay(unsigned int t)	123	char CheckDelay(unsigned int t)
124	{	124	{
125	// TIMSK0 &= ~_BV(TOIE0);	125	// TIMSK0 &= ~_BV(TOIE0);
126	return(((t - CountMilliseconds) & 0x8000) >> 9);	126	return(((t - CountMilliseconds) & 0x8000) >> 9);
127	// TIMSK0 \|= _BV(TOIE0);	127	// TIMSK0 \|= _BV(TOIE0);
128	}	128	}
129		129
130	// -----------------------------------------------------------------------	130	// -----------------------------------------------------------------------
131	void Delay_ms(unsigned int w)	131	void Delay_ms(unsigned int w)
132	{	132	{
133	unsigned int akt;	133	unsigned int akt;
134	akt = SetDelay(w);	134	akt = SetDelay(w);
135	while (!CheckDelay(akt));	135	while (!CheckDelay(akt));
136	}	136	}
137		137
138	void Delay_ms_Mess(unsigned int w)	138	void Delay_ms_Mess(unsigned int w)
139	{	139	{
140	unsigned int akt;	140	unsigned int akt;
141	akt = SetDelay(w);	141	akt = SetDelay(w);
142	while (!CheckDelay(akt)) ANALOG_ON;	142	while (!CheckDelay(akt)) ANALOG_ON;
143	}	143	}
144		144
145	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	145	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
146	// Servo ansteuern	146	// Servo ansteuern
147	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	147	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
148	SIGNAL(SIG_OVERFLOW2)	148	SIGNAL(SIG_OVERFLOW2)
149	{	149	{
150	if (ServoState > 0) PORTD \|= 0x80;	150	if (ServoState > 0) PORTD \|= 0x80;
151	else PORTD &= ~0x80;	151	else PORTD &= ~0x80;
152	TCCR2A =3;	152	TCCR2A =3;
153	TIMSK2 &= ~_BV(TOIE2);	153	TIMSK2 &= ~_BV(TOIE2);
154	}	154	}
155		155
156	SIGNAL(SIG_OUTPUT_COMPARE2A)	156	SIGNAL(SIG_OUTPUT_COMPARE2A)
157	{	157	{
158	static unsigned char postPulse = 0x80;	158	static unsigned char postPulse = 0x80;
159	static int filterServo = 100;	159	static int filterServo = 100;
160	static unsigned char restPulse = 50;	160	static unsigned char restPulse = 50;
161	#define MULTIPLIER 4	161	#define MULTIPLIER 4
162	if(PlatinenVersion < 20)	162	if(PlatinenVersion < 20)
163	{	163	{
164	if(ServoState == 4)	164	if(ServoState == 4)
165	{	165	{
166	ServoValue = 0x0030; // Offset Part1	166	ServoValue = 0x0030; // Offset Part1
167	filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;	167	filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;
168	ServoValue += filterServo;	168	ServoValue += filterServo;
169	if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );	169	if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );
170	else ServoValue -= (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );	170	else ServoValue -= (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );
171	if((ServoValue) < ((int)EE_Parameter.ServoNickMin3)) ServoValue = (int)EE_Parameter.ServoNickMin3;	171	if((ServoValue) < ((int)EE_Parameter.ServoNickMin3)) ServoValue = (int)EE_Parameter.ServoNickMin3;
172	else if((ServoValue) > ((int)EE_Parameter.ServoNickMax3)) ServoValue = (int)EE_Parameter.ServoNickMax3;	172	else if((ServoValue) > ((int)EE_Parameter.ServoNickMax3)) ServoValue = (int)EE_Parameter.ServoNickMax3;
173		173
174	DebugOut.Analog[20] = ServoValue;	174	DebugOut.Analog[20] = ServoValue;
175	if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60;	175	if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60;
176	OCR2A = 255-(ServoValue % 256);	176	OCR2A = 255-(ServoValue % 256);
177	TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|3;	177	TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|3;
178	}	178	}
179	else if ((ServoState > 0) && (ServoState < 4))	179	else if ((ServoState > 0) && (ServoState < 4))
180	{	180	{
181	if(ServoValue > 255)	181	if(ServoValue > 255)
182	{ PORTD \|= 0x80;	182	{ PORTD \|= 0x80;
183	TCCR2A =3;	183	TCCR2A =3;
184	ServoValue -= 255;	184	ServoValue -= 255;
185	}	185	}
186	else	186	else
187	{	187	{
188	TCCR2A=(1<<COM2A1)\|(0<<COM2A0)\|3;	188	TCCR2A=(1<<COM2A1)\|(0<<COM2A0)\|3;
189	OCR2A = postPulse; // Offset Part2	189	OCR2A = postPulse; // Offset Part2
190	ServoState = 1;	190	ServoState = 1;
191	}	191	}
192	}	192	}
193	else if (ServoState == 0)	193	else if (ServoState == 0)
194	{	194	{
195	ServoState = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER;	195	ServoState = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER;
196	PORTD&=~0x80;	196	PORTD&=~0x80;
197	TCCR2A = 3;	197	TCCR2A = 3;
198	}	198	}
199	}	199	}
200	else	200	else
201	{	201	{
202	if(ServoState == 4)	202	if(ServoState == 4)
203	{	203	{
204	PORTD &= ~0x80;	204	PORTD &= ~0x80;
205	PORTC \|= _BV(PC6);	205	PORTC \|= _BV(PC6);
206	ServoValue = 0x00030; // Offset Part1	206	ServoValue = 0x00030; // Offset Part1
207	filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;	207	filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;
208	ServoValue += filterServo;	208	ServoValue += filterServo;
209	if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );	209	if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );
210	else ServoValue -= (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );	210	else ServoValue -= (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );
211	if((ServoValue) < ((int)EE_Parameter.ServoNickMin3)) ServoValue = (int)EE_Parameter.ServoNickMin3;	211	if((ServoValue) < ((int)EE_Parameter.ServoNickMin3)) ServoValue = (int)EE_Parameter.ServoNickMin3;
212	else if((ServoValue) > ((int)EE_Parameter.ServoNickMax3)) ServoValue = (int)EE_Parameter.ServoNickMax3;	212	else if((ServoValue) > ((int)EE_Parameter.ServoNickMax3)) ServoValue = (int)EE_Parameter.ServoNickMax3;
213	PORTD \|= 0x80; // dummy clock to skip output Q0	213	PORTD \|= 0x80; // dummy clock to skip output Q0
214	if ((ServoValue % 256) < 1) ServoValue -=2;	214	if ((ServoValue % 256) < 1) ServoValue -=2;
215	if ((ServoValue % 256) > 253) ServoValue +=2;	215	if ((ServoValue % 256) > 253) ServoValue +=2;
216	DebugOut.Analog[20] = ServoValue;	216	DebugOut.Analog[20] = ServoValue;
217		217
218	OCR2A = 254-(ServoValue % 255);	218	OCR2A = 254-(ServoValue % 255);
219	PORTD &= ~0x80;	219	PORTD &= ~0x80;
220		220
221	TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|3; // set on match	221	TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|3; // set on match
222	//ServoValue -= OCR2A;	222	//ServoValue -= OCR2A;
223	PORTC &= ~_BV(PC6);	223	PORTC &= ~_BV(PC6);
224	}	224	}
225	else if ((ServoState > 0) && (ServoState < 4))	225	else if ((ServoState > 0) && (ServoState < 4))
226	{	226	{
227	if(ServoValue > 255)	227	if(ServoValue > 255)
228	{	228	{
229	PORTD &= ~0x80;	229	PORTD &= ~0x80;
230	TCCR2A =3;	230	TCCR2A =3;
231	ServoValue -= 255;	231	ServoValue -= 255;
232	OCR2A = postPulse; // Offset Part2	232	OCR2A = postPulse; // Offset Part2
233	//OCR2A = (ServoValue % 256);	233	//OCR2A = (ServoValue % 256);
234	}	234	}
235	else	235	else
236	{	236	{
237	OCR2A = postPulse; // Offset Part2	237	OCR2A = postPulse; // Offset Part2
238	//OCR2A = (ServoValue % 256);	238	//OCR2A = (ServoValue % 256);
239	//TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|3;	239	//TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|3;
240	ServoState = 1;	240	ServoState = 1;
241	}	241	}
242	}	242	}
243	else if (ServoState == 0)	243	else if (ServoState == 0)
244	{	244	{
245	PORTD &= ~0x80;	245	PORTD &= ~0x80;
246	TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|3;	246	TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|3;
247	ServoState = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER;	247	ServoState = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER;
248	TCCR2A =3;	248	TCCR2A =3;
249	}	249	}
250	ServoState--;	-
251	}	250	}
-		251	ServoState--;
252	}	252	}
253		253
254		254

Subversion Repositories FlightCtrl

(root)/branches/V0.82a_ACC-HH_MartinR/timer0.c @ 2754 - Rev 1111 → 1153