| 194,13 → 194,19 |
|---|
| signed char cosinus, sinus; |
| signed long nick, roll; |
| |
| nick = 0; // MartinR : StartWert bei abgeschalteten Nick/ Roll ausgleich |
| roll = 0; // MartinR : StartWert bei abgeschalteten Nick/ Roll ausgleich |
| |
| cosinus = sintab[EE_Parameter.CamOrientation + 6]; |
| sinus = sintab[EE_Parameter.CamOrientation]; |
| |
| if(CalculateServoSignals == 1) |
| { |
| if (Parameter_UserParam7 < 50) // MartinR: um per UserParameter den Nickausgleich abzuschalten |
| { |
| nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
| nick -= POI_KameraNick * 7; |
| } |
| nick = ((long)EE_Parameter.ServoNickComp * nick) / 512L; |
| ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
| ServoNickValue = ServoNickOffset / 16; // offset (Range from 0 to 255 * 3 = 765) |
| 227,7 → 233,10 |
|---|
| } |
| else |
| { |
| if (Parameter_UserParam7 < 100) // MartinR: um per UserParameter den Nickausgleich abzuschalten |
| { |
| roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
| } |
| roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L; |
| ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
| ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765) |