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 |