WebSVN - FlightCtrl - Diff - Rev 821 and 832 - /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 int16_t RC_Quality = 0;
 
volatile uint8_t NewRCFrames = 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)
        // Enable Overflow Interrupt (bit: TOIE1=0)
        TIMSK1 &= ~((1<<OCIE1B)|(1<<OCIE1A));
    TIMSK1 |= (1<<ICIE1)|(1<<TOIE1);
 
    RC_Quality = 0;
 
    SREG = sreg;
}
 
 
// happens every 0.209712 s.
// check for at least one new frame per timer overflow (timeout)
ISR(TIMER1_OVF_vect)
{
 
 
 
        if (NewRCFrames == 0) RC_Quality /= 2;
        NewRCFrames = 0;
}
 
 
/********************************************************************/
/*         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
{
    int16_t signal = 0, tmp;
        static int16_t index;
        static uint16_t oldICR1 = 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
                        NewRCFrames++;
                }
                // 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
                if(abs(signal-PPM_in[index]) < 6)
                {
                                        if(RC_Quality < 200) RC_Quality +=10;
                                }
                                // 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 >= 195)  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
            }
            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);
                }
        }
        }
        if(RC_Quality) RC_Quality--;
}
 
 
 
 
 
 

Subversion Repositories FlightCtrl

(root)/branches/V0.68d Code Redesign killagreg/rc.c @ 950 - Rev 821 → 832

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