WebSVN - FlightCtrl - Diff - Rev 1085 and 1086 - /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;
volatile unsigned char SendSPI = 0;
volatile unsigned int ServoState = 40;
 
unsigned int BeepMuster = 0xffff;
unsigned 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 24.12.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(SendSPI) SendSPI--;
   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) //Salvo 25.10.2008
 {
  if(PINC & 0x10)
   {
    cntKompass++;
   }
  else
   {
    if((cntKompass) && (cntKompass < 362))
    {
     cntKompass += cntKompass / 41;
     if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;
// 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);    // clk/256
    TCCR2B=(0<<CS20)|(0<<CS21)|(1<<CS22);      // clk/64
   
 
  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_OVERFLOW2)
{
  if (ServoState > 0) PORTD |= 0x80;
  else PORTD &= ~0x80;
  TCCR2A =3;
  TIMSK2 &= ~_BV(TOIE2);
}
 
SIGNAL(SIG_OUTPUT_COMPARE2A)
{
  static unsigned char postPulse = 0x80;
  static int filterServo = 100;
  #define MULTIPLIER 4
  if(ServoState == 4)  
    {
     ServoValue = 0x0030;  // Offset Part1
     filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;
         ServoValue += filterServo;
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER);
     else ServoValue -= ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER);
     if((ServoValue) < ((int)EE_Parameter.ServoNickMin*3)) ServoValue = (int)EE_Parameter.ServoNickMin*3;
     else if((ServoValue) > ((int)EE_Parameter.ServoNickMax*3)) ServoValue = (int)EE_Parameter.ServoNickMax*3;
   
         DebugOut.Analog[20] = ServoValue;
         if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60;
         OCR2A = 255-(ServoValue % 256);
         TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;  
        }
  else if ((ServoState > 0) && (ServoState < 4))
    {
          if(ServoValue > 255)
          { PORTD |= 0x80;
                TCCR2A =3;
                ServoValue -= 255;
          }
          else
      {  
             TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3;  
                 OCR2A = postPulse; // Offset Part2
                 ServoState = 1;
      }
  }
  else if (ServoState == 0)
          {
             ServoState  = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER;
         PORTD&=~0x80;
                 TCCR2A = 3;
          }
  ServoState--;
 
}
 

