Rev 1226 | Rev 1242 | Go to most recent revision | Show entire file | Regard 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 |