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