Rev 1085	Rev 1086
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	volatile unsigned char SendSPI = 0;	8	volatile unsigned char SendSPI = 0;
9	volatile unsigned int ServoState = 40;	9	volatile unsigned int ServoState = 40;
10		10
11	unsigned int BeepMuster = 0xffff;	11	unsigned int BeepMuster = 0xffff;
12	unsigned int ServoValue = 0;	12	unsigned int ServoValue = 0;
-		13	//Salvo 8.9.2007
-		14	volatile uint8_t Kompass_Neuer_Wert= 0;
-		15	volatile unsigned int Kompass_Value_Old = 0;
-		16	// Salvo End
-		17	//Salvo 21.9.2007
-		18	short unsigned int Kompass_present= 0; //>0 bedeutet dass der Kompass vorhanden ist
13		19	// Salvo End
14	enum {	20	enum {
15	STOP = 0,	21	STOP = 0,
16	CK = 1,	22	CK = 1,
17	CK8 = 2,	23	CK8 = 2,
18	CK64 = 3,	24	CK64 = 3,
19	CK256 = 4,	25	CK256 = 4,
20	CK1024 = 5,	26	CK1024 = 5,
21	T0_FALLING_EDGE = 6,	27	T0_FALLING_EDGE = 6,
22	T0_RISING_EDGE = 7	28	T0_RISING_EDGE = 7
23	};	29	};
-		30
-		31	// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 24.12.2007
-		32	/*
24		33	Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsubhaengige Kompassfunktion
25		34	*/
26	SIGNAL (SIG_OVERFLOW0) // 8kHz	35	SIGNAL (SIG_OVERFLOW0) // 8kHz
27	{	36	{
28	static unsigned char cnt_1ms = 1,cnt = 0;	37	static unsigned char cnt_1ms = 1,cnt = 0;
29	unsigned char pieper_ein = 0;	38	unsigned char pieper_ein = 0;
30	// TCNT0 -= 250;//TIMER_RELOAD_VALUE;	39	// TCNT0 -= 250;//TIMER_RELOAD_VALUE;
31	if(SendSPI) SendSPI--;	40	if(SendSPI) SendSPI--;
32	if(!cnt--)	41	if(!cnt--)
33	{	42	{
-		43	if (Kompass_present > 0) Kompass_present--; //Runterzaehlen. Wenn 0 ist der Kompass nicht vorhanden
34	cnt = 9;	44	cnt = 9;
35	cnt_1ms++;	45	cnt_1ms++;
36	cnt_1ms %= 2;	46	cnt_1ms %= 2;
37	if(!cnt_1ms) UpdateMotor = 1;	47	if(!cnt_1ms) UpdateMotor = 1;
38	CountMilliseconds++;	48	CountMilliseconds++;
39	}	49	}
40		50
41	if(beeptime > 1)	51	if(beeptime > 1)
42	{	52	{
43	beeptime--;	53	beeptime--;
44	if(beeptime & BeepMuster)	54	if(beeptime & BeepMuster)
45	{	55	{
46	pieper_ein = 1;	56	pieper_ein = 1;
47	}	57	}
48	else pieper_ein = 0;	58	else pieper_ein = 0;
49	}	59	}
50	else	60	else
51	{	61	{
52	pieper_ein = 0;	62	pieper_ein = 0;
53	BeepMuster = 0xffff;	63	BeepMuster = 0xffff;
54	}	64	}
55		65
56		66
57	if(pieper_ein)	67	if(pieper_ein)
58	{	68	{
59	if(PlatinenVersion == 10) PORTD \|= (1<<2); // Speaker an PORTD.2	69	if(PlatinenVersion == 10) PORTD \|= (1<<2); // Speaker an PORTD.2
60	else PORTC \|= (1<<7); // Speaker an PORTC.7	70	else PORTC \|= (1<<7); // Speaker an PORTC.7
61	}	71	}
62	else	72	else
63	{	73	{
64	if(PlatinenVersion == 10) PORTD &= ~(1<<2);	74	if(PlatinenVersion == 10) PORTD &= ~(1<<2);
65	else PORTC &= ~(1<<7);	75	else PORTC &= ~(1<<7);
66	}	76	}
67		77
68	if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)	78	if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) //Salvo 25.10.2008
69	{	79	{
70	if(PINC & 0x10)	80	if(PINC & 0x10)
71	{	81	{
72	cntKompass++;	82	cntKompass++;
73	}	83	}
74	else	84	else
75	{	85	{
76	if((cntKompass) && (cntKompass < 362))	86	if((cntKompass) && (cntKompass < 362))
77	{	87	{
78	cntKompass += cntKompass / 41;	88	cntKompass += cntKompass / 41;
79	if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;	89	if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;
-		90	// Salvo Kompassoffset 23.12.2007 ***********
-		91	Kompass_present = 255;
-		92	// Kompass_Value_Old = KompassValue;
-		93	if (KOMPASS_OFFSET > 0) KompassValue = cntKompass -KOMPASS_OFFSET;
-		94	else KompassValue = cntKompass - ((int) (Parameter_UserParam4*2));
-		95
-		96	if (KompassValue < 0)
-		97	{
-		98	KompassValue += 360;
-		99	}
-		100	if (KompassValue >= 360)
-		101	{
-		102	KompassValue -= 360;
-		103	}
-		104	// Salvo End
80	}	105	}
81	// if(cntKompass < 10) cntKompass = 10;	106	// if(cntKompass < 10) cntKompass = 10;
82	// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;	107	// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
83	KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;	108	KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
-		109	//Salvo 13.9.2007 Ok Erkennung des Magnetkompasses
-		110	Kompass_Neuer_Wert = 1;
-		111	// Salvo End
84	cntKompass = 0;	112	cntKompass = 0;
85	}	113	}
86	}	114	}
87	}	115	}
88		116
89	//----------------------------	117	//----------------------------
90	void Timer_Init(void)	118	void Timer_Init(void)
91	{	119	{
92	tim_main = SetDelay(10);	120	tim_main = SetDelay(10);
93	TCCR0B = CK8;	121	TCCR0B = CK8;
94	TCCR0A = (1<<COM0A1)\|(1<<COM0B1)\|3;//fast PWM	122	TCCR0A = (1<<COM0A1)\|(1<<COM0B1)\|3;//fast PWM
95	OCR0A = 0;	123	OCR0A = 0;
96	OCR0B = 120;	124	OCR0B = 120;
97	TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE; // reload	125	TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE; // reload
98	//OCR1 = 0x00;	126	//OCR1 = 0x00;
99		127
100	TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|3;	128	TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|3;
101	// TCCR2B=(0<<CS20)\|(1<<CS21)\|(1<<CS22); // clk/256	129	// TCCR2B=(0<<CS20)\|(1<<CS21)\|(1<<CS22); // clk/256
102	TCCR2B=(0<<CS20)\|(0<<CS21)\|(1<<CS22); // clk/64	130	TCCR2B=(0<<CS20)\|(0<<CS21)\|(1<<CS22); // clk/64
103		131
104		132
105	TIMSK2 \|= _BV(OCIE2A);	133	TIMSK2 \|= _BV(OCIE2A);
106		134
107	TIMSK0 \|= _BV(TOIE0);	135	TIMSK0 \|= _BV(TOIE0);
108	OCR2A = 10;	136	OCR2A = 10;
109	TCNT2 = 0;	137	TCNT2 = 0;
110		138
111	}	139	}
112		140
113	// -----------------------------------------------------------------------	141	// -----------------------------------------------------------------------
114		142
115	unsigned int SetDelay (unsigned int t)	143	unsigned int SetDelay (unsigned int t)
116	{	144	{
117	// TIMSK0 &= ~_BV(TOIE0);	145	// TIMSK0 &= ~_BV(TOIE0);
118	return(CountMilliseconds + t + 1);	146	return(CountMilliseconds + t + 1);
119	// TIMSK0 \|= _BV(TOIE0);	147	// TIMSK0 \|= _BV(TOIE0);
120	}	148	}
121		149
122	// -----------------------------------------------------------------------	150	// -----------------------------------------------------------------------
123	char CheckDelay(unsigned int t)	151	char CheckDelay(unsigned int t)
124	{	152	{
125	// TIMSK0 &= ~_BV(TOIE0);	153	// TIMSK0 &= ~_BV(TOIE0);
126	return(((t - CountMilliseconds) & 0x8000) >> 9);	154	return(((t - CountMilliseconds) & 0x8000) >> 9);
127	// TIMSK0 \|= _BV(TOIE0);	155	// TIMSK0 \|= _BV(TOIE0);
128	}	156	}
129		157
130	// -----------------------------------------------------------------------	158	// -----------------------------------------------------------------------
131	void Delay_ms(unsigned int w)	159	void Delay_ms(unsigned int w)
132	{	160	{
133	unsigned int akt;	161	unsigned int akt;
134	akt = SetDelay(w);	162	akt = SetDelay(w);
135	while (!CheckDelay(akt));	163	while (!CheckDelay(akt));
136	}	164	}
137		165
138	void Delay_ms_Mess(unsigned int w)	166	void Delay_ms_Mess(unsigned int w)
139	{	167	{
140	unsigned int akt;	168	unsigned int akt;
141	akt = SetDelay(w);	169	akt = SetDelay(w);
142	while (!CheckDelay(akt)) ANALOG_ON;	170	while (!CheckDelay(akt)) ANALOG_ON;
143	}	171	}
144		172
145	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	173	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
146	// Servo ansteuern	174	// Servo ansteuern
147	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	175	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
148	SIGNAL(SIG_OVERFLOW2)	176	SIGNAL(SIG_OVERFLOW2)
149	{	177	{
150	if (ServoState > 0) PORTD \|= 0x80;	178	if (ServoState > 0) PORTD \|= 0x80;
151	else PORTD &= ~0x80;	179	else PORTD &= ~0x80;
152	TCCR2A =3;	180	TCCR2A =3;
153	TIMSK2 &= ~_BV(TOIE2);	181	TIMSK2 &= ~_BV(TOIE2);
154	}	182	}
155		183
156	SIGNAL(SIG_OUTPUT_COMPARE2A)	184	SIGNAL(SIG_OUTPUT_COMPARE2A)
157	{	185	{
158	static unsigned char postPulse = 0x80;	186	static unsigned char postPulse = 0x80;
159	static int filterServo = 100;	187	static int filterServo = 100;
160	#define MULTIPLIER 4	188	#define MULTIPLIER 4
161	if(ServoState == 4)	189	if(ServoState == 4)
162	{	190	{
163	ServoValue = 0x0030; // Offset Part1	191	ServoValue = 0x0030; // Offset Part1
164	filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;	192	filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;
165	ServoValue += filterServo;	193	ServoValue += filterServo;
166	if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER);	194	if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER);
167	else ServoValue -= ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER);	195	else ServoValue -= ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER);
168	if((ServoValue) < ((int)EE_Parameter.ServoNickMin3)) ServoValue = (int)EE_Parameter.ServoNickMin3;	196	if((ServoValue) < ((int)EE_Parameter.ServoNickMin3)) ServoValue = (int)EE_Parameter.ServoNickMin3;
169	else if((ServoValue) > ((int)EE_Parameter.ServoNickMax3)) ServoValue = (int)EE_Parameter.ServoNickMax3;	197	else if((ServoValue) > ((int)EE_Parameter.ServoNickMax3)) ServoValue = (int)EE_Parameter.ServoNickMax3;
170		198
171	DebugOut.Analog[20] = ServoValue;	199	DebugOut.Analog[20] = ServoValue;
172	if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60;	200	if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60;
173	OCR2A = 255-(ServoValue % 256);	201	OCR2A = 255-(ServoValue % 256);
174	TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|3;	202	TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|3;
175	}	203	}
176	else if ((ServoState > 0) && (ServoState < 4))	204	else if ((ServoState > 0) && (ServoState < 4))
177	{	205	{
178	if(ServoValue > 255)	206	if(ServoValue > 255)
179	{ PORTD \|= 0x80;	207	{ PORTD \|= 0x80;
180	TCCR2A =3;	208	TCCR2A =3;
181	ServoValue -= 255;	209	ServoValue -= 255;
182	}	210	}
183	else	211	else
184	{	212	{
185	TCCR2A=(1<<COM2A1)\|(0<<COM2A0)\|3;	213	TCCR2A=(1<<COM2A1)\|(0<<COM2A0)\|3;
186	OCR2A = postPulse; // Offset Part2	214	OCR2A = postPulse; // Offset Part2
187	ServoState = 1;	215	ServoState = 1;
188	}	216	}
189	}	217	}
190	else if (ServoState == 0)	218	else if (ServoState == 0)
191	{	219	{
192	ServoState = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER;	220	ServoState = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER;
193	PORTD&=~0x80;	221	PORTD&=~0x80;
194	TCCR2A = 3;	222	TCCR2A = 3;
195	}	223	}
196	ServoState--;	224	ServoState--;
197		225
198	}	226	}
199		227

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_V0071h/trunk/timer0.c - Rev 1085 → 1086