Subversion Repositories FlightCtrl

Rev

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
                                                        }