WebSVN - FlightCtrl - Diff - Rev 766 and 800 - /branches/V0.68d Code Redesign killagreg/rc.c


/*#######################################################################################
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 <stdlib.h>
#include <avr/io.h>
#include <avr/interrupt.h>
 
#include "rc.h"
#include "main.h"
 
volatile int16_t PPM_in[15]; //PPM24 supports 12 channels per frame
volatile int16_t PPM_diff[15];
volatile uint8_t NewPpmData = 1;
volatile uint8_t RC_Quality = 0;
 
 
/***************************************************************/
/*  16bit timer 1 is used to decode the PPM-Signal            */
/***************************************************************/
void RC_Init (void)
{
        uint8_t sreg = SREG;
 
        // disable all interrupts before reconfiguration
        cli();
 
        // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
        DDRD &= ~(1<<DDD6);
        PORTD |= (1<<PORTD6);
 
        // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)
        // set as output
        DDRD |= (1<<DDD5)|(1<<DDD4);
        // low level
        PORTD &= ~((1<<PORTD5)|(1<<PORTD4));
 
        // PD3 can't be used in FC 1.1 if 2nd UART is activated
        // because TXD1 is at that port
        if(BoardRelease == 10)
        {
                DDRD |= (1<<PORTD3);
                PORTD &= ~(1<<PORTD3);
        }
 
        // Timer/Counter1 Control Register A, B, C
 
        // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
        // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
        // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
        // Enable input capture noise cancler (bit: ICNC1=1)
        // Trigger on positive edge of the input capture pin (bit: ICES1=1),
        // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs
    // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
        TCCR1A &= ~((1<<COM1A1)|(1<<COM1A0)|(1<<COM1B1)|(1<<COM1B0)|(1<<WGM11)|(1<<WGM10));
        TCCR1B &= ~((1<<WGM13)|(1<<WGM12)|(1<<CS12));
        TCCR1B |= (1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1);
        TCCR1C &= ~((1<<FOC1A)|(1<<FOC1B));
 
        // Timer/Counter1 Interrupt Mask Register
 
        // Enable Input Capture Interrupt (bit: ICIE1=1)
        // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
        // Disable Overflow Interrupt (bit: TOIE1=0)
        TIMSK1 &= ~((1<<OCIE1B)|(1<<OCIE1A)|(1<<TOIE1));
    TIMSK1 |= (1<<ICIE1);
 
    RC_Quality = 0;
 
    SREG = sreg;
}
 
 
/********************************************************************/
/*         Every time a positive edge is detected at PD6            */
/********************************************************************/
/*                               t-Frame
       <----------------------------------------------------------------------->
         ____   ______   _____   ________                ______    sync gap      ____
        |    | |      | |     | |        |              |      |                |
        |    | |      | |     | |        |              |      |                |
     ___|    |_|      |_|     |_|        |_.............|      |________________|
        <-----><-------><------><-------->              <------>                <---
          t0       t1      t2       t4                     tn                     t0
 
The PPM-Frame length is 22.5 ms.
Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms.
The minimum duration of all channels at minimum value is  8 * 1 ms = 8 ms.
The maximum duration of all channels at maximum value is  8 * 2 ms = 16 ms.
The remaining time of (22.5 - 8 ms) ms = 14.5 ms  to (22.5 - 16 ms) ms = 6.5 ms is
the syncronization gap.
*/
ISR(TIMER1_CAPT_vect) // typical rate of 1 ms to 2 ms
{
        static uint16_t oldICR1 = 0;
    int16_t signal = 0, tmp;
        static int16_t index;
        static int16_t Sum_RC_Quality = 0;
 
        // 16bit Input Capture Register ICR1 contains the timer value TCNT1
        // at the time the edge was detected
 
        // calculate the time delay to the previous event time which is stored in oldICR1
        // calculatiing the difference of the two uint16_t and converting the result to an int16_t
        // implicit handles a timer overflow 65535 -> 0 the right way.
        signal = (uint16_t) ICR1 - oldICR1;
        oldICR1 = ICR1;
 
 
    //sync gap? (3.52 ms < signal < 25.6 ms)
        if((signal > 1100) && (signal < 8000))
        {
                // if a sync gap happens and there where at least 4 channels decoded before
                // then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
                if(index >= 4)  NewPpmData = 0;  // Null means NewData for the first 4 channels
                // synchronize channel index
                index = 1;
        }
        else // within the PPM frame
        {
        if(index < 14) // PPM24 supports 12 channels
        {
                        // check for valid signal length (0.8 ms < signal < 2.1984 ms)
                        // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
            if((signal > 250) && (signal < 687))
            {
                                // shift signal to zero symmetric range  -154 to 159
                signal -= 466; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
                // check for stable signal
 
 
 
 
                                Sum_RC_Quality -= Sum_RC_Quality/128;
                                Sum_RC_Quality += 200 - abs(signal - PPM_in[index]); // max 200*128 = 25600
 
 
                                // calculate exponential history for signal
                tmp = (3 * (PPM_in[index]) + signal) / 4;
                if(tmp > signal+1) tmp--; else
                if(tmp < signal-1) tmp++;
                // calculate signal difference on good signal level
                if(RC_Quality >= 170)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
                else PPM_diff[index] = 0;
                PPM_in[index] = tmp; // update channel value
            }
            else
            {   // invalid PPM time
                                Sum_RC_Quality -= Sum_RC_Quality/4;
                        }
                        if(Sum_RC_Quality < 0) Sum_RC_Quality = 0;
                        RC_Quality = (uint8_t)(Sum_RC_Quality/128);
            index++; // next channel
            // demux sum signal for channels 5 to 7 to J3, J4, J5
                if(index == 5) PORTD |= (1<<PORTD5); else PORTD &= ~(1<<PORTD5);
                if(index == 6) PORTD |= (1<<PORTD4); else PORTD &= ~(1<<PORTD4);
                if(BoardRelease == 10)
                {
                                if(index == 7) PORTD |= (1<<PORTD3); else PORTD &= ~(1<<PORTD3);
                }
        }
        }
}
 
 
 
 
 
 

