Rev 1738 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1738 | Rev 1757 | ||
---|---|---|---|
Line 222... | Line 222... | ||
222 | TCCR2A &= ~(1<<COM2A0);// make a high pulse |
222 | TCCR2A &= ~(1<<COM2A0);// make a high pulse |
223 | RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
223 | RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
Line 224... | Line 224... | ||
224 | 224 | ||
225 | ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
225 | ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
- | 226 | ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
|
- | 227 | //if (Parameter_UserParam7 < 50) // MartinR: um per UserParameter den Nickausgleich abzuschalten |
|
226 | ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
228 | //{ |
227 | if(EE_Parameter.ServoCompInvert & 0x01) |
229 | if(EE_Parameter.ServoCompInvert & 0x01) |
228 | { // inverting movement of servo |
230 | { // inverting movement of servo |
229 | ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
231 | ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
230 | } |
232 | } |
231 | else |
233 | else |
232 | { // non inverting movement of servo |
234 | { // non inverting movement of servo |
233 | ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
235 | ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
- | 236 | } |
|
234 | } |
237 | //} |
235 | // limit servo value to its parameter range definition |
238 | // limit servo value to its parameter range definition |
236 | if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
239 | if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
237 | { |
240 | { |
238 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
241 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
Line 285... | Line 288... | ||
285 | switch(ServoIndex) // map servo channels |
288 | switch(ServoIndex) // map servo channels |
286 | { |
289 | { |
287 | case 1: // Nick Compensation Servo |
290 | case 1: // Nick Compensation Servo |
288 | ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
291 | ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
289 | ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
292 | ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
- | 293 | if (Parameter_UserParam7 < 50) // MartinR: um per UserParameter den Nickausgleich abzuschalten |
|
- | 294 | { |
|
290 | if(EE_Parameter.ServoCompInvert & 0x01) |
295 | if(EE_Parameter.ServoCompInvert & 0x01) |
291 | { // inverting movement of servo |
296 | { // inverting movement of servo |
292 | ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
297 | ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
293 | } |
298 | } |
294 | else |
299 | else |
295 | { // non inverting movement of servo |
300 | { // non inverting movement of servo |
296 | ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
301 | ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
297 | } |
302 | } |
- | 303 | } |
|
298 | // limit servo value to its parameter range definition |
304 | // limit servo value to its parameter range definition |
299 | if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
305 | if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
300 | { |
306 | { |
301 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
307 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
302 | } |
308 | } |
Line 309... | Line 315... | ||
309 | ServoNickValue /= MULTIPLYER; |
315 | ServoNickValue /= MULTIPLYER; |
310 | break; |
316 | break; |
311 | case 2: // Roll Compensation Servo |
317 | case 2: // Roll Compensation Servo |
312 | ServoRollOffset = (ServoRollOffset * 3 + (int16_t) Parameter_ServoRollControl * MULTIPLYER) / 4; // lowpass offset |
318 | ServoRollOffset = (ServoRollOffset * 3 + (int16_t) Parameter_ServoRollControl * MULTIPLYER) / 4; // lowpass offset |
313 | ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765) |
319 | ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765) |
- | 320 | if (Parameter_UserParam7 < 100) // MartinR: um per UserParameter den Nickausgleich abzuschalten |
|
- | 321 | { |
|
314 | if(EE_Parameter.ServoCompInvert & 0x02) |
322 | if(EE_Parameter.ServoCompInvert & 0x02) |
315 | { // inverting movement of servo |
323 | { // inverting movement of servo |
316 | ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
324 | ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
317 | } |
325 | } |
318 | else |
326 | else |
319 | { // non inverting movement of servo |
327 | { // non inverting movement of servo |
320 | ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
328 | ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
321 | } |
329 | } |
- | 330 | } |
|
322 | // limit servo value to its parameter range definition |
331 | // limit servo value to its parameter range definition |
323 | if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) ) |
332 | if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) ) |
324 | { |
333 | { |
325 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER; |
334 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER; |
326 | } |
335 | } |