WebSVN - FlightCtrl - Diff - Rev 1377 and 1438


/*#######################################################################################
Decodieren eines RC Summen Signals
#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) Holger Buss, Ingo Busker
// + only for non-profit use
// + www.MikroKopter.com
// + porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
// + see the File "License.txt" for further Informations
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#include "rc.h"
#include "main.h"
 
 
 
volatile int PPM_in[26];
volatile int PPM_diff[26];  // 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)
//############################################################################
{
 unsigned char i;
 for(i=0;i<26;i++)
  {
   PPM_in[i] = 0;
   PPM_diff[i] = 0;
  }
 
 
 
 
 
 
    AdNeutralGier = 0;
    AdNeutralRoll = 0;
    AdNeutralNick = 0;
    return;
}
 
#ifndef ACT_S3D_SUMMENSIGNAL
//############################################################################
//Diese Routine startet und inizialisiert den Timer für RC
SIGNAL(SIG_INPUT_CAPTURE1)
//############################################################################
{
if(!(EE_Parameter.ExtraConfig & CFG_SENSITIVE_RC))
 {
        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))   
        {
        Channels = index;
        if(index >= 4)  NewPpmData = 0;  // Null bedeutet: Neue Daten
        index = 1;             
        }
        else
        {
        if(index < 13)
            {
            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 
        }
        }
 }
 else
 {
        static unsigned int AltICR=0;
    static int ppm_in[13];
    static int ppm_diff[13];  
    static int old_ppm_in[13];
    static int old_ppm_diff[13];  
    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 < 13; 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 < 13)
            {
            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<13;i++) // restore from older data
                         {
                      PPM_in[i] = old_ppm_in[i];
                      PPM_diff[i] = 0;
//                        okay_cnt /= 2;
                 }
                  }
            }
 }
}
 
#else 
 
//############################################################################
//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 < 13)
            {
            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++;  
        }
        }
}
#endif
 
 

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

Subversion Repositories FlightCtrl

(root)/branches/Znib/V0.84a/rc.c @ 2100 - Rev 1377 → 1438