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