Rev 1984 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1984 | Rev 2013 | ||
|---|---|---|---|
| Line 8... | Line 8... | ||
| 8 | volatile unsigned int beeptime = 0; |
8 | volatile unsigned int beeptime = 0; |
| 9 | volatile unsigned char SendSPI = 0, ServoActive = 0, CalculateServoSignals = 1; |
9 | volatile unsigned char SendSPI = 0, ServoActive = 0, CalculateServoSignals = 1; |
| 10 | uint16_t RemainingPulse = 0; |
10 | uint16_t RemainingPulse = 0; |
| 11 | volatile int16_t ServoNickOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
11 | volatile int16_t ServoNickOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
| 12 | volatile int16_t ServoRollOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
12 | volatile int16_t ServoRollOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
| - | 13 | volatile int16_t ServoPanOffset = (255 / 2) * MULTIPLYER * 16; // MartinR: für Pan-Funktion |
|
| Line 13... | Line 14... | ||
| 13 | 14 | ||
| Line 14... | Line 15... | ||
| 14 | unsigned int BeepMuster = 0xffff; |
15 | unsigned int BeepMuster = 0xffff; |
| 15 | 16 | ||
| - | 17 | volatile int16_t ServoNickValue = 0; |
|
| Line 16... | Line 18... | ||
| 16 | volatile int16_t ServoNickValue = 0; |
18 | volatile int16_t ServoRollValue = 0; |
| 17 | volatile int16_t ServoRollValue = 0; |
19 | volatile int16_t ServoPanValue = 0; // MartinR : für PAN-Funktion |
| 18 | 20 | ||
| Line 189... | Line 191... | ||
| 189 | /*****************************************************/ |
191 | /*****************************************************/ |
| Line 190... | Line 192... | ||
| 190 | 192 | ||
| 191 | 193 | ||
| 192 | void CalculateServo(void) |
194 | void CalculateServo(void) |
| - | 195 | { |
|
| 193 | { |
196 | //signed char cosinus, sinus; // MartinR : so war es |
| Line -... | Line 197... | ||
| - | 197 | extern signed char cosinus, sinus; // MartinR : extern für PAN-Funktion |
|
| - | 198 | signed long nick, roll; |
|
| - | 199 | ||
| - | 200 | nick = 0; // MartinR : StartWert bei abgeschalteten Nick/ Roll ausgleich |
|
| - | 201 | roll = 0; // MartinR : StartWert bei abgeschalteten Nick/ Roll ausgleich |
|
| - | 202 | int tmp; // MartinR : für PAN-Funktion // Wert : 0-24 -> 0-360 -> 15° steps |
|
| - | 203 | /* // MartinR: bisher |
|
| - | 204 | tmp = EE_Parameter.CamOrientation + ((Parameter_Servo4 - 125) * (Parameter_UserParam8 - 125)) / 400 ; //MartinR : für PAN-Funktion |
|
| - | 205 | if (tmp < 0) tmp = 24- (abs(tmp)) % 24 ; // MartinR :Modulo 24 |
|
| - | 206 | else tmp = tmp % 24 ; // MartinR :Modulo 24 |
|
| - | 207 | */ |
|
| - | 208 | tmp = EE_Parameter.CamOrientation + ((Parameter_Servo4 - 125) * (Parameter_UserParam8 - 125)) / 200 ; //MartinR : für PAN-Funktion |
|
| 194 | signed char cosinus, sinus; |
209 | if (tmp < 0) tmp = 48- (abs(tmp)) % 48 ; // MartinR :Modulo 48 |
| 195 | signed long nick, roll; |
210 | else tmp = tmp % 48 ; // MartinR :Modulo 48 |
| - | 211 | ||
| - | 212 | // cosinus = sintab[EE_Parameter.CamOrientation + 6]; // MartinR : so war es |
|
| - | 213 | // sinus = sintab[EE_Parameter.CamOrientation]; // MartinR : so war es |
|
| Line 196... | Line 214... | ||
| 196 | 214 | //cosinus = sintab[tmp + 6]; // MartinR : für PAN-Funktion |
|
| 197 | cosinus = sintab[EE_Parameter.CamOrientation + 6]; |
215 | cosinus += (2*sintab[tmp + 12]- cosinus + 1) / 2; // MartinR : für PAN-Funktion |
| - | 216 | sinus += (2*sintab[tmp] - sinus + 1) / 2; // MartinR : für PAN-Funktion |
|
| - | 217 | ||
| 198 | sinus = sintab[EE_Parameter.CamOrientation]; |
218 | if(CalculateServoSignals == 1) |
| - | 219 | { |
|
| 199 | 220 | if (Parameter_UserParam7 < 50) // MartinR: um per UserParameter den Nickausgleich abzuschalten |
|
| - | 221 | { |
|
| 200 | if(CalculateServoSignals == 1) |
222 | //nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; // MartinR: so war es |
| 201 | { |
223 | nick = (cosinus * IntegralNick) / 512L - (sinus * IntegralRoll) / 512L; // MartinR: bessere Auflösung |
| 202 | nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
224 | nick -= POI_KameraNick * 7; |
| Line 203... | Line 225... | ||
| 203 | nick -= POI_KameraNick * 7; |
225 | } |
| Line 225... | Line 247... | ||
| 225 | } |
247 | } |
| 226 | if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++; |
248 | if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++; |
| 227 | } |
249 | } |
| 228 | else |
250 | else |
| 229 | { |
251 | { |
| - | 252 | if (Parameter_UserParam7 < 100) // MartinR: um per UserParameter den Nickausgleich abzuschalten |
|
| - | 253 | { |
|
| 230 | roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
254 | //roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; // MartinR: so war es |
| - | 255 | roll = (cosinus * IntegralRoll) / 512L + (sinus * IntegralNick) / 512L; // MartinR: bessere Auflösung |
|
| - | 256 | } |
|
| 231 | roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L; |
257 | roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L; |
| 232 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
258 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
| 233 | ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
259 | ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
| 234 | if(EE_Parameter.ServoCompInvert & 0x02) |
260 | if(EE_Parameter.ServoCompInvert & 0x02) |
| 235 | { // inverting movement of servo |
261 | { // inverting movement of servo |
| Line 247... | Line 273... | ||
| 247 | else |
273 | else |
| 248 | if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) ) |
274 | if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) ) |
| 249 | { |
275 | { |
| 250 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER; |
276 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER; |
| 251 | } |
277 | } |
| - | 278 | ||
| - | 279 | // MartinR: Filterung der Pan- Funktion |
|
| - | 280 | ServoPanOffset += ((int16_t)Parameter_Servo4 * (MULTIPLYER*16) - ServoPanOffset) / EE_Parameter.ServoManualControlSpeed; |
|
| - | 281 | ServoPanValue = (int16_t)ServoPanOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
|
| - | 282 | ||
| 252 | CalculateServoSignals = 0; |
283 | CalculateServoSignals = 0; |
| 253 | } |
284 | } |
| 254 | } |
285 | } |
| Line 255... | Line 286... | ||
| 255 | 286 | ||
| Line 273... | Line 304... | ||
| 273 | static uint8_t ServoIndex = 0; |
304 | static uint8_t ServoIndex = 0; |
| Line 274... | Line 305... | ||
| 274 | 305 | ||
| 275 | 306 | ||
| - | 307 | if(PlatinenVersion < 20) |
|
| 276 | if(PlatinenVersion < 20) |
308 | { |
| 277 | { |
309 | /* // MartinR : deaktiviert wegen Platzbedarf |
| 278 | //--------------------------- |
310 | //--------------------------- |
| 279 | // Nick servo state machine |
311 | // Nick servo state machine |
| 280 | //--------------------------- |
312 | //--------------------------- |
| Line 298... | Line 330... | ||
| 298 | CalculateServoSignals = 1; |
330 | CalculateServoSignals = 1; |
| 299 | } |
331 | } |
| 300 | // set pulse output active |
332 | // set pulse output active |
| 301 | PulseOutput = 1; |
333 | PulseOutput = 1; |
| 302 | } |
334 | } |
| - | 335 | */ |
|
| 303 | } // EOF Nick servo state machine |
336 | } // EOF Nick servo state machine |
| 304 | else |
337 | else |
| 305 | { |
338 | { |
| 306 | //----------------------------------------------------- |
339 | //----------------------------------------------------- |
| 307 | // PPM state machine, onboard demultiplexed by HEF4017 |
340 | // PPM state machine, onboard demultiplexed by HEF4017 |
| Line 331... | Line 364... | ||
| 331 | break; |
364 | break; |
| 332 | case 3: |
365 | case 3: |
| 333 | RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
366 | RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
| 334 | break; |
367 | break; |
| 335 | case 4: |
368 | case 4: |
| 336 | RemainingPulse += ((int16_t)Parameter_Servo4 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
369 | //RemainingPulse += ((int16_t)Parameter_Servo4 * MULTIPLYER) - (256 / 2) * MULTIPLYER; // MartinR: so war es |
| - | 370 | RemainingPulse += ServoPanValue - (256 / 2) * MULTIPLYER; // MartinR: zur Filterung der Pan-Funktion |
|
| 337 | break; |
371 | break; |
| 338 | case 5: |
372 | case 5: |
| 339 | RemainingPulse += ((int16_t)Parameter_Servo5 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
373 | RemainingPulse += ((int16_t)Parameter_Servo5 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
| 340 | break; |
374 | break; |
| 341 | default: // other servo channels |
375 | default: // other servo channels |