WebSVN - FlightCtrl - Diff - Rev 685 and 687 - /branches/V0.68d Code Redesign killagreg/timer2.c


#include <avr/io.h>
#include <avr/interrupt.h>
#include "fc.h"
#include "eeprom.h"
 
volatile int16_t ServoValue = 0;
 
 
/*****************************************************/
/*              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 for the PWM
        DDRD  |=(1<<DDD7);
        PORTB |= (1<<PORTD7);
 
 
        // Timer/Counter 2 Control Register A
 
        // Waveform Generation Mode is Fast PWM (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 1)
    // PD7: Clear OC2B on Compare Match, set OC2B at BOTTOM, noninverting PWM (Bits: COM2A1 = 1, COM2A0 = 0)
    // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
        TCCR2A &= ~((1<<COM2B1)|(1<<COM2B0)|(1<<COM2A0));
    TCCR2A |= (1<<COM2A1)|(1<<WGM21)|(1<<WGM20);
 
    // Timer/Counter 2 Control Register B
 
        // Set clock divider for timer 2 to SYSKLOCK/8 = 20MHz / 8 = 2.5MHz.
        // The timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz,
        // hence the timer overflow interrupt frequency is 2.5 MHz / 256 = 9.765 kHz
 
    // divider 8 (Bits: CS022 = 0, CS21 = 1, CS20 = 0)
        TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<WGM22));
    TCCR2B = (TCCR2B & 0xF8)|(0<<CS22)|(1<<CS21)|(0<<CS20);
 
        // Initialize the Output Compare Register A used for PWM generation on port PD7.
        OCR2A = 10;
 
        // Initialize the Timer/Counter 2 Register
    TCNT2 = 0;
 
        // 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)  // 9.765 kHz
{
  static uint8_t timer = 10;
 
  if(!timer--)
    {
     // enable PWM on PD7 in non inverting mode
     TCCR2A = (TCCR2A & 0x3F)|(1<<COM2A1)|(0<<COM2A0);
 
     ServoValue =  Parameter_ServoNickControl;
     // inverting movment of servo
     if(ParamSet.ServoNickCompInvert & 0x01)
     {
                 ServoValue += ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512;
         }
     else // non inverting movement of servo
     {
                 ServoValue -= ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512;
         }
 
         // limit servo value to its parameter range definition
     if(ServoValue < ParamSet.ServoNickMin)
     {
                 ServoValue = ParamSet.ServoNickMin;
         }
     else if(ServoValue > ParamSet.ServoNickMax)
     {
                 ServoValue = ParamSet.ServoNickMax;
     }
 
         // update PWM
     OCR2A = ServoValue;
     timer = ParamSet.ServoNickRefresh;
    }
    else
    {
         // disable PWM at PD7
     TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0));
     // set PD7 to low
     PORTD &= ~(1<<PORTD7);
    }
}
 

Rev 685	Rev 687
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		5
5	volatile int16_t ServoValue = 0;	6	volatile int16_t ServoValue = 0;
6		7
7		8
8	/*****************************************************/	9	/*****************************************************/
9	/* Initialize Timer 2 */	10	/* Initialize Timer 2 */
10	/*****************************************************/	11	/*****************************************************/
11	// The timer 2 is used to generate the PWM at PD7 (J7)	12	// The timer 2 is used to generate the PWM at PD7 (J7)
12	// to control a camera servo for nick compensation.	13	// to control a camera servo for nick compensation.
13	void TIMER2_Init(void)	14	void TIMER2_Init(void)
14	{	15	{
15	uint8_t sreg = SREG;	16	uint8_t sreg = SREG;
16		17
17	// disable all interrupts before reconfiguration	18	// disable all interrupts before reconfiguration
18	cli();	19	cli();
19		20
20	// set PD7 as output for the PWM	21	// set PD7 as output for the PWM
21	DDRD \|=(1<<DDD7);	22	DDRD \|=(1<<DDD7);
22	PORTB \|= (1<<PORTD7);	23	PORTB \|= (1<<PORTD7);
23		24
24		25
25	// Timer/Counter 2 Control Register A	26	// Timer/Counter 2 Control Register A
26		27
27	// Waveform Generation Mode is Fast PWM (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 1)	28	// Waveform Generation Mode is Fast PWM (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 1)
28	// PD7: Clear OC2B on Compare Match, set OC2B at BOTTOM, noninverting PWM (Bits: COM2A1 = 1, COM2A0 = 0)	29	// PD7: Clear OC2B on Compare Match, set OC2B at BOTTOM, noninverting PWM (Bits: COM2A1 = 1, COM2A0 = 0)
29	// PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)	30	// PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
30	TCCR2A &= ~((1<<COM2B1)\|(1<<COM2B0)\|(1<<COM2A0));	31	TCCR2A &= ~((1<<COM2B1)\|(1<<COM2B0)\|(1<<COM2A0));
31	TCCR2A \|= (1<<COM2A1)\|(1<<WGM21)\|(1<<WGM20);	32	TCCR2A \|= (1<<COM2A1)\|(1<<WGM21)\|(1<<WGM20);
32		33
33	// Timer/Counter 2 Control Register B	34	// Timer/Counter 2 Control Register B
34		35
35	// Set clock divider for timer 2 to SYSKLOCK/8 = 20MHz / 8 = 2.5MHz.	36	// Set clock divider for timer 2 to SYSKLOCK/8 = 20MHz / 8 = 2.5MHz.
36	// The timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz,	37	// The timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz,
37	// hence the timer overflow interrupt frequency is 2.5 MHz / 256 = 9.765 kHz	38	// hence the timer overflow interrupt frequency is 2.5 MHz / 256 = 9.765 kHz
38		39
39	// divider 8 (Bits: CS022 = 0, CS21 = 1, CS20 = 0)	40	// divider 8 (Bits: CS022 = 0, CS21 = 1, CS20 = 0)
40	TCCR2B &= ~((1<<FOC2A)\|(1<<FOC2B)\|(1<<WGM22));	41	TCCR2B &= ~((1<<FOC2A)\|(1<<FOC2B)\|(1<<WGM22));
41	TCCR2B = (TCCR2B & 0xF8)\|(0<<CS22)\|(1<<CS21)\|(0<<CS20);	42	TCCR2B = (TCCR2B & 0xF8)\|(0<<CS22)\|(1<<CS21)\|(0<<CS20);
42		43
43	// Initialize the Output Compare Register A used for PWM generation on port PD7.	44	// Initialize the Output Compare Register A used for PWM generation on port PD7.
44	OCR2A = 10;	45	OCR2A = 10;
45		46
46	// Initialize the Timer/Counter 2 Register	47	// Initialize the Timer/Counter 2 Register
47	TCNT2 = 0;	48	TCNT2 = 0;
48		49
49	// Timer/Counter 2 Interrupt Mask Register	50	// Timer/Counter 2 Interrupt Mask Register
50	// Enable timer output compare match A Interrupt only	51	// Enable timer output compare match A Interrupt only
51	TIMSK2 &= ~((1<<OCIE2B)\|(1<<TOIE2));	52	TIMSK2 &= ~((1<<OCIE2B)\|(1<<TOIE2));
52	TIMSK2 \|= (1<<OCIE2A);	53	TIMSK2 \|= (1<<OCIE2A);
53		54
54	SREG = sreg;	55	SREG = sreg;
55	}	56	}
56		57
57		58
58	/*****************************************************/	59	/*****************************************************/
59	/* Control Servo Position */	60	/* Control Servo Position */
60	/*****************************************************/	61	/*****************************************************/
61	ISR(TIMER2_COMPA_vect) // 9.765 kHz	62	ISR(TIMER2_COMPA_vect) // 9.765 kHz
62	{	63	{
63	static uint8_t timer = 10;	64	static uint8_t timer = 10;
64		65
65	if(!timer--)	66	if(!timer--)
66	{	67	{
67	// enable PWM on PD7 in non inverting mode	68	// enable PWM on PD7 in non inverting mode
68	TCCR2A = (TCCR2A & 0x3F)\|(1<<COM2A1)\|(0<<COM2A0);	69	TCCR2A = (TCCR2A & 0x3F)\|(1<<COM2A1)\|(0<<COM2A0);
69		70
70	ServoValue = Parameter_ServoNickControl;	71	ServoValue = Parameter_ServoNickControl;
71	// inverting movment of servo	72	// inverting movment of servo
72	if(EE_Parameter.ServoNickCompInvert & 0x01)	73	if(ParamSet.ServoNickCompInvert & 0x01)
73	{	74	{
74	ServoValue += ((int32_t) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;	75	ServoValue += ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512;
75	}	76	}
76	else // non inverting movement of servo	77	else // non inverting movement of servo
77	{	78	{
78	ServoValue -= ((int32_t) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;	79	ServoValue -= ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512;
79	}	80	}
80		81
81	// limit servo value to its parameter range definition	82	// limit servo value to its parameter range definition
82	if(ServoValue < EE_Parameter.ServoNickMin)	83	if(ServoValue < ParamSet.ServoNickMin)
83	{	84	{
84	ServoValue = EE_Parameter.ServoNickMin;	85	ServoValue = ParamSet.ServoNickMin;
85	}	86	}
86	else if(ServoValue > EE_Parameter.ServoNickMax)	87	else if(ServoValue > ParamSet.ServoNickMax)
87	{	88	{
88	ServoValue = EE_Parameter.ServoNickMax;	89	ServoValue = ParamSet.ServoNickMax;
89	}	90	}
90		91
91	// update PWM	92	// update PWM
92	OCR2A = ServoValue;	93	OCR2A = ServoValue;
93	timer = EE_Parameter.ServoNickRefresh;	94	timer = ParamSet.ServoNickRefresh;
94	}	95	}
95	else	96	else
96	{	97	{
97	// disable PWM at PD7	98	// disable PWM at PD7
98	TCCR2A &= ~((1<<COM2A1)\|(1<<COM2A0));	99	TCCR2A &= ~((1<<COM2A1)\|(1<<COM2A0));
99	// set PD7 to low	100	// set PD7 to low
100	PORTD &= ~(1<<PORTD7);	101	PORTD &= ~(1<<PORTD7);
101	}	102	}
102	}	103	}
103		104

Subversion Repositories FlightCtrl

(root)/branches/V0.68d Code Redesign killagreg/timer2.c - Rev 685 → 687