WebSVN - FlightCtrl - Diff - Rev 1278 and 1283 - /branches/V0.86d_ACC-HH_HR


/*#######################################################################################
Decodieren eines RC Summen Signals
#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + only for non-profit use
// + www.MikroKopter.com
// + see the File "License.txt" for further Informations
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#include "rc.h"
#include "main.h"
 
volatile int PPM_in[11];
volatile int PPM_diff[11];  // das diffenzierte Stick-Signal
volatile char Channels,tmpChannels = 0;
volatile unsigned char NewPpmData = 1;
 
//############################################################################
//zum decodieren des PPM-Signals wird Timer1 mit seiner Input
//Capture Funktion benutzt:
void rc_sum_init (void)
//############################################################################
{
        TCCR1B=(1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1);//|(1 << WGM12); //timer1 prescale 64
//      TCCR1B=(1<<CS11)|(0<<CS10)|(1<<ICES1)|(1<<ICNC1); //timer1 prescale 64
    TIMSK1 |= _BV(ICIE1);
    AdNeutralGier = 0;
    AdNeutralRoll = 0;
    AdNeutralNick = 0;
    return;
}
/*
//############################################################################
//Diese Routine startet und inizialisiert den Timer für RC
SIGNAL(SIG_INPUT_CAPTURE1)
//############################################################################
 
{
        static unsigned int AltICR=0;
    signed int signal = 0,tmp;
        static int index;              
               
        signal = (unsigned int) ICR1 - AltICR;         
        AltICR = ICR1; 
    //Syncronisationspause? (3.52 ms < signal < 25.6 ms)
        if((signal > 1100) && (signal < 8000))   
        {
        if(index >= 4)  NewPpmData = 0;  // Null bedeutet: Neue Daten
        index = 1;             
        }
        else
        {
        if(index < 10)
            {
            if((signal > 250) && (signal < 687))
                {
                signal -= 466;
                // Stabiles Signal
                if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10; else SenderOkay = 200;}
                tmp = (3 * (PPM_in[index]) + signal) / 4;  
                if(tmp > signal+1) tmp--; else
                if(tmp < signal-1) tmp++;
                if(SenderOkay >= 195)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3;
                else PPM_diff[index] = 0;
                PPM_in[index] = tmp;
                }
            index++;  
         if(index == 5) J3High; else J3Low;  // Servosignal an J3 anlegen
         if(index == 6) J4High; else J4Low;  // Servosignal an J4 anlegen
         if(index == 7) J5High; else J5Low;  // Servosignal an J5 anlegen
        }
        }
}
*/
 
 
//############################################################################
//Diese Routine startet und inizialisiert den Timer für RC
SIGNAL(SIG_INPUT_CAPTURE1)
//############################################################################
 