Rev 766	Rev 800
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 <stdlib.h>	11	#include <stdlib.h>
12	#include <avr/io.h>	12	#include <avr/io.h>
13	#include <avr/interrupt.h>	13	#include <avr/interrupt.h>
14		14
15	#include "rc.h"	15	#include "rc.h"
16	#include "main.h"	16	#include "main.h"
17		17
18	volatile int16_t PPM_in[15]; //PPM24 supports 12 channels per frame	18	volatile int16_t PPM_in[15]; //PPM24 supports 12 channels per frame
19	volatile int16_t PPM_diff[15];	19	volatile int16_t PPM_diff[15];
20	volatile uint8_t NewPpmData = 1;	20	volatile uint8_t NewPpmData = 1;
21	volatile uint8_t SenderOkay = 0;	21	volatile uint8_t RC_Quality = 0;
-		22
22		23
23	/***************************************************************/	24	/***************************************************************/
24	/* 16bit timer 1 is used to decode the PPM-Signal */	25	/* 16bit timer 1 is used to decode the PPM-Signal */
25	/***************************************************************/	26	/***************************************************************/
26	void RC_Init (void)	27	void RC_Init (void)
27	{	28	{
28	uint8_t sreg = SREG;	29	uint8_t sreg = SREG;
29		30
30	// disable all interrupts before reconfiguration	31	// disable all interrupts before reconfiguration
31	cli();	32	cli();
32		33
33	// PPM-signal is connected to the Input Capture Pin (PD6) of timer 1	34	// PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
34	DDRD &= ~(1<<DDD6);	35	DDRD &= ~(1<<DDD6);
35	PORTD \|= (1<<PORTD6);	36	PORTD \|= (1<<PORTD6);
36		37
37	// Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)	38	// Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)
38	// set as output	39	// set as output
39	DDRD \|= (1<<DDD5)\|(1<<DDD4);	40	DDRD \|= (1<<DDD5)\|(1<<DDD4);
40	// low level	41	// low level
41	PORTD &= ~((1<<PORTD5)\|(1<<PORTD4));	42	PORTD &= ~((1<<PORTD5)\|(1<<PORTD4));
42		43
43	// PD3 can't be used in FC 1.1 if 2nd UART is activated	44	// PD3 can't be used in FC 1.1 if 2nd UART is activated
44	// because TXD1 is at that port	45	// because TXD1 is at that port
45	if(BoardRelease == 10)	46	if(BoardRelease == 10)
46	{	47	{
47	DDRD \|= (1<<PORTD3);	48	DDRD \|= (1<<PORTD3);
48	PORTD &= ~(1<<PORTD3);	49	PORTD &= ~(1<<PORTD3);
49	}	50	}
50		51
51	// Timer/Counter1 Control Register A, B, C	52	// Timer/Counter1 Control Register A, B, C
52		53
53	// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)	54	// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
54	// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)	55	// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
55	// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)	56	// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
56	// Enable input capture noise cancler (bit: ICNC1=1)	57	// Enable input capture noise cancler (bit: ICNC1=1)
57	// Trigger on positive edge of the input capture pin (bit: ICES1=1),	58	// Trigger on positive edge of the input capture pin (bit: ICES1=1),
58	// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs	59	// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs
59	// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.	60	// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
60	TCCR1A &= ~((1<<COM1A1)\|(1<<COM1A0)\|(1<<COM1B1)\|(1<<COM1B0)\|(1<<WGM11)\|(1<<WGM10));	61	TCCR1A &= ~((1<<COM1A1)\|(1<<COM1A0)\|(1<<COM1B1)\|(1<<COM1B0)\|(1<<WGM11)\|(1<<WGM10));
61	TCCR1B &= ~((1<<WGM13)\|(1<<WGM12)\|(1<<CS12));	62	TCCR1B &= ~((1<<WGM13)\|(1<<WGM12)\|(1<<CS12));
62	TCCR1B \|= (1<<CS11)\|(1<<CS10)\|(1<<ICES1)\|(1<<ICNC1);	63	TCCR1B \|= (1<<CS11)\|(1<<CS10)\|(1<<ICES1)\|(1<<ICNC1);
63	TCCR1C &= ~((1<<FOC1A)\|(1<<FOC1B));	64	TCCR1C &= ~((1<<FOC1A)\|(1<<FOC1B));
64		65
65	// Timer/Counter1 Interrupt Mask Register	66	// Timer/Counter1 Interrupt Mask Register
66		67
67	// Enable Input Capture Interrupt (bit: ICIE1=1)	68	// Enable Input Capture Interrupt (bit: ICIE1=1)
68	// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)	69	// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
69	// Disable Overflow Interrupt (bit: TOIE1=0)	70	// Disable Overflow Interrupt (bit: TOIE1=0)
70	TIMSK1 &= ~((1<<OCIE1B)\|(1<<OCIE1A)\|(1<<TOIE1));	71	TIMSK1 &= ~((1<<OCIE1B)\|(1<<OCIE1A)\|(1<<TOIE1));
71	TIMSK1 \|= (1<<ICIE1);	72	TIMSK1 \|= (1<<ICIE1);
-		73
-		74	RC_Quality = 0;
72		75
73	SREG = sreg;	76	SREG = sreg;
74	}	77	}
75		78
76		79
77	/********************************************************************/	80	/********************************************************************/
78	/* Every time a positive edge is detected at PD6 */	81	/* Every time a positive edge is detected at PD6 */
79	/********************************************************************/	82	/********************************************************************/
80	/* t-Frame	83	/* t-Frame
81	<----------------------------------------------------------------------->	84	<----------------------------------------------------------------------->
82	____ ______ _____ ________ ______ sync gap ____	85	____ ______ _____ ________ ______ sync gap ____
83	\| \| \| \| \| \| \| \| \| \| \|	86	\| \| \| \| \| \| \| \| \| \| \|
84	\| \| \| \| \| \| \| \| \| \| \|	87	\| \| \| \| \| \| \| \| \| \| \|
85	___\| \|_\| \|_\| \|_\| \|_.............\| \|________________\|	88	___\| \|_\| \|_\| \|_\| \|_.............\| \|________________\|
86	<-----><-------><------><--------> <------> <---	89	<-----><-------><------><--------> <------> <---
87	t0 t1 t2 t4 tn t0	90	t0 t1 t2 t4 tn t0
88		91
89	The PPM-Frame length is 22.5 ms.	92	The PPM-Frame length is 22.5 ms.
90	Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.	93	Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
91	The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.	94	The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
92	The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms.	95	The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms.
93	The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms.	96	The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms.
94	The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms.	97	The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms.
95	The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is	98	The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is
96	the syncronization gap.	99	the syncronization gap.
97	*/	100	*/
98	ISR(TIMER1_CAPT_vect) // typical rate of 1 ms to 2 ms	101	ISR(TIMER1_CAPT_vect) // typical rate of 1 ms to 2 ms
99	{	102	{
100	static uint16_t oldICR1 = 0;	103	static uint16_t oldICR1 = 0;
101	int16_t signal = 0, tmp;	104	int16_t signal = 0, tmp;
102	static int16_t index;	105	static int16_t index;
-		106	static int16_t Sum_RC_Quality = 0;
103		107
104	// 16bit Input Capture Register ICR1 contains the timer value TCNT1	108	// 16bit Input Capture Register ICR1 contains the timer value TCNT1
105	// at the time the edge was detected	109	// at the time the edge was detected
106		110
107	// calculate the time delay to the previous event time which is stored in oldICR1	111	// calculate the time delay to the previous event time which is stored in oldICR1
-		112	// calculatiing the difference of the two uint16_t and converting the result to an int16_t
-		113	// implicit handles a timer overflow 65535 -> 0 the right way.
108	signal = (uint16_t) ICR1 - oldICR1;	114	signal = (uint16_t) ICR1 - oldICR1;
109	oldICR1 = ICR1;	115	oldICR1 = ICR1;
110		116
111		117
112	//sync gap? (3.52 ms < signal < 25.6 ms)	118	//sync gap? (3.52 ms < signal < 25.6 ms)
113	if((signal > 1100) && (signal < 8000))	119	if((signal > 1100) && (signal < 8000))
114	{	120	{
115	// if a sync gap happens and there where at least 4 channels decoded before	121	// if a sync gap happens and there where at least 4 channels decoded before
116	// then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.	122	// then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
117	if(index >= 4) NewPpmData = 0; // Null means NewData for the first 4 channels	123	if(index >= 4) NewPpmData = 0; // Null means NewData for the first 4 channels
118	// synchronize channel index	124	// synchronize channel index
119	index = 1;	125	index = 1;
120	}	126	}
121	else // within the PPM frame	127	else // within the PPM frame
122	{	128	{
123	if(index < 14) // PPM24 supports 12 channels	129	if(index < 14) // PPM24 supports 12 channels
124	{	130	{
125	// check for valid signal length (0.8 ms < signal < 2.1984 ms)	131	// check for valid signal length (0.8 ms < signal < 2.1984 ms)
126	// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625	132	// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
127	if((signal > 250) && (signal < 687))	133	if((signal > 250) && (signal < 687))
128	{	134	{
129	// shift signal to zero symmetric range -154 to 159	135	// shift signal to zero symmetric range -154 to 159
130	signal -= 466; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)	136	signal -= 466; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
131	// check for stable signal	137	// check for stable signal
132	// the deviation of the current signal level from the average must be less than 6 (aprox. 1%)	-
133	if(abs(signal - PPM_in[index]) < 6)	-
134	{	-
135	// a good signal condition increases SenderOkay by 10	-
136	// SignalOkay is decremented every 2 ms in main.c	138	Sum_RC_Quality -= Sum_RC_Quality/128;
137	// this variable is a level for the average rate of a noiseless rc signal	139	Sum_RC_Quality += 200 - abs(signal - PPM_in[index]); // max 200*128 = 25600
138	if(SenderOkay < 200) SenderOkay += 10;	-
139	}	-
140	// calculate exponential history for signal	140	// calculate exponential history for signal
141	tmp = (3 * (PPM_in[index]) + signal) / 4;	141	tmp = (3 * (PPM_in[index]) + signal) / 4;
142	if(tmp > signal+1) tmp--; else	142	if(tmp > signal+1) tmp--; else
143	if(tmp < signal-1) tmp++;	143	if(tmp < signal-1) tmp++;
144	// calculate signal difference on good signal level	144	// calculate signal difference on good signal level
145	if(SenderOkay >= 195) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction	145	if(RC_Quality >= 170) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
146	else PPM_diff[index] = 0;	146	else PPM_diff[index] = 0;
147	PPM_in[index] = tmp; // update channel value	147	PPM_in[index] = tmp; // update channel value
148	}	148	}
-		149	else
-		150	{ // invalid PPM time
-		151	Sum_RC_Quality -= Sum_RC_Quality/4;
-		152	}
-		153	if(Sum_RC_Quality < 0) Sum_RC_Quality = 0;
-		154	RC_Quality = (uint8_t)(Sum_RC_Quality/128);
149	index++; // next channel	155	index++; // next channel
150	// demux sum signal for channels 5 to 7 to J3, J4, J5	156	// demux sum signal for channels 5 to 7 to J3, J4, J5
151	if(index == 5) PORTD \|= (1<<PORTD5); else PORTD &= ~(1<<PORTD5);	157	if(index == 5) PORTD \|= (1<<PORTD5); else PORTD &= ~(1<<PORTD5);
152	if(index == 6) PORTD \|= (1<<PORTD4); else PORTD &= ~(1<<PORTD4);	158	if(index == 6) PORTD \|= (1<<PORTD4); else PORTD &= ~(1<<PORTD4);
153	if(BoardRelease == 10)	159	if(BoardRelease == 10)
154	{	160	{
155	if(index == 7) PORTD \|= (1<<PORTD3); else PORTD &= ~(1<<PORTD3);	161	if(index == 7) PORTD \|= (1<<PORTD3); else PORTD &= ~(1<<PORTD3);
156	}	162	}
157	}	163	}
158	}	164	}
159	}	165	}
160		166
161		167
162		168
163		169
164		170
165		171

Subversion Repositories FlightCtrl

(root)/branches/V0.68d Code Redesign killagreg/rc.c - Rev 766 → 800