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

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_v0067g/trunk/timer0.c - Rev 553 → 569