{
        static unsigned int AltICR=0;
    static int ppm_in[11];
    static int ppm_diff[11];  
    static int old_ppm_in[11];
    static int old_ppm_diff[11];  
    signed int signal = 0,tmp;
        static unsigned char index, okay_cnt = 0;              
        signal = (unsigned int) ICR1 - AltICR;         
        AltICR = ICR1; 
    //Syncronisationspause? (3.52 ms < signal < 25.6 ms)
        if((signal > 1100) && (signal < 8000))   
        {
        tmpChannels = index;
        if(tmpChannels >= 4 && Channels == tmpChannels)  
                 {
          if(okay_cnt > 10)
                   {
                   NewPpmData = 0;  // Null bedeutet: Neue Daten
                   for(index = 0; index < 11; index++)
                    {
                         if(okay_cnt > 30)
                          {
                           old_ppm_in[index] = PPM_in[index];
                           old_ppm_diff[index] = PPM_diff[index];
                          }
                     PPM_in[index] = ppm_in[index];
                     PPM_diff[index] = ppm_diff[index];
                    }
                   }
          if(okay_cnt < 255) okay_cnt++;
                 }
         else
                  {
                   if(okay_cnt > 100) okay_cnt = 10; else okay_cnt = 0;
                   ROT_ON;
                  }
        index = 1;             
        if(!MotorenEin) Channels = tmpChannels;
        }
        else
        {
        if(index < 10)
            {
            if((signal > 250) && (signal < 687))
                {
                signal -= 466;
                // Stabiles Signal
                if((abs(signal - ppm_in[index]) < 6))
                                 {
                                  if(okay_cnt > 25)  SenderOkay += 10;
                                  else
                                  if(okay_cnt > 10)  SenderOkay += 2;
                                  if(SenderOkay > 200) SenderOkay = 200;
                                 }
                tmp = (3 * (ppm_in[index]) + signal) / 4;  
                if(tmp > signal+1) tmp--; else
                if(tmp < signal-1) tmp++;
                if(SenderOkay >= 190)  ppm_diff[index] = ((tmp - ppm_in[index]) / 3) * 3;
                else ppm_diff[index] = 0;
                ppm_in[index] = tmp;
                }
                        else ROT_ON;
            if(index == 5) J3High; else J3Low;  // Servosignal an J3 anlegen
            if(index == 6) J4High; else J4Low;  // Servosignal an J4 anlegen
            if(index == 7) J5High; else J5Low;  // Servosignal an J5 anlegen 
          }
                  if(index < 20) index++;
          else
                  if(index == 20)
                  {
            unsigned char i;
            ROT_ON;
                    index = 30;
            for(i=0;i<11;i++) // restore from older data
                         {
                      PPM_in[i] = old_ppm_in[i];
                      PPM_diff[i] = 0;
//                        okay_cnt /= 2;
                 }
                  }
            }
 DebugOut.Analog[16] = okay_cnt;
 DebugOut.Analog[17] = PPM_in[2];
}
 
 
/*
//############################################################################
//Diese Routine startet und inizialisiert den Timer für RC
SIGNAL(SIG_INPUT_CAPTURE1)
//############################################################################
 
{
        static unsigned int AltICR=0;
    signed int signal = 0,tmp;
        static int index;              
               
        signal = (unsigned int) ICR1 - AltICR;         
DebugOut.Analog[16] = signal;
        signal /= 2;
        AltICR = ICR1; 
    //Syncronisationspause?
        if((signal > 1100*2) && (signal < 8000*2))       
        {
        if(index >= 4)  NewPpmData = 0;  // Null bedeutet: Neue Daten
        index = 1;             
        }
        else
        {
        if(index < 10)
            {
            if((signal > 250) && (signal < 687*2))
                {
                signal -= 962;
                // Stabiles Signal
                if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10;}
                tmp = (3 * (PPM_in[index]) + signal) / 4;  
                if(tmp > signal+1) tmp--; else
                if(tmp < signal-1) tmp++;
                if(SenderOkay >= 195)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3;
                else PPM_diff[index] = 0;
                PPM_in[index] = tmp;
                }
            index++;  
         if(index == 5) J3High; else J3Low;  // Servosignal an J3 anlegen
         if(index == 2) J4High; else J4Low;  // Servosignal an J4 anlegen
         if(index == 7) J5High; else J5Low;  // Servosignal an J5 anlegen
        }
        }
}
*/
 
 
 
 

