Subversion Repositories FlightCtrl

Rev

Rev 2176 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2176 Rev 2177
Line 58... Line 58...
58
volatile static unsigned int tim_main;
58
volatile static unsigned int tim_main;
59
volatile unsigned char UpdateMotor = 0;
59
volatile unsigned char UpdateMotor = 0;
60
volatile unsigned int cntKompass = 0;
60
volatile unsigned int cntKompass = 0;
61
volatile unsigned int beeptime = 0;
61
volatile unsigned int beeptime = 0;
62
volatile unsigned char SendSPI = 0, ServoActive = 0, CalculateServoSignals = 1;
62
volatile unsigned char SendSPI = 0, ServoActive = 0, CalculateServoSignals = 1;
63
uint16_t RemainingPulse = 0;
63
uint16_t RemainingPulse = 255; // MartinR zur Sicherheit bei Interrupts
64
volatile int16_t ServoNickOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon
64
volatile int16_t ServoNickOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon
65
volatile int16_t ServoRollOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon
65
volatile int16_t ServoRollOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon
-
 
66
volatile int16_t ServoPanOffset = (255 / 2) * MULTIPLYER * 16; // MartinR: für Pan-Funktion
Line 66... Line 67...
66
 
67
 
Line 67... Line 68...
67
unsigned int BeepMuster = 0xffff;
68
unsigned int BeepMuster = 0xffff;
68
 
69
 
-
 
70
volatile int16_t        ServoNickValue = 0;
Line 69... Line 71...
69
volatile int16_t        ServoNickValue = 0;
71
volatile int16_t        ServoRollValue = 0;
70
volatile int16_t        ServoRollValue = 0;
72
volatile int16_t        ServoPanValue = 0; // MartinR : für PAN-Funktion
71
 
73
 
Line 242... Line 244...
242
/*****************************************************/
244
/*****************************************************/
Line 243... Line 245...
243
 
245
 
244
 
246
 
245
void CalculateServo(void)
247
void CalculateServo(void)
-
 
248
{
246
{
249
 //signed char cosinus, sinus; // MartinR : so war es
Line -... Line 250...
-
 
250
 extern signed char cosinus, sinus; // MartinR : extern für PAN-Funktion
-
 
251
 signed long nick, roll;
-
 
252
 
-
 
253
        nick = 0; // MartinR : StartWert bei abgeschalteten Nick/ Roll ausgleich
-
 
254
        roll = 0; // MartinR : StartWert bei abgeschalteten Nick/ Roll ausgleich
-
 
255
        int tmp; // MartinR : für PAN-Funktion // Wert : 0-24 -> 0-360 -> 15° steps
-
 
256
        /* // MartinR: bisher
-
 
257
        tmp = EE_Parameter.CamOrientation + ((Parameter_Servo4 - 125) * (Parameter_UserParam8 - 125)) / 400 ; //MartinR : für PAN-Funktion
-
 
258
        if (tmp < 0) tmp = 24- (abs(tmp)) % 24 ; // MartinR :Modulo 24
-
 
259
        else tmp = tmp % 24 ; // MartinR :Modulo 24
-
 
260
        */
-
 
261
        tmp = EE_Parameter.CamOrientation + ((Parameter_Servo4 - 125) * (Parameter_UserParam8 - 125)) / 200 ; //MartinR : für PAN-Funktion
247
 signed char cosinus, sinus;
262
        if (tmp < 0) tmp = 48- (abs(tmp)) % 48 ; // MartinR :Modulo 48
248
 signed long nick, roll;
263
        else tmp = tmp % 48 ; // MartinR :Modulo 48
-
 
264
       
-
 
265
        // cosinus = sintab[EE_Parameter.CamOrientation + 6];  // MartinR : so war es
-
 
266
        // sinus = sintab[EE_Parameter.CamOrientation];  // MartinR : so war es
Line 249... Line 267...
249
 
267
        //cosinus = sintab[tmp + 6];  // MartinR : für PAN-Funktion
250
        cosinus = sintab[EE_Parameter.CamOrientation + 6];
268
        cosinus += (2*sintab[tmp + 12]- cosinus + 1) / 2;  // MartinR : für PAN-Funktion
-
 
269
        sinus += (2*sintab[tmp] - sinus + 1) / 2;  // MartinR : für PAN-Funktion
-
 
270
 
251
        sinus = sintab[EE_Parameter.CamOrientation];
271
  if(CalculateServoSignals == 1)
-
 
272
   {
252
 
273
        if (Parameter_UserParam7 < 50)  // MartinR: um per UserParameter den Nickausgleich abzuschalten
-
 
274
                {
253
  if(CalculateServoSignals == 1)
275
                //nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; // MartinR: so war es
254
   {
276
                nick = (cosinus * IntegralNick) / 512L - (sinus * IntegralRoll) / 512L; // MartinR: bessere Auflösung
255
                nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L;
277
        nick -= POI_KameraNick * 7;
256
        nick -= POI_KameraNick * 7;
278
                }
257
                nick = ((long)EE_Parameter.ServoNickComp * nick) / 512L;
279
                nick = ((long)EE_Parameter.ServoNickComp * nick) / 512L;
Line 279... Line 301...
279
                }
301
                }
280
                if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++;
302
                if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++;
281
        }
303
        }
