WebSVN - FlightCtrl - Diff - Rev 756 and 757 - /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[11];
volatile int16_t PPM_diff[11];
volatile uint8_t NewPpmData = 1;
volatile uint8_t SenderOkay = 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
        // becouse 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);
 
    SREG = sreg;
}
 
 
/********************************************************************/
/*         Every time a positive edge is detected at PD6            */
/********************************************************************/
/*
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;
 
        // 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
        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
                // synchronize channel index
                index = 1;
        }
        else // within the PPM frame
        {
        if(index < 10) // channel limit is 9 because of the frame length of 22.5 ms
        {
                        // 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
                // the deviation of the current signal level from the average must be less than 6 (aprox. 1%)
                if(abs(signal - PPM_in[index]) < 6)
                {
                                        // a good signal condition increases SenderOkay by 10
                                        // SignalOkay is decremented every 2 ms in main.c
                                        // this variable is a level for the average rate of a noiseless rc signal
                                        if(SenderOkay < 200) SenderOkay += 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(SenderOkay >= 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);
                }
        }
        }
}
 
 
 
 
 
 

Subversion Repositories FlightCtrl

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

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