WebSVN - FlightCtrl - Diff - Rev 157 and 162 - /branches/salvo


#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;
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;
//    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(Timeout) Timeout--;
     }  
 
     if(beeptime > 1)
        {
        beeptime--;
        PORTD |= (1<<2);
        }
     else  
        PORTD &= ~(1<<2);
 
// 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  
    }
     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 = -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));
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
//  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;
 
//     DebugOut.Analog[10] = ServoValue;     
     OCR2A = ServoValue;// + 75;
     timer = EE_Parameter.ServoNickRefresh;
    }
    else
    {
     TCCR2A =3;
     PORTD&=~0x80;
    }
}
 

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

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/timer0.c @ 519 - Rev 157 → 162