Rev 2240 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2240 | Rev 2296 | ||
|---|---|---|---|
| Line 265... | Line 265... | ||
| 265 | 265 | ||
| 266 | if(CalculateServoSignals == 1) |
266 | if(CalculateServoSignals == 1) |
| 267 | { |
267 | { |
| 268 | nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
268 | nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
| 269 | nick -= POI_KameraNick * 7; |
269 | nick -= POI_KameraNick * 7; |
| 270 | nick = ((long)EE_Parameter.ServoNickComp * nick) / 512L; |
270 | nick = ((long)Parameter_ServoNickComp * nick) / 512L; |
| 271 | // offset (Range from 0 to 255 * 3 = 765) |
271 | // offset (Range from 0 to 255 * 3 = 765) |
| 272 | if(EE_Parameter.ServoCompInvert & SERVO_RELATIVE) ServoNickOffset = NickServoValue; |
272 | if(EE_Parameter.ServoCompInvert & SERVO_RELATIVE) ServoNickOffset = NickServoValue; |
| Line 273... | Line 273... | ||
| 273 | else ServoNickOffset += (NickServoValue - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
273 | else ServoNickOffset += (NickServoValue - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
| Line 295... | Line 295... | ||
| 295 | if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++; |
295 | if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++; |
| 296 | } |
296 | } |
| 297 | else |
297 | else |
| 298 | { |
298 | { |
| 299 | roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
299 | roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
| 300 | roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L; |
300 | roll = ((long)Parameter_ServoRollComp * roll) / 512L; |
| 301 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
301 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
| 302 | if(EE_Parameter.ServoCompInvert & SERVO_ROLL_INV) |
302 | if(EE_Parameter.ServoCompInvert & SERVO_ROLL_INV) |
| 303 | { // inverting movement of servo |
303 | { // inverting movement of servo |
| 304 | roll = ServoRollOffset / 16 + roll; |
304 | roll = ServoRollOffset / 16 + roll; |
| 305 | } |
305 | } |