Rev 2176 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2176 | Rev 2177 | ||
|---|---|---|---|
| Line 58... | Line 58... | ||
| 58 | volatile static unsigned int tim_main; |
58 | volatile static unsigned int tim_main; |
| 59 | volatile unsigned char UpdateMotor = 0; |
59 | volatile unsigned char UpdateMotor = 0; |
| 60 | volatile unsigned int cntKompass = 0; |
60 | volatile unsigned int cntKompass = 0; |
| 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 = 255; // MartinR zur Sicherheit bei Interrupts |
| 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 390... | Line 421... | ||
| 390 | break; |
421 | break; |
| 391 | case 3: |
422 | case 3: |
| 392 | RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
423 | RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
| 393 | break; |
424 | break; |
| 394 | case 4: |
425 | case 4: |
| 395 | RemainingPulse += ((int16_t)Parameter_Servo4 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
426 | //RemainingPulse += ((int16_t)Parameter_Servo4 * MULTIPLYER) - (256 / 2) * MULTIPLYER; // MartinR: so war es |
| - | 427 | RemainingPulse += ServoPanValue - (256 / 2) * MULTIPLYER; // MartinR: zur Filterung der Pan-Funktion |
|
| 396 | break; |
428 | break; |
| 397 | case 5: |
429 | case 5: |
| 398 | RemainingPulse += ((int16_t)Parameter_Servo5 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
430 | RemainingPulse += ((int16_t)Parameter_Servo5 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
| 399 | break; |
431 | break; |
| 400 | default: // other servo channels |
432 | default: // other servo channels |