| 64,6 → 64,7 |
|---|
| uint16_t RemainingPulse = 0; |
| volatile int16_t ServoNickOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
| volatile int16_t ServoRollOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
| volatile int16_t ServoPanOffset = (255 / 2) * MULTIPLYER * 16; // MartinR: für Pan-Funktion |
| |
| unsigned int BeepMuster = 0xffff; |
| signed int NickServoValue = 128 * MULTIPLYER * 16; |
| 70,6 → 71,7 |
|---|
| |
| volatile int16_t ServoNickValue = 0; |
| volatile int16_t ServoRollValue = 0; |
| volatile int16_t ServoPanValue = 0; // MartinR : für PAN-Funktion |
| |
| |
| enum { |
| 263,16 → 265,36 |
|---|
| |
| void CalculateServo(void) |
| { |
| signed char cosinus, sinus; |
| //signed char cosinus, sinus; // MartinR : so war es |
| extern signed char cosinus, sinus; // MartinR : extern für PAN-Funktion |
| signed long nick, roll; |
| |
| cosinus = sintab[EE_Parameter.CamOrientation + 6]; |
| sinus = sintab[EE_Parameter.CamOrientation]; |
| nick = 0; // MartinR : StartWert bei abgeschalteten Nick/ Roll ausgleich |
| roll = 0; // MartinR : StartWert bei abgeschalteten Nick/ Roll ausgleich |
| int tmp; // MartinR : für PAN-Funktion // Wert : 0-24 -> 0-360 -> 15° steps |
| /* // MartinR: bisher |
| tmp = EE_Parameter.CamOrientation + ((Parameter_Servo4 - 125) * (Parameter_UserParam8 - 125)) / 400 ; //MartinR : für PAN-Funktion |
| if (tmp < 0) tmp = 24- (abs(tmp)) % 24 ; // MartinR :Modulo 24 |
| else tmp = tmp % 24 ; // MartinR :Modulo 24 |
| */ |
| tmp = EE_Parameter.CamOrientation + ((Parameter_Servo4 - 125) * (Parameter_UserParam8 - 125)) / 200 ; //MartinR : für PAN-Funktion |
| if (tmp < 0) tmp = 48- (abs(tmp)) % 48 ; // MartinR :Modulo 48 |
| else tmp = tmp % 48 ; // MartinR :Modulo 48 |
| |
| // cosinus = sintab[EE_Parameter.CamOrientation + 6]; // MartinR : so war es |
| // sinus = sintab[EE_Parameter.CamOrientation]; // MartinR : so war es |
| //cosinus = sintab[tmp + 6]; // MartinR : für PAN-Funktion |
| cosinus += (2*sintab[tmp + 12]- cosinus + 1) / 2; // MartinR : für PAN-Funktion |
| sinus += (2*sintab[tmp] - sinus + 1) / 2; // MartinR : für PAN-Funktion |
| |
| if(CalculateServoSignals == 1) |
| { |
| nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
| if (Parameter_UserParam7 < 50) // MartinR: um per UserParameter den Nickausgleich abzuschalten |
| { |
| //nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; // MartinR: so war es |
| nick = (cosinus * IntegralNick) / 512L - (sinus * IntegralRoll) / 512L; // MartinR: bessere Auflösung |
| nick -= POI_KameraNick * 7; |
| } |
| nick = ((long)Parameter_ServoNickComp * nick) / 512L; |
| // offset (Range from 0 to 255 * 3 = 765) |
| if(EE_Parameter.ServoCompInvert & SERVO_RELATIVE) ServoNickOffset = NickServoValue; |
| 302,7 → 324,11 |
|---|
| } |
| else |
| { |
| roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
| if (Parameter_UserParam7 < 100) // MartinR: um per UserParameter den Nickausgleich abzuschalten |
| { |
| //roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; // MartinR: so war es |
| roll = (cosinus * IntegralRoll) / 512L + (sinus * IntegralNick) / 512L; // MartinR: bessere Auflösung |
| } |
| roll = ((long)Parameter_ServoRollComp * roll) / 512L; |
| ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
| if(EE_Parameter.ServoCompInvert & SERVO_ROLL_INV) |
| 325,6 → 351,11 |
|---|
| { |
| ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER; |
| } |
| |
| // MartinR: Filterung der Pan- Funktion |
| ServoPanOffset += ((int16_t)Parameter_Servo4 * (MULTIPLYER*16) - ServoPanOffset) / EE_Parameter.ServoManualControlSpeed; |
| ServoPanValue = (int16_t)ServoPanOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
| |
| CalculateServoSignals = 0; |
| } |
| } |
| 413,7 → 444,8 |
|---|
| RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
| break; |
| case 4: |
| RemainingPulse += ((int16_t)Parameter_Servo4 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
| //RemainingPulse += ((int16_t)Parameter_Servo4 * MULTIPLYER) - (256 / 2) * MULTIPLYER; // MartinR: so war es |
| RemainingPulse += ServoPanValue - (256 / 2) * MULTIPLYER; // MartinR: zur Filterung der Pan-Funktion |
| break; |
| case 5: |
| RemainingPulse += ((int16_t)Parameter_Servo5 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |