WebSVN - FlightCtrl - Diff - Rev 1104 and 1107 - /branches/V0.71h Code Redesign killagreg/timer2.c


#include <avr/io.h>
#include <avr/interrupt.h>
#include "fc.h"
#include "eeprom.h"
#include "uart.h"
#include "main.h"
 
volatile int16_t ServoValue = 0;
 
#define HEF4017R_ON     PORTC |=  (1<<PORTC6)
#define HEF4017R_OFF    PORTC &= ~(1<<PORTC6)
 
 
/*****************************************************/
/*              Initialize Timer 2                   */
/*****************************************************/
// The timer 2 is used to generate the PWM at PD7 (J7)
// to control a camera servo for nick compensation.
void TIMER2_Init(void)
{
        uint8_t sreg = SREG;
 
        // disable all interrupts before reconfiguration
        cli();
 
        // set PD7 as output of the PWM for nick servo
        DDRD  |= (1<<DDD7);
        PORTD &= ~(1<<PORTD7);  // set PD7 to low
 
        DDRC  |= (1<<DDC6);     // set PC6 as output (Reset for HEF4017)
        PORTC &= ~(1<<PORTC6);  // set PC6 to low
 
        // Timer/Counter 2 Control Register A
 
        // Waveform Generation Mode is Fast PWM (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 1)
    // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)
    // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
        TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0));
    TCCR2A |= (1<<WGM21)|(1<<WGM20);
 
    // Timer/Counter 2 Control Register B
 
        // Set clock divider for timer 2 to SYSKLOCK/64 = 20MHz / 64 = 312.5 kHz
        // The timer increments from 0x00 to 0xFF with an update rate of 312.5 kHz or 3.2 us
        // hence the timer overflow interrupt frequency is 312.5 kHz / 256 = 1220.7 Hz or 0.8192 ms
 
    // divider 64 (Bits: CS022 = 1, CS21 = 0, CS20 = 0)
        TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS21)|(1<<CS20)|(1<<WGM22));
    TCCR2B |= (1<<CS22);
 
        // Initialize the Timer/Counter 2 Register
    TCNT2 = 0;
 
        // Initialize the Output Compare Register A used for PWM generation on port PD7.
        OCR2A = 10;
 
        // Timer/Counter 2 Interrupt Mask Register
        // Enable timer output compare match A Interrupt only
        TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2));
        TIMSK2 |= (1<<OCIE2A);
 
    SREG = sreg;
}
 
 
/*****************************************************/
/*              Control Servo Position               */
/*****************************************************/
 
ISR(TIMER2_COMPA_vect)  // every  256 * 3.2 us = 0.819 us ( on compare match of TCNT2 and OC2A)
{
        static  uint8_t PostPulse = 0x80;       // value for last pwm cycle in non inverting mode (clear pin on compare match)
        static uint16_t FilterServo = 100;      // initial value, after some iterations it becomes the average value of 2 * FCParam.ServoNickControl
        static uint16_t ServoState = 40;        // cycle down counter for this ISR
 
        #define MULTIPLIER 4
 
        if(BoardRelease < 99)
        {
                switch(ServoState)
                {
                        case 4:
                                // recalculate new ServoValue
                                ServoValue = 0x0030; // Offset (part 1)
 
                                FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset
                                ServoValue += FilterServo; // add filtered static offset
 
                                if(ParamSet.ServoNickCompInvert & 0x01)
                                {       // inverting movement of servo
                                        ServoValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) );
                                }
                                else
                                {       // non inverting movement of servo
                                        ServoValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) );
                                }
 
                                // limit servo value to its parameter range definition
                                if(ServoValue < ((int16_t)ParamSet.ServoNickMin * 3) )
                                {
                                        ServoValue = (int16_t)ParamSet.ServoNickMin * 3;
                                }
                                else
                                if(ServoValue > ((int16_t)ParamSet.ServoNickMax * 3) )
                                {
                                        ServoValue = (int16_t)ParamSet.ServoNickMax * 3;
                                }
 
                                DebugOut.Analog[20] = ServoValue;
 
                                // determine prepulse width (remaining part of ServoValue/Timer Cycle)
                                if ((ServoValue % 255) < 45)
                                {       // if prepulse width is to short the execution time of this ISR is longer than the next compare match
                                        // so balance with postpulse width
                                        ServoValue += 77;
                                        PostPulse = 0x60 - 77;
                                }
                                else
                                {
                                        PostPulse = 0x60;
                                }
                                // set output compare register to 255 - prepulse width
                                OCR2A = 255 - (ServoValue % 256);
                                // connect OC2A in inverting mode (Clear pin on overflow, Set pin on compare match)
                                TCCR2A=(1<<COM2A1)|(1<<COM2A0)|(1<<WGM21)|(1<<WGM20);
 
                                break;
 
                        case 3:
                        case 2:
                        case 1:
 
                                if(ServoValue > 255)        // is larger than a full timer 2 cycle
                                {
                                        PORTD |= (1<<PORTD7);                   // set PD7 to high
                                        TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A
                                        ServoValue -= 255;              // substract full timer cycle
                                }
                                else // the post pule must be generated
                                {
                                        TCCR2A=(1<<COM2A1)|(0<<COM2A0)|(1<<WGM21)|(1<<WGM20); // connect OC2A in non inverting mode
                                        OCR2A = PostPulse; // Offset Part2
                                        ServoState = 1;    // jump to ServoState 0 with next ISR call
                                }
                        break;
 
                        case 0:
                                ServoState  = (uint16_t) ParamSet.ServoNickRefresh * MULTIPLIER;        // reload ServoState
                                PORTD &= ~(1<<PORTD7);                                                                                          // set PD7 to low
                                TCCR2A = (1<<WGM21)|(1<<WGM20);                                                         // disconnect OC2A
                                break;
 
                        default:
                                // do nothing
                                break;
                }
        ServoState--;
        } // EOF BoardRelease < 20
        else // output to HEF4014
        {
                // code for HEF4014 output must be placed here
        }
}
 
 

