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--; |
} |
|