WebSVN - FlightCtrl - Diff - Rev 528 and 529 - /branches/salvo_gps/Basis


#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;
unsigned int BeepMuster = 0xffff;
int ServoValue = 0;
//Salvo 8.9.2007
volatile uint8_t Kompass_Neuer_Wert= 0;
volatile unsigned int Kompass_Value_Old = 0;
// Salvo End
//Salvo 21.9.2007
short unsigned int Kompass_present= 0; //>0 bedeutet dass der Kompass vorhanden ist
// Salvo End
enum {
  STOP             = 0,
  CK               = 1,
  CK8              = 2,
  CK64             = 3,
  CK256            = 4,
  CK1024           = 5,
  T0_FALLING_EDGE  = 6,
  T0_RISING_EDGE   = 7
};
 
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 21.9.2007
/*
Driftkompensation fuer Gyros verbessert
Linearsensor mit fixem Neutralwert
Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsubhaengige Kompassfunktion
*/
SIGNAL (SIG_OVERFLOW0)    // 8kHz
{
    static unsigned char cnt_1ms = 1,cnt = 0;
    unsigned char pieper_ein = 0;
//    TCNT0 -= 250;//TIMER_RELOAD_VALUE;
 
   if(!cnt--)
    {
         if (Kompass_present > 0) Kompass_present--; //Runterzaehlen. Wenn 0 ist der Kompass nicht vorhanden
     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 < 4000))
    {
// Salvo Kompassoffset 30.8.2007 und 21.9.2007 ***********
         Kompass_present = 255;
         Kompass_Value_Old      =       KompassValue;
     KompassValue = cntKompass -KOMPASS_OFFSET;
 
         if (KompassValue < 0)
         {
                KompassValue += 360;
         }
         if (KompassValue >= 360)
         {
                KompassValue -= 360;
         }     
 // Salvo End  
    }
//     if(cntKompass < 10) cntKompass = 10;
//     KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
     KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
//Salvo 13.9.2007 Ok Erkennung des Magnetkompasses 
         Kompass_Neuer_Wert     = 1;
 // Salvo End
     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);
   
//    TIMSK2 |= _BV(TOIE2);
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_OUTPUT_COMPARE2A)
{
  static unsigned char timer = 10;
 
  if(!timer--)  
    {
     TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3;  
     ServoValue =  Parameter_ServoNickControl;
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
     else ServoValue -= ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
     
     if(ServoValue < EE_Parameter.ServoNickMin) ServoValue = EE_Parameter.ServoNickMin;
     else if(ServoValue > EE_Parameter.ServoNickMax) ServoValue = EE_Parameter.ServoNickMax;
 
     OCR2A = ServoValue;// + 75;
     timer = EE_Parameter.ServoNickRefresh;
    }
    else
    {
     TCCR2A =3;
     PORTD&=~0x80;
    }
}
 