Subversion Repositories FlightCtrl

(root)/branches/V0.71h Code Redesign killagreg/timer2.c - Rev 1104 → 1107

Rev 1104		Rev 1107
1	#include <avr/io.h>	1	#include <avr/io.h>
2	#include <avr/interrupt.h>	2	#include <avr/interrupt.h>
3	#include "fc.h"	3	#include "fc.h"
4	#include "eeprom.h"	4	#include "eeprom.h"
5	#include "uart.h"	5	#include "uart.h"
6	#include "main.h"	6	#include "main.h"
7		7
8	volatile int16_t ServoValue = 0;	8	volatile int16_t ServoValue = 0;
9		9
10	#define HEF4017R_ON PORTC \|= (1<<PORTC6)	10	#define HEF4017R_ON PORTC \|= (1<<PORTC6)
11	#define HEF4017R_OFF PORTC &= ~(1<<PORTC6)	11	#define HEF4017R_OFF PORTC &= ~(1<<PORTC6)
12		12
13		13
14	/*****************************************************/	14	/*****************************************************/
15	/* Initialize Timer 2 */	15	/* Initialize Timer 2 */
16	/*****************************************************/	16	/*****************************************************/
17	// The timer 2 is used to generate the PWM at PD7 (J7)	17	// The timer 2 is used to generate the PWM at PD7 (J7)
18	// to control a camera servo for nick compensation.	18	// to control a camera servo for nick compensation.
19	void TIMER2_Init(void)	19	void TIMER2_Init(void)
20	{	20	{
21	uint8_t sreg = SREG;	21	uint8_t sreg = SREG;
22		22
23	// disable all interrupts before reconfiguration	23	// disable all interrupts before reconfiguration
24	cli();	24	cli();
25		25
26	// set PD7 as output of the PWM for nick servo	26	// set PD7 as output of the PWM for nick servo
27	DDRD \|= (1<<DDD7);	27	DDRD \|= (1<<DDD7);
28	PORTD &= ~(1<<PORTD7); // set PD7 to low	28	PORTD &= ~(1<<PORTD7); // set PD7 to low
29		29
30	DDRC \|= (1<<DDC6); // set PC6 as output (Reset for HEF4017)	30	DDRC \|= (1<<DDC6); // set PC6 as output (Reset for HEF4017)
31	PORTC &= ~(1<<PORTC6); // set PC6 to low	31	PORTC &= ~(1<<PORTC6); // set PC6 to low
32		32
33	// Timer/Counter 2 Control Register A	33	// Timer/Counter 2 Control Register A
34		34
35	// Waveform Generation Mode is Fast PWM (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 1)	35	// Waveform Generation Mode is Fast PWM (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 1)
36	// PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)	36	// PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)
37	// PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)	37	// PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
38	TCCR2A &= ~((1<<COM2A1)\|(1<<COM2A0)\|(1<<COM2B1)\|(1<<COM2B0));	38	TCCR2A &= ~((1<<COM2A1)\|(1<<COM2A0)\|(1<<COM2B1)\|(1<<COM2B0));
39	TCCR2A \|= (1<<WGM21)\|(1<<WGM20);	39	TCCR2A \|= (1<<WGM21)\|(1<<WGM20);
40		40
41	// Timer/Counter 2 Control Register B	41	// Timer/Counter 2 Control Register B
42		42
43	// Set clock divider for timer 2 to SYSKLOCK/64 = 20MHz / 64 = 312.5 kHz	43	// Set clock divider for timer 2 to SYSKLOCK/64 = 20MHz / 64 = 312.5 kHz
44	// The timer increments from 0x00 to 0xFF with an update rate of 312.5 kHz or 3.2 us	44	// The timer increments from 0x00 to 0xFF with an update rate of 312.5 kHz or 3.2 us
45	// hence the timer overflow interrupt frequency is 312.5 kHz / 256 = 1220.7 Hz or 0.8192 ms	45	// hence the timer overflow interrupt frequency is 312.5 kHz / 256 = 1220.7 Hz or 0.8192 ms
46		46
47	// divider 64 (Bits: CS022 = 1, CS21 = 0, CS20 = 0)	47	// divider 64 (Bits: CS022 = 1, CS21 = 0, CS20 = 0)
48	TCCR2B &= ~((1<<FOC2A)\|(1<<FOC2B)\|(1<<CS21)\|(1<<CS20)\|(1<<WGM22));	48	TCCR2B &= ~((1<<FOC2A)\|(1<<FOC2B)\|(1<<CS21)\|(1<<CS20)\|(1<<WGM22));
49	TCCR2B \|= (1<<CS22);	49	TCCR2B \|= (1<<CS22);
50		50
51	// Initialize the Timer/Counter 2 Register	51	// Initialize the Timer/Counter 2 Register
52	TCNT2 = 0;	52	TCNT2 = 0;
53		53
54	// Initialize the Output Compare Register A used for PWM generation on port PD7.	54	// Initialize the Output Compare Register A used for PWM generation on port PD7.
55	OCR2A = 10;	55	OCR2A = 10;
56		56
57	// Timer/Counter 2 Interrupt Mask Register	57	// Timer/Counter 2 Interrupt Mask Register
58	// Enable timer output compare match A Interrupt only	58	// Enable timer output compare match A Interrupt only
59	TIMSK2 &= ~((1<<OCIE2B)\|(1<<TOIE2));	59	TIMSK2 &= ~((1<<OCIE2B)\|(1<<TOIE2));
60	TIMSK2 \|= (1<<OCIE2A);	60	TIMSK2 \|= (1<<OCIE2A);
61		61
62	SREG = sreg;	62	SREG = sreg;
63	}	63	}
64		64
65		65
66	/*****************************************************/	66	/*****************************************************/
67	/* Control Servo Position */	67	/* Control Servo Position */
68	/*****************************************************/	68	/*****************************************************/
69		69
70	ISR(TIMER2_COMPA_vect) // every 256 * 3.2 us = 0.819 us ( on compare match of TCNT2 and OC2A)	70	ISR(TIMER2_COMPA_vect) // every 256 * 3.2 us = 0.819 us ( on compare match of TCNT2 and OC2A)
71	{	71	{
72	static uint8_t PostPulse = 0x80; // value for last pwm cycle in non inverting mode (clear pin on compare match)	72	static uint8_t PostPulse = 0x80; // value for last pwm cycle in non inverting mode (clear pin on compare match)
73	static uint16_t FilterServo = 100; // initial value, after some iterations it becomes the average value of 2 * FCParam.ServoNickControl	73	static uint16_t FilterServo = 100; // initial value, after some iterations it becomes the average value of 2 * FCParam.ServoNickControl
74	static uint16_t ServoState = 40; // cycle down counter for this ISR	74	static uint16_t ServoState = 40; // cycle down counter for this ISR
75		75
76	#define MULTIPLIER 4	76	#define MULTIPLIER 4
77		77
78	if(BoardRelease < 99)	78	if(BoardRelease < 99)
79	{	79	{
80	switch(ServoState)	80	switch(ServoState)
81	{	81	{
82	case 4:	82	case 4:
83	// recalculate new ServoValue	83	// recalculate new ServoValue
84	ServoValue = 0x0030; // Offset (part 1)	84	ServoValue = 0x0030; // Offset (part 1)
85	DebugOut.Analog[16] = ServoValue;	-
86	FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset	85	FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset
87	ServoValue += FilterServo; // add filtered static offset	86	ServoValue += FilterServo; // add filtered static offset
88		-
89	if(ParamSet.ServoNickCompInvert & 0x01)	87	if(ParamSet.ServoNickCompInvert & 0x01)
90	{ // inverting movement of servo	88	{ // inverting movement of servo
91	ServoValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) );	89	ServoValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) );
92	}	90	}
93	else	91	else
94	{ // non inverting movement of servo	92	{ // non inverting movement of servo
95	ServoValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) );	93	ServoValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) );
96	}	94	}
97		-
98	// limit servo value to its parameter range definition	95	// limit servo value to its parameter range definition
99	if(ServoValue < ((int16_t)ParamSet.ServoNickMin * 3) )	96	if(ServoValue < ((int16_t)ParamSet.ServoNickMin * 3) )
100	{	97	{
101	ServoValue = (int16_t)ParamSet.ServoNickMin * 3;	98	ServoValue = (int16_t)ParamSet.ServoNickMin * 3;
102	}	99	}
103	else	100	else
104	if(ServoValue > ((int16_t)ParamSet.ServoNickMax * 3) )	101	if(ServoValue > ((int16_t)ParamSet.ServoNickMax * 3) )
105	{	102	{
106	ServoValue = (int16_t)ParamSet.ServoNickMax * 3;	103	ServoValue = (int16_t)ParamSet.ServoNickMax * 3;
107	}	104	}
108		-
109	DebugOut.Analog[20] = ServoValue;	105	DebugOut.Analog[20] = ServoValue;
-		106
110	// determine prepulse width (remaining part of ServoValue/Timer Cycle)	107	// determine prepulse width (remaining part of ServoValue/Timer Cycle)
111	if ((ServoValue % 255) < 45)	108	if ((ServoValue % 255) < 45)
112	{ // if prepulse width is to short the execution time of this ISR is longer than the next compare match	109	{ // if prepulse width is to short the execution time of this ISR is longer than the next compare match
113	// so balance with postpulse width	110	// so balance with postpulse width
114	ServoValue += 77;	111	ServoValue += 77;
115	PostPulse = 0x60 - 77;	112	PostPulse = 0x60 - 77;
116	}	113	}
117	else	114	else
118	{	115	{
119	PostPulse = 0x60;	116	PostPulse = 0x60;
120	}	117	}
121	// set output compare register to 255 - prepulse width	118	// set output compare register to 255 - prepulse width
122	OCR2A = 255 - (ServoValue % 256);	119	OCR2A = 255 - (ServoValue % 256);
123	// connect OC2A in inverting mode (Clear pin on overflow, Set pin on compare match)	120	// connect OC2A in inverting mode (Clear pin on overflow, Set pin on compare match)
124	TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|(1<<WGM21)\|(1<<WGM20);	121	TCCR2A=(1<<COM2A1)\|(1<<COM2A0)\|(1<<WGM21)\|(1<<WGM20);
125		122
126	break;	123	break;
127		124
128	case 3:	125	case 3:
129	case 2:	126	case 2:
130	case 1:	127	case 1:
131		128
132	if(ServoValue > 255) // is larger than a full timer 2 cycle	129	if(ServoValue > 255) // is larger than a full timer 2 cycle
133	{	130	{
134	PORTD \|= (1<<PORTD7); // set PD7 to high	131	PORTD \|= (1<<PORTD7); // set PD7 to high
135	TCCR2A = (1<<WGM21)\|(1<<WGM20); // disconnect OC2A	132	TCCR2A = (1<<WGM21)\|(1<<WGM20); // disconnect OC2A
136	ServoValue -= 255; // substract full timer cycle	133	ServoValue -= 255; // substract full timer cycle
137	}	134	}
138	else // the post pule must be generated	135	else // the post pule must be generated
139	{	136	{
140	TCCR2A=(1<<COM2A1)\|(0<<COM2A0)\|(1<<WGM21)\|(1<<WGM20); // connect OC2A in non inverting mode	137	TCCR2A=(1<<COM2A1)\|(0<<COM2A0)\|(1<<WGM21)\|(1<<WGM20); // connect OC2A in non inverting mode
141	OCR2A = PostPulse; // Offset Part2	138	OCR2A = PostPulse; // Offset Part2
142	ServoState = 1; // jump to ServoState 0 with next ISR call	139	ServoState = 1; // jump to ServoState 0 with next ISR call
143	}	140	}
144	break;	141	break;
145		142
146	case 0:	143	case 0:
147	ServoState = (uint16_t) ParamSet.ServoNickRefresh * MULTIPLIER; // reload ServoState	144	ServoState = (uint16_t) ParamSet.ServoNickRefresh * MULTIPLIER; // reload ServoState
148	PORTD &= ~(1<<PORTD7); // set PD7 to low	145	PORTD &= ~(1<<PORTD7); // set PD7 to low
149	TCCR2A = (1<<WGM21)\|(1<<WGM20); // disconnect OC2A	146	TCCR2A = (1<<WGM21)\|(1<<WGM20); // disconnect OC2A
150	break;	147	break;
151		148
152	default:	149	default:
153	// do nothing	150	// do nothing
154	break;	151	break;
155	}	152	}
156	ServoState--;	153	ServoState--;
157	} // EOF BoardRelease < 20	154	} // EOF BoardRelease < 20
158	else // output to HEF4014	155	else // output to HEF4014
159	{	156	{
160	// code for HEF4014 output must be placed here	157	// code for HEF4014 output must be placed here
161	}	158	}
162	}	159	}
163		160
164		161