Rev 685 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 685 | Rev 687 | ||
|---|---|---|---|
| Line 1... | Line 1... | ||
| 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" |
|
| Line 4... | Line 5... | ||
| 4 | 5 | ||
| Line 5... | Line 6... | ||
| 5 | volatile int16_t ServoValue = 0; |
6 | volatile int16_t ServoValue = 0; |
| Line 67... | Line 68... | ||
| 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); |
| Line 69... | Line 70... | ||
| 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; |
| Line 79... | Line 80... | ||
| 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 | { |
| Line 88... | Line 89... | ||
| 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 |