WebSVN - FlightCtrl - Diff - Rev 309 and 312 - /branches/MergedVersionsByOsiair/alpha/v060SalvoKompassNick666MM3Gyro/timer0.c


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

Subversion Repositories FlightCtrl

(root)/branches/MergedVersionsByOsiair/alpha/v060SalvoKompassNick666MM3Gyro/timer0.c @ 322 - Rev 309 → 312