Rev 2081 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2081 | Rev 2093 | ||
|---|---|---|---|
| Line 61... | Line 61... | ||
| 61 | volatile unsigned int beeptime = 0; |
61 | volatile unsigned int beeptime = 0; |
| 62 | volatile unsigned char SendSPI = 0, ServoActive = 0, CalculateServoSignals = 1; |
62 | volatile unsigned char SendSPI = 0, ServoActive = 0, CalculateServoSignals = 1; |
| 63 | uint16_t RemainingPulse = 0; |
63 | uint16_t RemainingPulse = 0; |
| 64 | volatile int16_t ServoNickOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
64 | volatile int16_t ServoNickOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
| 65 | volatile int16_t ServoRollOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
65 | volatile int16_t ServoRollOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
| - | 66 | volatile int16_t ServoPanOffset = (255 / 2) * MULTIPLYER * 16; // MartinR: für Pan-Funktion |
|
| Line 66... | Line 67... | ||
| 66 | 67 | ||
| Line 67... | Line 68... | ||
| 67 | unsigned int BeepMuster = 0xffff; |
68 | unsigned int BeepMuster = 0xffff; |
| 68 | 69 | ||
| - | 70 | volatile int16_t ServoNickValue = 0; |
|
| Line 69... | Line 71... | ||
| 69 | volatile int16_t ServoNickValue = 0; |
71 | volatile int16_t ServoRollValue = 0; |
| 70 | volatile int16_t ServoRollValue = 0; |
72 | volatile int16_t ServoPanValue = 0; // MartinR : für PAN-Funktion |
| 71 | 73 | ||
| Line 242... | Line 244... | ||
| 242 | /*****************************************************/ |
244 | /*****************************************************/ |
| Line 243... | Line 245... | ||
| 243 | 245 | ||
| 244 | 246 | ||
| 245 | void CalculateServo(void) |
247 | void CalculateServo(void) |
| - | 248 | { |
|
| 246 | { |
249 | //signed char cosinus, sinus; // MartinR : so war es |
| Line -... | Line 250... | ||
| - | 250 | extern signed char cosinus, sinus; // MartinR : extern für PAN-Funktion |
|
| - | 251 | signed long nick, roll; |
|
| - | 252 | ||
| - | 253 | nick = 0; // MartinR : StartWert bei abgeschalteten Nick/ Roll ausgleich |
|
| - | 254 | roll = 0; // MartinR : StartWert bei abgeschalteten Nick/ Roll ausgleich |
|
| - | 255 | int tmp; // MartinR : für PAN-Funktion // Wert : 0-24 -> 0-360 -> 15° steps |
|
| - | 256 | /* // MartinR: bisher |
|
| - | 257 | tmp = EE_Parameter.CamOrientation + ((Parameter_Servo4 - 125) * (Parameter_UserParam8 - 125)) / 400 ; //MartinR : für PAN-Funktion |
|
| - | 258 | if (tmp < 0) tmp = 24- (abs(tmp)) % 24 ; // MartinR :Modulo 24 |
|
| - | 259 | else tmp = tmp % 24 ; // MartinR :Modulo 24 |
|
| - | 260 | */ |
|
| - | 261 | tmp = EE_Parameter.CamOrientation + ((Parameter_Servo4 - 125) * (Parameter_UserParam8 - 125)) / 200 ; //MartinR : für PAN-Funktion |
|
| 247 | signed char cosinus, sinus; |
262 | if (tmp < 0) tmp = 48- (abs(tmp)) % 48 ; // MartinR :Modulo 48 |
| 248 | signed long nick, roll; |
263 | else tmp = tmp % 48 ; // MartinR :Modulo 48 |
| - | 264 | ||
| - | 265 | // cosinus = sintab[EE_Parameter.CamOrientation + 6]; // MartinR : so war es |
|
| - | 266 | // sinus = sintab[EE_Parameter.CamOrientation]; // MartinR : so war es |
|
| Line 249... | Line 267... | ||
| 249 | 267 | //cosinus = sintab[tmp + 6]; // MartinR : für PAN-Funktion |
|
| 250 | cosinus = sintab[EE_Parameter.CamOrientation + 6]; |
268 | cosinus += (2*sintab[tmp + 12]- cosinus + 1) / 2; // MartinR : für PAN-Funktion |
| - | 269 | sinus += (2*sintab[tmp] - sinus + 1) / 2; // MartinR : für PAN-Funktion |
|
| - | 270 | ||
| 251 | sinus = sintab[EE_Parameter.CamOrientation]; |
271 | if(CalculateServoSignals == 1) |
| - | 272 | { |
|
| 252 | 273 | if (Parameter_UserParam7 < 50) // MartinR: um per UserParameter den Nickausgleich abzuschalten |
|
| - | 274 | { |
|
| 253 | if(CalculateServoSignals == 1) |
275 | //nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; // MartinR: so war es |
| 254 | { |
276 | nick = (cosinus * IntegralNick) / 512L - (sinus * IntegralRoll) / 512L; // MartinR: bessere Auflösung |
| 255 | nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
277 | nick -= POI_KameraNick * 7; |
| 256 | nick -= POI_KameraNick * 7; |
278 | } |
| 257 | nick = ((long)EE_Parameter.ServoNickComp * nick) / 512L; |
279 | nick = ((long)EE_Parameter.ServoNickComp * nick) / 512L; |
| Line 279... | Line 301... | ||
| 279 | } |
301 | } |
| 280 | if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++; |
302 | if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++; |
| 281 | } |
303 | } |
| 282 | else |
304 | else |
| 283 | { |
305 | { |
| - | 306 | if (Parameter_UserParam7 < 100) // MartinR: um per UserParameter den Nickausgleich abzuschalten |
|
| - | 307 | { |
|
| 284 | roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
308 | //roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; // MartinR: so war es |
| - | 309 | roll = (cosinus * IntegralRoll) / 512L + (sinus * IntegralNick) / 512L; // MartinR: bessere Auflösung |
|
| - | 310 | } |
|
| 285 | roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L; |
311 | roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L; |
| 286 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
312 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
| 287 | if(EE_Parameter.ServoCompInvert & 0x02) |
313 | if(EE_Parameter.ServoCompInvert & 0x02) |
| 288 | { // inverting movement of servo |
314 | { // inverting movement of servo |
| 289 | roll = ServoRollOffset / 16 + roll; |
315 | roll = ServoRollOffset / 16 + roll; |
| Line 302... | Line 328... | ||
| 302 | else |
328 | else |
| 303 | if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER)) |
329 | if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER)) |
| 304 | { |
330 | { |
| 305 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER; |
331 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER; |
| 306 | } |
332 | } |
| - | 333 | ||
| - | 334 | // MartinR: Filterung der Pan- Funktion |
|
| - | 335 | ServoPanOffset += ((int16_t)Parameter_Servo4 * (MULTIPLYER*16) - ServoPanOffset) / EE_Parameter.ServoManualControlSpeed; |
|
| - | 336 | ServoPanValue = (int16_t)ServoPanOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
|
| - | 337 | ||
| 307 | CalculateServoSignals = 0; |
338 | CalculateServoSignals = 0; |
| 308 | } |
339 | } |
| 309 | } |
340 | } |
| Line 310... | Line 341... | ||
| 310 | 341 | ||
| Line 328... | Line 359... | ||
| 328 | static uint8_t ServoIndex = 0; |
359 | static uint8_t ServoIndex = 0; |
| Line 329... | Line 360... | ||
| 329 | 360 | ||
| 330 | 361 | ||
| - | 362 | if(PlatinenVersion < 20) |
|
| 331 | if(PlatinenVersion < 20) |
363 | { |
| 332 | { |
364 | /* // MartinR : deaktiviert wegen Platzbedarf |
| 333 | //--------------------------- |
365 | //--------------------------- |
| 334 | // Nick servo state machine |
366 | // Nick servo state machine |
| 335 | //--------------------------- |
367 | //--------------------------- |
| Line 353... | Line 385... | ||
| 353 | CalculateServoSignals = 1; |
385 | CalculateServoSignals = 1; |
| 354 | } |
386 | } |
| 355 | // set pulse output active |
387 | // set pulse output active |
| 356 | PulseOutput = 1; |
388 | PulseOutput = 1; |
| 357 | } |
389 | } |
| - | 390 | */ |
|
| 358 | } // EOF Nick servo state machine |
391 | } // EOF Nick servo state machine |
| 359 | else |
392 | else |
| 360 | { |
393 | { |
| 361 | //----------------------------------------------------- |
394 | //----------------------------------------------------- |
| 362 | // PPM state machine, onboard demultiplexed by HEF4017 |
395 | // PPM state machine, onboard demultiplexed by HEF4017 |
| Line 386... | Line 419... | ||
| 386 | break; |
419 | break; |
| 387 | case 3: |
420 | case 3: |
| 388 | RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
421 | RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
| 389 | break; |
422 | break; |
| 390 | case 4: |
423 | case 4: |
| 391 | RemainingPulse += ((int16_t)Parameter_Servo4 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
424 | //RemainingPulse += ((int16_t)Parameter_Servo4 * MULTIPLYER) - (256 / 2) * MULTIPLYER; // MartinR: so war es |
| - | 425 | RemainingPulse += ServoPanValue - (256 / 2) * MULTIPLYER; // MartinR: zur Filterung der Pan-Funktion |
|
| 392 | break; |
426 | break; |
| 393 | case 5: |
427 | case 5: |
| 394 | RemainingPulse += ((int16_t)Parameter_Servo5 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
428 | RemainingPulse += ((int16_t)Parameter_Servo5 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
| 395 | break; |
429 | break; |
| 396 | default: // other servo channels |
430 | default: // other servo channels |