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

Subversion Repositories FlightCtrl

(root)/branches/salvo_gierkompass/timer0.c @ 415 - Rev 136 → 143