Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1077 → Rev 1078

/branches/V0.71h Code Redesign killagreg/timer2.c
3,9 → 3,12
#include "fc.h"
#include "eeprom.h"
#include "uart.h"
#include "main.h"
 
volatile uint16_t ServoValue = 0;
 
#define HEF4017R_ON PORTC |= (1<<PORTC6)
#define HEF4017R_OFF PORTC &= ~(1<<PORTC6)
 
 
/*****************************************************/
21,9 → 24,11
cli();
 
// set PD7 as output of the PWM for nick servo
DDRD |=(1<<DDD7);
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
 
70,83 → 75,88
 
#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 += ((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);
}
 
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
// 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;
}
 
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);
}
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);
 
// 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;
}
break;
 
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);
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 3:
case 2:
case 1:
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;
 
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;
default:
// do nothing
break;
}
ServoState--;
} // EOF BoardRelease < 20
else // output to HEF4014
{
// code for HEF4014 output must be placed here
}
ServoState--;
}