Rev 120 | Rev 136 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 120 | Rev 121 | ||
---|---|---|---|
Line 55... | Line 55... | ||
55 | // + POSSIBILITY OF SUCH DAMAGE. |
55 | // + POSSIBILITY OF SUCH DAMAGE. |
56 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
56 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
57 | #include "91x_lib.h" |
57 | #include "91x_lib.h" |
58 | #include "timer2.h" |
58 | #include "timer2.h" |
59 | #include "uart1.h" |
59 | #include "uart1.h" |
- | 60 | #include "spi_slave.h" |
|
Line 60... | Line 61... | ||
60 | 61 | ||
61 | #define CR1_OLVL1_MASK 0x0100 |
62 | #define CR1_OLVL1_MASK 0x0100 |
Line 62... | Line 63... | ||
62 | #define CR1_OLVL2_MASK 0x0200 |
63 | #define CR1_OLVL2_MASK 0x0200 |
Line 68... | Line 69... | ||
68 | // stop pulse: 0.3 ms = 188 * 1.6 us |
69 | // stop pulse: 0.3 ms = 188 * 1.6 us |
69 | // min servo pulse: 0.6 ms = 375 * 1.6 us |
70 | // min servo pulse: 0.6 ms = 375 * 1.6 us |
70 | // max servo pulse: 2.4 ms = 1500 * 1.6 us |
71 | // max servo pulse: 2.4 ms = 1500 * 1.6 us |
71 | // resolution: 1500 - 375 = 1125 steps |
72 | // resolution: 1500 - 375 = 1125 steps |
72 | #define PPM_STOPPULSE 188 |
73 | #define PPM_STOPPULSE 188 |
73 | #define PPM_FRAMELEN 14063 |
74 | //#define PPM_FRAMELEN 14063 |
74 | //#define PPM_FRAMELEN (1757 * ServoParams.Refresh) // 22.5 ms / 8 Channels = 2.8125ms per Servo Channel |
75 | #define PPM_FRAMELEN (1757 * ServoParams.Refresh) // 22.5 ms / 8 Channels = 2.8125ms per Servo Channel |
75 | #define MINSERVOPULSE 375 |
76 | #define MINSERVOPULSE 375 |
76 | #define MAXSERVOPULSE 1500 |
77 | #define MAXSERVOPULSE 1500 |
77 | #define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE) |
78 | #define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE) |
Line 78... | Line 79... | ||
78 | 79 | ||
79 | //---------------------------------------------------------------------------------------------------- |
80 | //---------------------------------------------------------------------------------------------------- |
80 | void TIM2_IRQHandler(void) |
81 | void TIM2_IRQHandler(void) |
81 | { |
82 | { |
- | 83 | #define MULTIPLYER 4 |
|
82 | //static s16 ServoValue1 = 0; |
84 | static s16 ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center position |
- | 85 | static s16 ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center position |
|
83 | //static s16 ServoValue2 = 0; |
86 | |
84 | static u16 LowPulseTime1 = PPM_FRAMELEN; |
87 | static u16 LowPulseTime1 = 14063; |
- | 88 | static u16 LowPulseTime2 = 14063; |
|
- | 89 | ||
- | 90 | s16 ServoNickValue = 0; |
|
Line 85... | Line 91... | ||
85 | static u16 LowPulseTime2 = PPM_FRAMELEN; |
91 | s16 ServoRollValue = 0; |
Line 86... | Line 92... | ||
86 | 92 | ||
87 | u16 pulselen; |
93 | u16 pulselen; |
88 | 94 | ||
89 | if(TIM_GetFlagStatus(TIM2, TIM_FLAG_OC1) == SET) |
95 | if(TIM_GetFlagStatus(TIM2, TIM_FLAG_OC1) == SET) |
90 | { |
96 | { |
- | 97 | if (TIM2->CR1 & CR1_OLVL1_MASK) // start of high pulse |
|
- | 98 | { |
|
- | 99 | pulselen = MINSERVOPULSE + SERVORANGE/2; |
|
91 | if (TIM2->CR1 & CR1_OLVL1_MASK) // start of high pulse |
100 | ServoNickOffset = (ServoNickOffset * 3 + (s16)ServoParams.NickControl * MULTIPLYER) / 4; // lowpass offset |
- | 101 | ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
|
- | 102 | if(ServoParams.CompInvert & 0x01) |
|
- | 103 | { // inverting movement of servo FromFlightCtrl.AngleNick |
|
- | 104 | ServoNickValue += (s16)( ( (s32)ServoParams.NickComp * MULTIPLYER * (FromFlightCtrl.AngleNick) ) / (256L) ); |
|
- | 105 | } |
|
- | 106 | else |
|
- | 107 | { // non inverting movement of servo FromFlightCtrl.AngleNick |
|
- | 108 | ServoNickValue -= (s16)( ( (s32)ServoParams.NickComp * MULTIPLYER * (FromFlightCtrl.AngleNick) ) / (256L) ); |
|
- | 109 | } |
|
- | 110 | // limit servo value to its parameter range definition |
|
- | 111 | if(ServoNickValue < ((s16)ServoParams.NickMin * MULTIPLYER) ) |
|
- | 112 | { |
|
- | 113 | ServoNickValue = (s16)ServoParams.NickMin * MULTIPLYER; |
|
- | 114 | } |
|
- | 115 | else |
|
- | 116 | if(ServoNickValue > ((s16)ServoParams.NickMax * MULTIPLYER) ) |
|
- | 117 | { |
|
- | 118 | ServoNickValue = (s16)ServoParams.NickMax * MULTIPLYER; |
|
- | 119 | } |
|
92 | { |
120 | |
93 | pulselen = MINSERVOPULSE + SERVORANGE/2; |
121 | pulselen += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
94 | // input code for nick servo here |
122 | DebugOut.Analog[7] = ServoNickValue / MULTIPLYER; |
95 | LowPulseTime1 = PPM_FRAMELEN - pulselen; |
123 | LowPulseTime1 = PPM_FRAMELEN - pulselen; |
96 | TIM2->CR1 &= ~CR1_OLVL1_MASK; // make next a low pulse |
124 | TIM2->CR1 &= ~CR1_OLVL1_MASK; // make next a low pulse |
Line 107... | Line 135... | ||
107 | if(TIM_GetFlagStatus(TIM2, TIM_FLAG_OC2) == SET) |
135 | if(TIM_GetFlagStatus(TIM2, TIM_FLAG_OC2) == SET) |
108 | { |
136 | { |
109 | if (TIM2->CR1 & CR1_OLVL2_MASK) // was high pulse |
137 | if (TIM2->CR1 & CR1_OLVL2_MASK) // was high pulse |
110 | { |
138 | { |
111 | pulselen = MINSERVOPULSE + SERVORANGE/2; |
139 | pulselen = MINSERVOPULSE + SERVORANGE/2; |
- | 140 | ServoRollOffset = (ServoRollOffset * 3 + (s16)ServoParams.RollControl * MULTIPLYER) / 4; // lowpass offset |
|
- | 141 | ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765) |
|
- | 142 | if(ServoParams.CompInvert & 0x02) |
|
112 | // input code for roll servo here |
143 | { // inverting movement of servo FromFlightCtrl.AngleRoll |
- | 144 | ServoRollValue += (s16)( ( (s32)ServoParams.RollComp * MULTIPLYER * (FromFlightCtrl.AngleRoll) ) / (256L) ); |
|
- | 145 | } |
|
- | 146 | else |
|
- | 147 | { // non inverting movement of servo FromFlightCtrl.AngleRoll |
|
- | 148 | ServoRollValue -= (s16)( ( (s32)ServoParams.RollComp * MULTIPLYER * (FromFlightCtrl.AngleRoll) ) / (256L) ); |
|
- | 149 | } |
|
- | 150 | // limit servo value to its parameter range definition |
|
- | 151 | if(ServoRollValue < ((s16)ServoParams.RollMin * MULTIPLYER) ) |
|
- | 152 | { |
|
- | 153 | ServoRollValue = (s16)ServoParams.RollMin * MULTIPLYER; |
|
- | 154 | } |
|
- | 155 | else |
|
- | 156 | if(ServoRollValue > ((s16)ServoParams.RollMax * MULTIPLYER) ) |
|
- | 157 | { |
|
- | 158 | ServoRollValue = (s16)ServoParams.RollMax * MULTIPLYER; |
|
- | 159 | } |
|
- | 160 | ||
- | 161 | pulselen += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
|
- | 162 | DebugOut.Analog[8] = ServoRollValue / MULTIPLYER; |
|
113 | LowPulseTime2 = PPM_FRAMELEN - pulselen; |
163 | LowPulseTime2 = PPM_FRAMELEN - pulselen; |
114 | TIM2->CR1 &= ~CR1_OLVL2_MASK; // make next a low pulse |
164 | TIM2->CR1 &= ~CR1_OLVL2_MASK; // make next a low pulse |
115 | } |
165 | } |
116 | else |
166 | else |
117 | { |
167 | { |
Line 174... | Line 224... | ||
174 | 224 | ||
175 | VIC_Config(TIM2_ITLine, VIC_IRQ, 3); |
225 | VIC_Config(TIM2_ITLine, VIC_IRQ, 3); |
Line 176... | Line 226... | ||
176 | VIC_ITCmd(TIM2_ITLine, ENABLE); |
226 | VIC_ITCmd(TIM2_ITLine, ENABLE); |
177 | 227 | ||
- | 228 | // set servo params to defaults |
|
178 | // set servo params to defaults |
229 | ServoParams.Refresh = 5; |
179 | ServoParams.Refresh = 5; |
230 | ServoParams.CompInvert = 0; |
180 | ServoParams.NickOffset = 127; |
231 | ServoParams.NickControl = 127; |
181 | ServoParams.NickComp = 40; |
232 | ServoParams.NickComp = 40; |
182 | ServoParams.NickMin = 50; |
233 | ServoParams.NickMin = 50; |
183 | ServoParams.NickMax = 205; |
234 | ServoParams.NickMax = 205; |
184 | ServoParams.RollOffset = 127; |
235 | ServoParams.RollControl = 127; |
185 | ServoParams.RollComp = 40; |
236 | ServoParams.RollComp = 40; |
Line 186... | Line 237... | ||
186 | ServoParams.RollMin = 50; |
237 | ServoParams.RollMin = 50; |