WebSVN - FlightCtrl - Diff - Rev 1914 and 1933 - /branches/V0.88n_ACC-HH_HR


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

Subversion Repositories FlightCtrl

(root)/branches/V0.88n_ACC-HH_HR_MartinR/rc.c @ 2759 - Rev 1914 → 1933