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 | } |