Rev 528	Rev 529
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	unsigned int BeepMuster = 0xffff;	8	unsigned int BeepMuster = 0xffff;
9	int ServoValue = 0;	9	int ServoValue = 0;
10	//Salvo 8.9.2007	10	//Salvo 8.9.2007
11	volatile uint8_t Kompass_Neuer_Wert= 0;	11	volatile uint8_t Kompass_Neuer_Wert= 0;
12	volatile unsigned int Kompass_Value_Old = 0;	12	volatile unsigned int Kompass_Value_Old = 0;
13	// Salvo End	13	// Salvo End
14	//Salvo 21.9.2007	14	//Salvo 21.9.2007
15	short unsigned int Kompass_present= 0; //>0 bedeutet dass der Kompass vorhanden ist	15	short unsigned int Kompass_present= 0; //>0 bedeutet dass der Kompass vorhanden ist
16	// Salvo End	16	// Salvo End
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	// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 21.9.2007	28	// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 21.9.2007
29	/*	29	/*
30	Driftkompensation fuer Gyros verbessert	30	Driftkompensation fuer Gyros verbessert
31	Linearsensor mit fixem Neutralwert	31	Linearsensor mit fixem Neutralwert
32	Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsubhaengige Kompassfunktion	32	Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsubhaengige Kompassfunktion
33	*/	33	*/
34	SIGNAL (SIG_OVERFLOW0) // 8kHz	34	SIGNAL (SIG_OVERFLOW0) // 8kHz
35	{	35	{
36	static unsigned char cnt_1ms = 1,cnt = 0;	36	static unsigned char cnt_1ms = 1,cnt = 0;
37	unsigned char pieper_ein = 0;	37	unsigned char pieper_ein = 0;
38	// TCNT0 -= 250;//TIMER_RELOAD_VALUE;	38	// TCNT0 -= 250;//TIMER_RELOAD_VALUE;
39		39
40	if(!cnt--)	40	if(!cnt--)
41	{	41	{
42	if (Kompass_present > 0) Kompass_present--; //Runterzaehlen. Wenn 0 ist der Kompass nicht vorhanden	42	if (Kompass_present > 0) Kompass_present--; //Runterzaehlen. Wenn 0 ist der Kompass nicht vorhanden
43	cnt = 9;	43	cnt = 9;
44	cnt_1ms++;	44	cnt_1ms++;
45	cnt_1ms %= 2;	45	cnt_1ms %= 2;
46	if(!cnt_1ms) UpdateMotor = 1;	46	if(!cnt_1ms) UpdateMotor = 1;
47	CountMilliseconds++;	47	CountMilliseconds++;
48	if(Timeout) Timeout--;	-
49	}	48	}
50		49
51	if(beeptime > 1)	50	if(beeptime > 1)
52	{	51	{
53	beeptime--;	52	beeptime--;
54	if(beeptime & BeepMuster)	53	if(beeptime & BeepMuster)
55	{	54	{
56	pieper_ein = 1;	55	pieper_ein = 1;
57	}	56	}
58	else pieper_ein = 0;	57	else pieper_ein = 0;
59	}	58	}
60	else	59	else
61	{	60	{
62	pieper_ein = 0;	61	pieper_ein = 0;
63	BeepMuster = 0xffff;	62	BeepMuster = 0xffff;
64	}	63	}
65		64
66		65
67	if(pieper_ein)	66	if(pieper_ein)
68	{	67	{
69	if(PlatinenVersion == 10) PORTD \|= (1<<2); // Speaker an PORTD.2	68	if(PlatinenVersion == 10) PORTD \|= (1<<2); // Speaker an PORTD.2
70	else PORTC \|= (1<<7); // Speaker an PORTC.7	69	else PORTC \|= (1<<7); // Speaker an PORTC.7
71	}	70	}
72	else	71	else
73	{	72	{
74	if(PlatinenVersion == 10) PORTD &= ~(1<<2);	73	if(PlatinenVersion == 10) PORTD &= ~(1<<2);
75	else PORTC &= ~(1<<7);	74	else PORTC &= ~(1<<7);
76	}	75	}
77		76
78	// if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)	77	// if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
79	{	78	{
80	if(PINC & 0x10)	79	if(PINC & 0x10)
81	{	80	{
82	cntKompass++;	81	cntKompass++;
83	}	82	}
84	else	83	else
85	{	84	{
86	if((cntKompass) && (cntKompass < 4000))	85	if((cntKompass) && (cntKompass < 4000))
87	{	86	{
88	// Salvo Kompassoffset 30.8.2007 und 21.9.2007 ***********	87	// Salvo Kompassoffset 30.8.2007 und 21.9.2007 ***********
89	Kompass_present = 255;	88	Kompass_present = 255;
90	Kompass_Value_Old = KompassValue;	89	Kompass_Value_Old = KompassValue;
91	KompassValue = cntKompass -KOMPASS_OFFSET;	90	KompassValue = cntKompass -KOMPASS_OFFSET;
92		91
93	if (KompassValue < 0)	92	if (KompassValue < 0)
94	{	93	{
95	KompassValue += 360;	94	KompassValue += 360;
96	}	95	}
97	if (KompassValue >= 360)	96	if (KompassValue >= 360)
98	{	97	{
99	KompassValue -= 360;	98	KompassValue -= 360;
100	}	99	}
101	// Salvo End	100	// Salvo End
102	}	101	}
103	// if(cntKompass < 10) cntKompass = 10;	102	// if(cntKompass < 10) cntKompass = 10;
104	// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;	103	// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
105	KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;	104	KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
106	//Salvo 13.9.2007 Ok Erkennung des Magnetkompasses	105	//Salvo 13.9.2007 Ok Erkennung des Magnetkompasses
107	Kompass_Neuer_Wert = 1;	106	Kompass_Neuer_Wert = 1;
108	// Salvo End	107	// Salvo End
109	cntKompass = 0;	108	cntKompass = 0;
110	}	109	}
111	}	110	}
112	}	111	}
113		112
114		113
115	void Timer_Init(void)	114	void Timer_Init(void)
116	{	115	{
117	tim_main = SetDelay(10);	116	tim_main = SetDelay(10);
118	TCCR0B = CK8;	117	TCCR0B = CK8;
119	TCCR0A = (1<<COM0A1)\|(1<<COM0B1)\|3;//fast PWM	118	TCCR0A = (1<<COM0A1)\|(1<<COM0B1)\|3;//fast PWM
120	OCR0A = 0;	119	OCR0A = 0;
121	OCR0B = 120;	120	OCR0B = 120;
122	TCNT0 = -TIMER_RELOAD_VALUE; // reload	121	TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE; // reload
123	//OCR1 = 0x00;	122	//OCR1 = 0x00;
124		123
125	TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|3;	124	TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|3;
126	TCCR2B=(0<<CS20)\|(1<<CS21)\|(1<<CS22);	125	TCCR2B=(0<<CS20)\|(1<<CS21)\|(1<<CS22);
127		126
128	// TIMSK2 \|= _BV(TOIE2);	127	// TIMSK2 \|= _BV(TOIE2);
129	TIMSK2 \|= _BV(OCIE2A);	128	TIMSK2 \|= _BV(OCIE2A);
130		129
131	TIMSK0 \|= _BV(TOIE0);	130	TIMSK0 \|= _BV(TOIE0);
132	OCR2A = 10;	131	OCR2A = 10;
133	TCNT2 = 0;	132	TCNT2 = 0;
134		133
135	}	134	}
136		135
137	// -----------------------------------------------------------------------	136	// -----------------------------------------------------------------------
138		137
139	unsigned int SetDelay (unsigned int t)	138	unsigned int SetDelay (unsigned int t)
140	{	139	{
141	// TIMSK0 &= ~_BV(TOIE0);	140	// TIMSK0 &= ~_BV(TOIE0);
142	return(CountMilliseconds + t + 1);	141	return(CountMilliseconds + t + 1);
143	// TIMSK0 \|= _BV(TOIE0);	142	// TIMSK0 \|= _BV(TOIE0);
144	}	143	}
145		144
146	// -----------------------------------------------------------------------	145	// -----------------------------------------------------------------------
147	char CheckDelay(unsigned int t)	146	char CheckDelay(unsigned int t)
148	{	147	{
149	// TIMSK0 &= ~_BV(TOIE0);	148	// TIMSK0 &= ~_BV(TOIE0);
150	return(((t - CountMilliseconds) & 0x8000) >> 9);	149	return(((t - CountMilliseconds) & 0x8000) >> 9);
151	// TIMSK0 \|= _BV(TOIE0);	150	// TIMSK0 \|= _BV(TOIE0);
152	}	151	}
153		152
154	// -----------------------------------------------------------------------	153	// -----------------------------------------------------------------------
155	void Delay_ms(unsigned int w)	154	void Delay_ms(unsigned int w)
156	{	155	{
157	unsigned int akt;	156	unsigned int akt;
158	akt = SetDelay(w);	157	akt = SetDelay(w);
159	while (!CheckDelay(akt));	158	while (!CheckDelay(akt));
160	}	159	}
-		160
-		161	void Delay_ms_Mess(unsigned int w)
-		162	{
-		163	unsigned int akt;
-		164	akt = SetDelay(w);
-		165	while (!CheckDelay(akt)) ANALOG_ON;
-		166	}
161		167
162	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	168	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
163	// Servo ansteuern	169	// Servo ansteuern
164	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	170	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
165	SIGNAL(SIG_OUTPUT_COMPARE2A)	171	SIGNAL(SIG_OUTPUT_COMPARE2A)
166	{	172	{
167	static unsigned char timer = 10;	173	static unsigned char timer = 10;
168		174
169	if(!timer--)	175	if(!timer--)
170	{	176	{
171	TCCR2A=(1<<COM2A1)\|(0<<COM2A0)\|3;	177	TCCR2A=(1<<COM2A1)\|(0<<COM2A0)\|3;
172	ServoValue = Parameter_ServoNickControl;	178	ServoValue = Parameter_ServoNickControl;
173	if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;	179	if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
174	else ServoValue -= ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;	180	else ServoValue -= ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
175		181
176	if(ServoValue < EE_Parameter.ServoNickMin) ServoValue = EE_Parameter.ServoNickMin;	182	if(ServoValue < EE_Parameter.ServoNickMin) ServoValue = EE_Parameter.ServoNickMin;
177	else if(ServoValue > EE_Parameter.ServoNickMax) ServoValue = EE_Parameter.ServoNickMax;	183	else if(ServoValue > EE_Parameter.ServoNickMax) ServoValue = EE_Parameter.ServoNickMax;
178		184
179	OCR2A = ServoValue;// + 75;	185	OCR2A = ServoValue;// + 75;
180	timer = EE_Parameter.ServoNickRefresh;	186	timer = EE_Parameter.ServoNickRefresh;
181	}	187	}
182	else	188	else
183	{	189	{
184	TCCR2A =3;	190	TCCR2A =3;
185	PORTD&=~0x80;	191	PORTD&=~0x80;
186	}	192	}
187	}	193	}
188		194

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_v0067g/trunk/timer0.c - Rev 528 → 529