Rev 1278	Rev 1283
1	/*#######################################################################################	1	/*#######################################################################################
2	Decodieren eines RC Summen Signals	2	Decodieren eines RC Summen Signals
3	#######################################################################################*/	3	#######################################################################################*/
4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5	// + Copyright (c) 04.2007 Holger Buss	5	// + Copyright (c) 04.2007 Holger Buss
6	// + only for non-profit use	6	// + only for non-profit use
7	// + www.MikroKopter.com	7	// + www.MikroKopter.com
8	// + see the File "License.txt" for further Informations	8	// + see the File "License.txt" for further Informations
9	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	9	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
10		10
11	#include "rc.h"	11	#include "rc.h"
12	#include "main.h"	12	#include "main.h"
13		13
14	volatile int PPM_in[11];	14	volatile int PPM_in[11];
15	volatile int PPM_diff[11]; // das diffenzierte Stick-Signal	15	volatile int PPM_diff[11]; // das diffenzierte Stick-Signal
16	volatile char Channels,tmpChannels = 0;	16	volatile char Channels,tmpChannels = 0;
17	volatile unsigned char NewPpmData = 1;	17	volatile unsigned char NewPpmData = 1;
18		18
19	//############################################################################	19	//############################################################################
20	//zum decodieren des PPM-Signals wird Timer1 mit seiner Input	20	//zum decodieren des PPM-Signals wird Timer1 mit seiner Input
21	//Capture Funktion benutzt:	21	//Capture Funktion benutzt:
22	void rc_sum_init (void)	22	void rc_sum_init (void)
23	//############################################################################	23	//############################################################################
24	{	24	{
25	TCCR1B=(1<<CS11)\|(1<<CS10)\|(1<<ICES1)\|(1<<ICNC1);//\|(1 << WGM12); //timer1 prescale 64	25	TCCR1B=(1<<CS11)\|(1<<CS10)\|(1<<ICES1)\|(1<<ICNC1);//\|(1 << WGM12); //timer1 prescale 64
26	// TCCR1B=(1<<CS11)\|(0<<CS10)\|(1<<ICES1)\|(1<<ICNC1); //timer1 prescale 64	26	// TCCR1B=(1<<CS11)\|(0<<CS10)\|(1<<ICES1)\|(1<<ICNC1); //timer1 prescale 64
27	TIMSK1 \|= _BV(ICIE1);	27	TIMSK1 \|= _BV(ICIE1);
28	AdNeutralGier = 0;	28	AdNeutralGier = 0;
29	AdNeutralRoll = 0;	29	AdNeutralRoll = 0;
30	AdNeutralNick = 0;	30	AdNeutralNick = 0;
31	return;	31	return;
32	}	32	}
33	/*	33	/*
34	//############################################################################	34	//############################################################################
35	//Diese Routine startet und inizialisiert den Timer für RC	35	//Diese Routine startet und inizialisiert den Timer für RC
36	SIGNAL(SIG_INPUT_CAPTURE1)	36	SIGNAL(SIG_INPUT_CAPTURE1)
37	//############################################################################	37	//############################################################################
38		38
39	{	39	{
40	static unsigned int AltICR=0;	40	static unsigned int AltICR=0;
41	signed int signal = 0,tmp;	41	signed int signal = 0,tmp;
42	static int index;	42	static int index;
43		43
44	signal = (unsigned int) ICR1 - AltICR;	44	signal = (unsigned int) ICR1 - AltICR;
45	AltICR = ICR1;	45	AltICR = ICR1;
46	//Syncronisationspause?	46	//Syncronisationspause? (3.52 ms < signal < 25.6 ms)
47	if((signal > 1100) && (signal < 8000))	47	if((signal > 1100) && (signal < 8000))
48	{	48	{
49	if(index >= 4) NewPpmData = 0; // Null bedeutet: Neue Daten	49	if(index >= 4) NewPpmData = 0; // Null bedeutet: Neue Daten
50	index = 1;	50	index = 1;
51	}	51	}
52	else	52	else
53	{	53	{
54	if(index < 10)	54	if(index < 10)
55	{	55	{
56	if((signal > 250) && (signal < 687))	56	if((signal > 250) && (signal < 687))
57	{	57	{
58	signal -= 466;	58	signal -= 466;
59	// Stabiles Signal	59	// Stabiles Signal
60	if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10; else SenderOkay = 200;}	60	if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10; else SenderOkay = 200;}
61	tmp = (3 * (PPM_in[index]) + signal) / 4;	61	tmp = (3 * (PPM_in[index]) + signal) / 4;
62	if(tmp > signal+1) tmp--; else	62	if(tmp > signal+1) tmp--; else
63	if(tmp < signal-1) tmp++;	63	if(tmp < signal-1) tmp++;
64	if(SenderOkay >= 195) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3;	64	if(SenderOkay >= 195) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3;
65	else PPM_diff[index] = 0;	65	else PPM_diff[index] = 0;
66	PPM_in[index] = tmp;	66	PPM_in[index] = tmp;
67	}	67	}
68	index++;	68	index++;
69	if(index == 5) J3High; else J3Low; // Servosignal an J3 anlegen	69	if(index == 5) J3High; else J3Low; // Servosignal an J3 anlegen
70	if(index == 6) J4High; else J4Low; // Servosignal an J4 anlegen	70	if(index == 6) J4High; else J4Low; // Servosignal an J4 anlegen
71	if(index == 7) J5High; else J5Low; // Servosignal an J5 anlegen	71	if(index == 7) J5High; else J5Low; // Servosignal an J5 anlegen
72	}	72	}
73	}	73	}
74	}	74	}
75	*/	75	*/
76		76
77		77
78	//############################################################################	78	//############################################################################
79	//Diese Routine startet und inizialisiert den Timer für RC	79	//Diese Routine startet und inizialisiert den Timer für RC
80	SIGNAL(SIG_INPUT_CAPTURE1)	80	SIGNAL(SIG_INPUT_CAPTURE1)
81	//############################################################################	81	//############################################################################
82		82
83	{	83	{
84	static unsigned int AltICR=0;	84	static unsigned int AltICR=0;
85	static int ppm_in[11];	85	static int ppm_in[11];
86	static int ppm_diff[11];	86	static int ppm_diff[11];
87	static int old_ppm_in[11];	87	static int old_ppm_in[11];
88	static int old_ppm_diff[11];	88	static int old_ppm_diff[11];
89	signed int signal = 0,tmp;	89	signed int signal = 0,tmp;
90	static unsigned char index, okay_cnt = 0;	90	static unsigned char index, okay_cnt = 0;
91	signal = (unsigned int) ICR1 - AltICR;	91	signal = (unsigned int) ICR1 - AltICR;
92	AltICR = ICR1;	92	AltICR = ICR1;
93	//Syncronisationspause?	93	//Syncronisationspause? (3.52 ms < signal < 25.6 ms)
94	if((signal > 1100) && (signal < 8000))	94	if((signal > 1100) && (signal < 8000))
95	{	95	{
96	tmpChannels = index;	96	tmpChannels = index;
97	if(tmpChannels >= 4 && Channels == tmpChannels)	97	if(tmpChannels >= 4 && Channels == tmpChannels)
98	{	98	{
99	if(okay_cnt > 10)	99	if(okay_cnt > 10)
100	{	100	{
101	NewPpmData = 0; // Null bedeutet: Neue Daten	101	NewPpmData = 0; // Null bedeutet: Neue Daten
102	for(index = 0; index < 11; index++)	102	for(index = 0; index < 11; index++)
103	{	103	{
104	if(okay_cnt > 30)	104	if(okay_cnt > 30)
105	{	105	{
106	old_ppm_in[index] = PPM_in[index];	106	old_ppm_in[index] = PPM_in[index];
107	old_ppm_diff[index] = PPM_diff[index];	107	old_ppm_diff[index] = PPM_diff[index];
108	}	108	}
109	PPM_in[index] = ppm_in[index];	109	PPM_in[index] = ppm_in[index];
110	PPM_diff[index] = ppm_diff[index];	110	PPM_diff[index] = ppm_diff[index];
111	}	111	}
112	}	112	}
113	if(okay_cnt < 255) okay_cnt++;	113	if(okay_cnt < 255) okay_cnt++;
114	}	114	}
115	else	115	else
116	{	116	{
117	if(okay_cnt > 100) okay_cnt = 10; else okay_cnt = 0;	117	if(okay_cnt > 100) okay_cnt = 10; else okay_cnt = 0;
118	ROT_ON;	118	ROT_ON;
119	}	119	}
120	index = 1;	120	index = 1;
121	if(!MotorenEin) Channels = tmpChannels;	121	if(!MotorenEin) Channels = tmpChannels;
122	}	122	}
123	else	123	else
124	{	124	{
125	if(index < 10)	125	if(index < 10)
126	{	126	{
127	if((signal > 250) && (signal < 687))	127	if((signal > 250) && (signal < 687))
128	{	128	{
129	signal -= 466;	129	signal -= 466;
130	// Stabiles Signal	130	// Stabiles Signal
131	if((abs(signal - ppm_in[index]) < 6))	131	if((abs(signal - ppm_in[index]) < 6))
132	{	132	{
133	if(okay_cnt > 25) SenderOkay += 10;	133	if(okay_cnt > 25) SenderOkay += 10;
134	else	134	else
135	if(okay_cnt > 10) SenderOkay += 2;	135	if(okay_cnt > 10) SenderOkay += 2;
136	if(SenderOkay > 200) SenderOkay = 200;	136	if(SenderOkay > 200) SenderOkay = 200;
137	}	137	}
138	tmp = (3 * (ppm_in[index]) + signal) / 4;	138	tmp = (3 * (ppm_in[index]) + signal) / 4;
139	if(tmp > signal+1) tmp--; else	139	if(tmp > signal+1) tmp--; else
140	if(tmp < signal-1) tmp++;	140	if(tmp < signal-1) tmp++;
141	if(SenderOkay >= 190) ppm_diff[index] = ((tmp - ppm_in[index]) / 3) * 3;	141	if(SenderOkay >= 190) ppm_diff[index] = ((tmp - ppm_in[index]) / 3) * 3;
142	else ppm_diff[index] = 0;	142	else ppm_diff[index] = 0;
143	ppm_in[index] = tmp;	143	ppm_in[index] = tmp;
144	}	144	}
145	else ROT_ON;	145	else ROT_ON;
146	if(index == 5) J3High; else J3Low; // Servosignal an J3 anlegen	146	if(index == 5) J3High; else J3Low; // Servosignal an J3 anlegen
147	if(index == 6) J4High; else J4Low; // Servosignal an J4 anlegen	147	if(index == 6) J4High; else J4Low; // Servosignal an J4 anlegen
148	if(index == 7) J5High; else J5Low; // Servosignal an J5 anlegen	148	if(index == 7) J5High; else J5Low; // Servosignal an J5 anlegen
149	}	149	}
150	if(index < 20) index++;	150	if(index < 20) index++;
151	else	151	else
152	if(index == 20)	152	if(index == 20)
153	{	153	{
154	unsigned char i;	154	unsigned char i;
155	ROT_ON;	155	ROT_ON;
156	index = 30;	156	index = 30;
157	for(i=0;i<11;i++) // restore from older data	157	for(i=0;i<11;i++) // restore from older data
158	{	158	{
159	PPM_in[i] = old_ppm_in[i];	159	PPM_in[i] = old_ppm_in[i];
160	PPM_diff[i] = 0;	160	PPM_diff[i] = 0;
161	// okay_cnt /= 2;	161	// okay_cnt /= 2;
162	}	162	}
163	}	163	}
164	}	164	}
165	DebugOut.Analog[16] = okay_cnt;	165	DebugOut.Analog[16] = okay_cnt;
166	DebugOut.Analog[17] = PPM_in[2];	166	DebugOut.Analog[17] = PPM_in[2];
167	}	167	}
168		168
169		169
170	/*	170	/*
171	//############################################################################	171	//############################################################################
172	//Diese Routine startet und inizialisiert den Timer für RC	172	//Diese Routine startet und inizialisiert den Timer für RC
173	SIGNAL(SIG_INPUT_CAPTURE1)	173	SIGNAL(SIG_INPUT_CAPTURE1)
174	//############################################################################	174	//############################################################################
175		175
176	{	176	{
177	static unsigned int AltICR=0;	177	static unsigned int AltICR=0;
178	signed int signal = 0,tmp;	178	signed int signal = 0,tmp;
179	static int index;	179	static int index;
180		180
181	signal = (unsigned int) ICR1 - AltICR;	181	signal = (unsigned int) ICR1 - AltICR;
182	DebugOut.Analog[16] = signal;	182	DebugOut.Analog[16] = signal;
183	signal /= 2;	183	signal /= 2;
184	AltICR = ICR1;	184	AltICR = ICR1;
185	//Syncronisationspause?	185	//Syncronisationspause?
186	if((signal > 11002) && (signal < 80002))	186	if((signal > 11002) && (signal < 80002))
187	{	187	{
188	if(index >= 4) NewPpmData = 0; // Null bedeutet: Neue Daten	188	if(index >= 4) NewPpmData = 0; // Null bedeutet: Neue Daten
189	index = 1;	189	index = 1;
190	}	190	}
191	else	191	else
192	{	192	{
193	if(index < 10)	193	if(index < 10)
194	{	194	{
195	if((signal > 250) && (signal < 687*2))	195	if((signal > 250) && (signal < 687*2))
196	{	196	{
197	signal -= 962;	197	signal -= 962;
198	// Stabiles Signal	198	// Stabiles Signal
199	if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10;}	199	if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10;}
200	tmp = (3 * (PPM_in[index]) + signal) / 4;	200	tmp = (3 * (PPM_in[index]) + signal) / 4;
201	if(tmp > signal+1) tmp--; else	201	if(tmp > signal+1) tmp--; else
202	if(tmp < signal-1) tmp++;	202	if(tmp < signal-1) tmp++;
203	if(SenderOkay >= 195) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3;	203	if(SenderOkay >= 195) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3;
204	else PPM_diff[index] = 0;	204	else PPM_diff[index] = 0;
205	PPM_in[index] = tmp;	205	PPM_in[index] = tmp;
206	}	206	}
207	index++;	207	index++;
208	if(index == 5) J3High; else J3Low; // Servosignal an J3 anlegen	208	if(index == 5) J3High; else J3Low; // Servosignal an J3 anlegen
209	if(index == 2) J4High; else J4Low; // Servosignal an J4 anlegen	209	if(index == 2) J4High; else J4Low; // Servosignal an J4 anlegen
210	if(index == 7) J5High; else J5Low; // Servosignal an J5 anlegen	210	if(index == 7) J5High; else J5Low; // Servosignal an J5 anlegen
211	}	211	}
212	}	212	}
213	}	213	}
214	*/	214	*/
215		215
216		216
217		217
218		218

Subversion Repositories FlightCtrl

(root)/branches/V0.86d_ACC-HH_HR_MartinR/rc.c @ 2139 - Rev 1278 → 1283