Rev 1762 | Rev 1771 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1762 | Rev 1763 | ||
---|---|---|---|
Line 198... | Line 198... | ||
198 | sinus = sintab[EE_Parameter.CamOrientation]; |
198 | sinus = sintab[EE_Parameter.CamOrientation]; |
Line 199... | Line 199... | ||
199 | 199 | ||
200 | if(CalculateServoSignals == 1) |
200 | if(CalculateServoSignals == 1) |
201 | { |
201 | { |
202 | nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
202 | nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
203 | nick = (EE_Parameter.ServoNickComp * nick) / 512L; |
203 | nick = ((long)EE_Parameter.ServoNickComp * nick) / 512L; |
204 | ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
204 | ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
205 | ServoNickValue = ServoNickOffset / 16; // offset (Range from 0 to 255 * 3 = 765) |
205 | ServoNickValue = ServoNickOffset / 16; // offset (Range from 0 to 255 * 3 = 765) |
206 | if(EE_Parameter.ServoCompInvert & 0x01) |
206 | if(EE_Parameter.ServoCompInvert & 0x01) |
207 | { // inverting movement of servo |
207 | { // inverting movement of servo |
Line 224... | Line 224... | ||
224 | if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++; |
224 | if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++; |
225 | } |
225 | } |
226 | else |
226 | else |
227 | { |
227 | { |
228 | roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
228 | roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
229 | roll = (EE_Parameter.ServoRollComp * roll) / 512L; |
229 | roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L; |
230 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
230 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
231 | ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
231 | ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
232 | if(EE_Parameter.ServoCompInvert & 0x02) |
232 | if(EE_Parameter.ServoCompInvert & 0x02) |
233 | { // inverting movement of servo |
233 | { // inverting movement of servo |
234 | ServoRollValue += roll; |
234 | ServoRollValue += roll; |
Line 291... | Line 291... | ||
291 | } |
291 | } |
292 | else // we had a high pulse |
292 | else // we had a high pulse |
293 | { |
293 | { |
294 | TCCR2A |= (1<<COM2A0); // make a low pulse |
294 | TCCR2A |= (1<<COM2A0); // make a low pulse |
295 | RemainingPulse = PPM_FRAMELEN - ServoFrameTime; |
295 | RemainingPulse = PPM_FRAMELEN - ServoFrameTime; |
- | 296 | CalculateServoSignals = 1; |
|
296 | } |
297 | } |
297 | // set pulse output active |
298 | // set pulse output active |
298 | PulseOutput = 1; |
299 | PulseOutput = 1; |
299 | } |
300 | } |
300 | } // EOF Nick servo state machine |
301 | } // EOF Nick servo state machine |