Rev 1226 | Rev 1242 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1226 | Rev 1229 | ||
---|---|---|---|
Line 216... | Line 216... | ||
216 | 216 | ||
217 | #define MULTIPLYER 4 |
217 | #define MULTIPLYER 4 |
218 | static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
218 | static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
Line 219... | Line 219... | ||
219 | static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
219 | static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
- | 220 | ||
220 | 221 | // if((PlatinenVersion < 20) |
|
221 | if(PlatinenVersion < 20) |
222 | if((PlatinenVersion < 20) && (Parameter_UserParam8 < 128 )) |
222 | { |
223 | { |
223 | //--------------------------- |
224 | //--------------------------- |
224 | // Nick servo state machine |
225 | // Nick servo state machine |
Line 320... | Line 321... | ||
320 | break; |
321 | break; |
321 | case 2: // Roll Compensation Servo |
322 | case 2: // Roll Compensation Servo |
322 | ServoRollOffset = (ServoRollOffset * 3 + (int16_t) 80 * MULTIPLYER) / 4; // lowpass offset |
323 | ServoRollOffset = (ServoRollOffset * 3 + (int16_t) 80 * MULTIPLYER) / 4; // lowpass offset |
323 | ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765) |
324 | ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765) |
324 | //if(EE_Parameter.ServoRollCompInvert & 0x01) |
325 | //if(EE_Parameter.ServoRollCompInvert & 0x01) |
- | 326 | if(Parameter_UserParam8 & 0x40) |
|
325 | { // inverting movement of servo |
327 | { // inverting movement of servo if 64 has been added to User Parameter8 |
326 | ServoRollValue += (int16_t)( ( (int32_t) 50 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
328 | ServoRollValue += (int16_t)( ( (int32_t) 50 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
327 | } |
329 | } |
328 | /* else |
330 | /**/ else |
329 | { // non inverting movement of servo |
331 | { // non inverting movement of servo |
330 | ServoRollValue -= (int16_t)( ( (int32_t) 40 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
332 | ServoRollValue -= (int16_t)( ( (int32_t) 40 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
331 | } |
333 | } |
332 | */ // limit servo value to its parameter range definition |
334 | /**/ // limit servo value to its parameter range definition |
333 | if(ServoRollValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
335 | if(ServoRollValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
334 | { |
336 | { |
335 | ServoRollValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
337 | ServoRollValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
336 | } |
338 | } |
337 | else |
339 | else |