Rev 2523 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2523 | Rev 2541 | ||
|---|---|---|---|
| Line 269... | Line 269... | ||
| 269 | signed long nick, roll; |
269 | signed long nick, roll; |
| Line 270... | Line 270... | ||
| 270 | 270 | ||
| 271 | cosinus = sintab[EE_Parameter.CamOrientation + 6]; |
271 | cosinus = sintab[EE_Parameter.CamOrientation + 6]; |
| Line -... | Line 272... | ||
| - | 272 | sinus = sintab[EE_Parameter.CamOrientation]; |
|
| 272 | sinus = sintab[EE_Parameter.CamOrientation]; |
273 | |
| 273 | 274 | ||
| 274 | if(CalculateServoSignals == 1) |
275 | if(CalculateServoSignals == 1) |
| 275 | { |
276 | { |
| 276 | if(EE_Parameter.GlobalConfig3 & CFG3_SERVO_NICK_COMP_OFF) nick = 0; |
277 | if(EE_Parameter.GlobalConfig3 & CFG3_SERVO_NICK_COMP_OFF) nick = 0; |
| Line 405... | Line 406... | ||
| 405 | RemainingPulse = 10; // end it here |
406 | RemainingPulse = 10; // end it here |
| 406 | } |
407 | } |
| 407 | else |
408 | else |
| 408 | { |
409 | { |
| 409 | RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
410 | RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
| - | 411 | if(ServoFailsafeActive && ServoIndex < 6 && EE_Parameter.ServoFS_Pos[ServoIndex-1]) RemainingPulse += ((int16_t)EE_Parameter.ServoFS_Pos[ServoIndex-1] * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
|
| - | 412 | else |
|
| 410 | switch(ServoIndex) // map servo channels |
413 | switch(ServoIndex) // map servo channels |
| 411 | { |
414 | { |
| 412 | case 1: // Nick Compensation Servo |
415 | case 1: // Nick Compensation Servo |
| 413 | RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
416 | RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; |
| 414 | break; |
417 | break; |
| 415 | case 2: // Roll Compensation Servo |
418 | case 2: // Roll Compensation Servo |
| 416 | RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
419 | RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; |
| 417 | break; |
420 | break; |
| 418 | case 3: |
421 | case 3: |
| 419 | RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
422 | RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
| 420 | break; |
423 | break; |
| 421 | case 4: |
424 | case 4: |
| 422 | RemainingPulse += ((int16_t)Parameter_Servo4 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
425 | RemainingPulse += ((int16_t)Parameter_Servo4 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
| 423 | break; |
426 | break; |
| 424 | case 5: |
427 | case 5: |
| 425 | RemainingPulse += ((int16_t)Parameter_Servo5 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
428 | RemainingPulse += ((int16_t)Parameter_Servo5 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
| 426 | break; |
429 | break; |
| 427 | default: // other servo channels |
430 | default: // other servo channels |
| 428 | RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs |
431 | RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs |
| 429 | break; |
432 | break; |
| 430 | } |
433 | } |
| 431 | // range servo pulse width |
434 | // range servo pulse width |
| 432 | if(RemainingPulse > MAXSERVOPULSE) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
435 | if(RemainingPulse > MAXSERVOPULSE) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
| Line 442... | Line 445... | ||
| 442 | TCCR2A |= (1<<COM2A0); // make a low pulse |
445 | TCCR2A |= (1<<COM2A0); // make a low pulse |
| 443 | // set pulsewidth to stop pulse width |
446 | // set pulsewidth to stop pulse width |
| 444 | RemainingPulse = PPM_STOPPULSE; |
447 | RemainingPulse = PPM_STOPPULSE; |
| 445 | // accumulate time for correct sync gap |
448 | // accumulate time for correct sync gap |
| 446 | ServoFrameTime += RemainingPulse; |
449 | ServoFrameTime += RemainingPulse; |
| 447 | if((ServoActive && SenderOkay) || ServoActive == 2) HEF4017Reset_OFF; // disable HEF4017 reset |
450 | if((ServoActive/* && SenderOkay*/) || ServoActive == 2) HEF4017Reset_OFF; // disable HEF4017 reset |
| 448 | else HEF4017Reset_ON; |
451 | else HEF4017Reset_ON; |
| 449 | ServoIndex++; |
452 | ServoIndex++; |
| 450 | if(ServoIndex > EE_Parameter.ServoNickRefresh+1) |
453 | if(ServoIndex > EE_Parameter.ServoNickRefresh+1) |
| 451 | { |
454 | { |
| 452 | CalculateServoSignals = 1; |
455 | CalculateServoSignals = 1; |