WebSVN - FlightCtrl - Diff - Rev 1356 and 1377 - /branches/V0.84a_var


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

Subversion Repositories FlightCtrl

(root)/branches/V0.84a_var_motorsmoothing/rc.c @ 1971 - Rev 1356 → 1377