Subversion Repositories FlightCtrl

Rev

Rev 1171 | Rev 1232 | 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;