Rev 1880 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1880 | Rev 1888 | ||
|---|---|---|---|
| Line 192... | Line 192... | ||
| 192 | void CalculateServo(void) |
192 | void CalculateServo(void) |
| 193 | { |
193 | { |
| 194 | signed char cosinus, sinus; |
194 | signed char cosinus, sinus; |
| 195 | signed long nick, roll; |
195 | signed long nick, roll; |
| Line -... | Line 196... | ||
| - | 196 | ||
| - | 197 | nick = 0; // MartinR : StartWert bei abgeschalteten Nick/ Roll ausgleich |
|
| - | 198 | roll = 0; // MartinR : StartWert bei abgeschalteten Nick/ Roll ausgleich |
|
| 196 | 199 | ||
| 197 | cosinus = sintab[EE_Parameter.CamOrientation + 6]; |
200 | cosinus = sintab[EE_Parameter.CamOrientation + 6]; |
| Line 198... | Line 201... | ||
| 198 | sinus = sintab[EE_Parameter.CamOrientation]; |
201 | sinus = sintab[EE_Parameter.CamOrientation]; |
| 199 | 202 | ||
| - | 203 | if(CalculateServoSignals == 1) |
|
| - | 204 | { |
|
| 200 | if(CalculateServoSignals == 1) |
205 | if (Parameter_UserParam7 < 50) // MartinR: um per UserParameter den Nickausgleich abzuschalten |
| 201 | { |
206 | { |
| - | 207 | nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
|
| 202 | nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
208 | nick -= POI_KameraNick * 7; |
| 203 | nick -= POI_KameraNick * 7; |
209 | } |
| 204 | nick = ((long)EE_Parameter.ServoNickComp * nick) / 512L; |
210 | nick = ((long)EE_Parameter.ServoNickComp * nick) / 512L; |
| Line 205... | Line 211... | ||
| 205 | ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
211 | ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
| Line 225... | Line 231... | ||
| 225 | } |
231 | } |
| 226 | if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++; |
232 | if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++; |
| 227 | } |
233 | } |
| 228 | else |
234 | else |
| 229 | { |
235 | { |
| - | 236 | if (Parameter_UserParam7 < 100) // MartinR: um per UserParameter den Nickausgleich abzuschalten |
|
| - | 237 | { |
|
| 230 | roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
238 | roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
| - | 239 | } |
|
| 231 | roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L; |
240 | roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L; |
| 232 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
241 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
| 233 | ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
242 | ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
| 234 | if(EE_Parameter.ServoCompInvert & 0x02) |
243 | if(EE_Parameter.ServoCompInvert & 0x02) |
| 235 | { // inverting movement of servo |
244 | { // inverting movement of servo |