Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 935 → Rev 936

/branches/V0.70d Code Redesign killagreg/timer2.c
2,10 → 2,12
#include <avr/interrupt.h>
#include "fc.h"
#include "eeprom.h"
#include "uart.h"
 
volatile int16_t ServoValue = 0;
volatile uint16_t ServoValue = 0;
 
 
 
/*****************************************************/
/* Initialize Timer 2 */
/*****************************************************/
20,33 → 22,33
 
// set PD7 as output of the PWM for nick servo
DDRD |=(1<<DDD7);
PORTD |= (1<<PORTD7);
PORTD &= ~(1<<PORTD7); // set PD7 to low
 
 
// 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, non inverting PWM (Bits: COM2A1 = 1, COM2A0 = 0)
// PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, 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);
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/256 = 20MHz / 256 = 78.128 kHz
// The timer increments from 0x00 to 0xFF with an update rate of 78.128 kHz or 12.8 us
// hence the timer overflow interrupt frequency is 78.128 kHz / 256 = 305.176 Hz or 3.276 ms
// 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 256 (Bits: CS022 = 1, CS21 = 1, CS20 = 0)
TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS20)|(1<<WGM22));
TCCR2B |= (1<<CS22)|(1<<CS21);
// 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 Output Compare Register A used for PWM generation on port PD7.
OCR2A = 10; //10 * 12.8us = 1.28 ms high time
 
// 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));
59,46 → 61,92
/*****************************************************/
/* Control Servo Position */
/*****************************************************/
ISR(TIMER2_COMPA_vect) // every OCR2A * 12.8 us (compare match)
 
ISR(TIMER2_COMPA_vect) // every 256 * 3.2 us = 0.819 us ( on compare match of TCNT2 and OC2A)
{
static uint8_t timer = 10;
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
 
if(!timer--)
{
// enable PWM on PD7 in non inverting mode
TCCR2A &= ~(0<<COM2A0);
TCCR2A |= (1<<COM2A1);
#define MULTIPLIER 4
 
ServoValue = FCParam.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);
}
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 += ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER);
}
else
{ // non inverting movement of servo
ServoValue -= ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER);
}
 
// limit servo value to its parameter range definition
if(ServoValue < ((uint16_t)ParamSet.ServoNickMin * 3) )
{
ServoValue = (uint16_t)ParamSet.ServoNickMin * 3;
}
else
if(ServoValue > ((uint16_t)ParamSet.ServoNickMax * 3) )
{
ServoValue = (uint16_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 thios 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--;
}