282
        else
304
        else
283
        {
305
        {
-
 
306
                if (Parameter_UserParam7 < 100)  // MartinR: um per UserParameter den Nickausgleich abzuschalten
-
 
307
                {
284
        roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L;
308
        //roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; // MartinR: so war es
-
 
309
                roll = (cosinus * IntegralRoll) / 512L + (sinus * IntegralNick) / 512L; // MartinR: bessere Auflösung
-
 
310
                }
285
        roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L;
311
        roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L;
286
                ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed;
312
                ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed;
287
                if(EE_Parameter.ServoCompInvert & 0x02)
313
                if(EE_Parameter.ServoCompInvert & 0x02)
288
                {       // inverting movement of servo
314
                {       // inverting movement of servo
289
                        roll = ServoRollOffset / 16 + roll;
315
                        roll = ServoRollOffset / 16 + roll;
Line 302... Line 328...
302
                else
328
                else
303
                if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER))
329
                if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER))
304
                {
330
                {
305
                        ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER;
331
                        ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER;
306
                }
332
                }
-
 
333
               
-
 
334
                // MartinR: Filterung der Pan- Funktion
-
 
335
                ServoPanOffset += ((int16_t)Parameter_Servo4 * (MULTIPLYER*16) - ServoPanOffset) / EE_Parameter.ServoManualControlSpeed;
-
 
336
                ServoPanValue = (int16_t)ServoPanOffset/16; // offset (Range from 0 to 255 * 3 = 765)
-
 
337
               
307
                CalculateServoSignals = 0;
338
                CalculateServoSignals = 0;
308
        }
339
        }
309
}
340
}
Line 310... Line 341...
310
 
341
 
Line 390... Line 421...
390
                                                        break;
421
                                                        break;
391
                                         case 3:
422
                                         case 3:
392
                                                        RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER;
423
                                                        RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER;
393
                                                        break;
424
                                                        break;
394
                                         case 4:
425
                                         case 4:
395
                                                        RemainingPulse += ((int16_t)Parameter_Servo4 * MULTIPLYER) - (256 / 2) * MULTIPLYER;
426
                                                        //RemainingPulse += ((int16_t)Parameter_Servo4 * MULTIPLYER) - (256 / 2) * MULTIPLYER; // MartinR: so war es
-
 
427
                                                        RemainingPulse += ServoPanValue - (256 / 2) * MULTIPLYER; // MartinR: zur Filterung der Pan-Funktion
396
                                                        break;
428
                                                        break;
397
                                         case 5:
429
                                         case 5:
398
                                                        RemainingPulse += ((int16_t)Parameter_Servo5 * MULTIPLYER) - (256 / 2) * MULTIPLYER;
430
                                                        RemainingPulse += ((int16_t)Parameter_Servo5 * MULTIPLYER) - (256 / 2) * MULTIPLYER;
399
                                                        break;
431
                                                        break;
400
                                                default: // other servo channels
432
                                                default: // other servo channels