Rev 1171 | Rev 1226 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1171 | Rev 1224 | ||
|---|---|---|---|
| Line 214... | Line 214... | ||
| 214 | static uint16_t ServoFrameTime = 0; |
214 | static uint16_t ServoFrameTime = 0; |
| 215 | static uint8_t ServoIndex = 0; |
215 | static uint8_t ServoIndex = 0; |
| 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 |
|
| Line 218... | Line 219... | ||
| 218 | static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
219 | static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
| 219 | 220 | ||
| 220 | if(PlatinenVersion < 20) |
221 | if(PlatinenVersion < 20) |
| 221 | { |
222 | { |
| Line 315... | Line 316... | ||
| 315 | } |
316 | } |
| 316 | RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
317 | RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
| 317 | ServoNickValue /= MULTIPLYER; |
318 | ServoNickValue /= MULTIPLYER; |
| 318 | DebugOut.Analog[20] = ServoNickValue; |
319 | DebugOut.Analog[20] = ServoNickValue; |
| 319 | break; |
320 | break; |
| - | 321 | case 2: // Roll Compensation Servo |
|
| - | 322 | ServoRollOffset = (ServoRollOffset * 3 + (int16_t) 80 * MULTIPLYER) / 4; // lowpass offset |
|
| - | 323 | ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765) |
|
| - | 324 | //if(EE_Parameter.ServoRollCompInvert & 0x01) |
|
| - | 325 | { // inverting movement of servo |
|
| - | 326 | ServoRollValue += (int16_t)( ( (int32_t) 50 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
|
| - | 327 | } |
|
| - | 328 | /* else |
|
| - | 329 | { // non inverting movement of servo |
|
| - | 330 | ServoRollValue -= (int16_t)( ( (int32_t) 40 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
|
| - | 331 | } |
|
| - | 332 | */ // limit servo value to its parameter range definition |
|
| - | 333 | if(ServoRollValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
|
| - | 334 | { |
|
| - | 335 | ServoRollValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
|
| - | 336 | } |
|
| - | 337 | else |
|
| - | 338 | if(ServoRollValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) ) |
|
| - | 339 | { |
|
| - | 340 | ServoRollValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER; |
|
| - | 341 | } |
|
| - | 342 | RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
|
| - | 343 | ServoRollValue /= MULTIPLYER; |
|
| - | 344 | //DebugOut.Analog[20] = ServoRollValue; |
|
| - | 345 | ||
| - | 346 | /* ServoRollOffset = (ServoRollOffset * 3 + (int16_t)Parameter_ServoRollControl * MULTIPLYER) / 4; // lowpass offset |
|
| - | 347 | ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765) |
|
| - | 348 | if(EE_Parameter.ServoRollCompInvert & 0x01) |
|
| - | 349 | { // inverting movement of servo |
|
| - | 350 | ServoRollValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
|
| - | 351 | } |
|
| - | 352 | else |
|
| - | 353 | { // non inverting movement of servo |
|
| - | 354 | ServoRollValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
|
| - | 355 | } |
|
| - | 356 | // limit servo value to its parameter range definition |
|
| - | 357 | if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) ) |
|
| - | 358 | { |
|
| - | 359 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER; |
|
| - | 360 | } |
|
| - | 361 | else |
|
| - | 362 | if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) ) |
|
| - | 363 | { |
|
| - | 364 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER; |
|
| - | 365 | } |
|
| - | 366 | RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
|
| - | 367 | ServoRollValue /= MULTIPLYER; |
|
| - | 368 | //DebugOut.Analog[20] = ServoRollValue; |
|
| - | 369 | */ break; |
|
| Line 320... | Line 370... | ||
| 320 | 370 | ||
| 321 | default: // other servo channels |
371 | default: // other servo channels |
| 322 | RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs |
372 | RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs |
| 323 | break; |
373 | break; |