Subversion Repositories FlightCtrl

Rev

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

Rev 761 Rev 762
Line 68... Line 68...
68
#include "mm3.h"
68
#include "mm3.h"
69
#endif
69
#endif
70
#ifdef USE_CMPS03
70
#ifdef USE_CMPS03
71
#include "cmps03.h"
71
#include "cmps03.h"
72
#endif
72
#endif
-
 
73
#include "led.h"
Line 73... Line 74...
73
 
74
 
74
volatile uint16_t I2CTimeout = 100;
75
volatile uint16_t I2CTimeout = 100;
75
// gyro readings
76
// gyro readings
76
volatile int16_t Reading_GyroPitch, Reading_GyroRoll, Reading_GyroYaw;
77
volatile int16_t Reading_GyroPitch, Reading_GyroRoll, Reading_GyroYaw;
Line 112... Line 113...
112
float Gyro_P_Factor;
113
float Gyro_P_Factor;
113
float Gyro_I_Factor;
114
float Gyro_I_Factor;
Line 114... Line 115...
114
 
115
 
Line 115... Line 116...
115
volatile int16_t  DiffPitch, DiffRoll;
116
volatile int16_t  DiffPitch, DiffRoll;
Line 116... Line 117...
116
 
117
 
117
int16_t  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
118
int16_t  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
Line 118... Line 119...
118
 
119
 
Line 360... Line 361...
360
    //update poti values by rc-signals
361
    //update poti values by rc-signals
361
    if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
362
    if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
362
    if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
363
    if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
363
    if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
364
    if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
364
    if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
365
    if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
-
 
366
 
-
 
367
        //PPM24-Extension
-
 
368
        if(Poti5 < PPM_in[9] + 110)  Poti5++; else if(Poti5 >  PPM_in[9] + 110 && Poti5) Poti5--;
-
 
369
        if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--;
-
 
370
        if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--;
-
 
371
        if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--;
-
 
372
 
365
        //limit poti values
373
        //limit poti values
366
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
374
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
367
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
375
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
368
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
376
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
369
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
377
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
-
 
378
    //PPM24-Extension
-
 
379
        if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255;
-
 
380
        if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255;
-
 
381
        if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255;
-
 
382
    if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255;
Line 370... Line 383...
370
 
383
 
371
        TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
384
        TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
372
        TurnOver180Roll =  (int32_t) ParamSet.AngleTurnOverRoll  * 2500L;
385
        TurnOver180Roll =  (int32_t) ParamSet.AngleTurnOverRoll  * 2500L;
Line 405... Line 418...
405
/************************************************************************/
418
/************************************************************************/
406
/*  Maps the parameter to poti values                                   */
419
/*  Maps the parameter to poti values                                   */
407
/************************************************************************/
420
/************************************************************************/
408
void ParameterMapping(void)
421
void ParameterMapping(void)
409
{
422
{
-
 
423
        if(SenderOkay > 140) // do the mapping of RC-Potis only if the rc-signal is ok
-
 
424
        // else the last updated values are used
410
 
425
        {
411
        #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
426
                #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
412
        CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight,0,255);
427
                CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight,0,255);
413
        CHK_POTI(FCParam.Height_D,ParamSet.Height_D,0,100);
428
                CHK_POTI(FCParam.Height_D,ParamSet.Height_D,0,100);
414
        CHK_POTI(FCParam.Height_P,ParamSet.Height_P,0,100);
429
                CHK_POTI(FCParam.Height_P,ParamSet.Height_P,0,100);
415
        CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect,0,255);
430
                CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect,0,255);
416
        CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect,0,255);
431
                CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect,0,255);
417
        CHK_POTI(FCParam.Gyro_P,ParamSet.Gyro_P,10,255);
432
                CHK_POTI(FCParam.Gyro_P,ParamSet.Gyro_P,10,255);
418
        CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I,0,255);
433
                CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I,0,255);
419
        CHK_POTI(FCParam.I_Factor,ParamSet.I_Factor,0,255);
434
                CHK_POTI(FCParam.I_Factor,ParamSet.I_Factor,0,255);
420
        CHK_POTI(FCParam.UserParam1,ParamSet.UserParam1,0,255);
435
                CHK_POTI(FCParam.UserParam1,ParamSet.UserParam1,0,255);
421
        CHK_POTI(FCParam.UserParam2,ParamSet.UserParam2,0,255);
436
                CHK_POTI(FCParam.UserParam2,ParamSet.UserParam2,0,255);
422
        CHK_POTI(FCParam.UserParam3,ParamSet.UserParam3,0,255);
437
                CHK_POTI(FCParam.UserParam3,ParamSet.UserParam3,0,255);
423
        CHK_POTI(FCParam.UserParam4,ParamSet.UserParam4,0,255);
438
                CHK_POTI(FCParam.UserParam4,ParamSet.UserParam4,0,255);
424
        CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5,0,255);
439
                CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5,0,255);
425
        CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6,0,255);
440
                CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6,0,255);
426
        CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7,0,255);
441
                CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7,0,255);
427
        CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8,0,255);
442
                CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8,0,255);
428
        CHK_POTI(FCParam.ServoPitchControl,ParamSet.ServoPitchControl,0,255);
443
                CHK_POTI(FCParam.ServoPitchControl,ParamSet.ServoPitchControl,0,255);
429
        CHK_POTI(FCParam.LoopThrustLimit,ParamSet.LoopThrustLimit,0,255);
444
                CHK_POTI(FCParam.LoopThrustLimit,ParamSet.LoopThrustLimit,0,255);
430
        CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255);
445
                CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255);
431
        CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255);
446
                CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255);
432
        CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255);
447
                CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255);
433
 
-
 
434
        Ki = (float) FCParam.I_Factor * FACTOR_I;
448
                Ki = (float) FCParam.I_Factor * FACTOR_I;
-
 
449
        }
435
}
450
}
Line 436... Line 451...
436
 
451
 
437
 
452
 
438
/************************************************************************/
453
/************************************************************************/
439
/*  MotorControl                                                        */
454
/*  MotorControl                                                        */
440
/************************************************************************/
455
/************************************************************************/
441
void MotorControl(void)
456
void MotorControl(void)
442
{
457
{
443
         int16_t MotorValue, pd_result, h, tmp_int;
458
        int16_t MotorValue, pd_result, h, tmp_int;
444
         int16_t YawMixFraction, ThrustMixFraction;
459
        int16_t YawMixFraction, ThrustMixFraction;
445
     static int32_t SumPitch = 0, SumRoll = 0;
460
        static int32_t SumPitch = 0, SumRoll = 0;
446
     static int32_t SetPointYaw = 0;
461
        static int32_t SetPointYaw = 0;
447
     static int32_t IntegralErrorPitch = 0;
462
        static int32_t IntegralErrorPitch = 0;
448
     static int32_t IntegralErrorRoll = 0;
463
        static int32_t IntegralErrorRoll = 0;
449
         static uint16_t RcLostTimer;
464
        static uint16_t RcLostTimer;
450
         static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0;
465
        static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0;
451
         static uint16_t Modell_Is_Flying = 0;
466
        static uint16_t Modell_Is_Flying = 0;
452
         static uint8_t HeightControlActive = 0;
467
        static uint8_t HeightControlActive = 0;
453
     static int16_t HeightControlThrust = 0;
468
        static int16_t HeightControlThrust = 0;
454
     static int8_t TimerDebugOut = 0;
469
        static int8_t TimerDebugOut = 0;
Line 455... Line 470...
455
     static int8_t StoreNewCompassCourse = 0;
470
        static int8_t StoreNewCompassCourse = 0;
-
 
471
        static int32_t CorrectionPitch, CorrectionRoll;
Line 456... Line -...
456
     static int32_t CorrectionPitch, CorrectionRoll;
-
 
457
 
472
 
458
        Mean();
473
        Mean();
459
 
474
        GRN_ON;
460
    GRN_ON;
475
 
461
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
476
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 469... Line 484...
469
// SenderOkay is incremented at good rc-level, i.e. if the ppm-signal deviation
484
// SenderOkay is incremented at good rc-level, i.e. if the ppm-signal deviation
470
// of a channel to previous frame is less than 1% the SenderOkay is incremented by 10.
485
// of a channel to previous frame is less than 1% the SenderOkay is incremented by 10.
471
// Typicaly within a frame of 8 channels (22.5ms) the SenderOkay is incremented by 8 * 10 = 80
486
// Typicaly within a frame of 8 channels (22.5ms) the SenderOkay is incremented by 8 * 10 = 80
472
// The decremtation of 1 in the mainloop is done every 2 ms, i.e. within a time of one rc frame
487
// The decremtation of 1 in the mainloop is done every 2 ms, i.e. within a time of one rc frame
473
// the main loop is running 11 times that decrements the SenderOkay by 11.
488
// the main loop is running 11 times that decrements the SenderOkay by 11.
474
if(SenderOkay < 100)  // the rc-frame signal is not reveived or noisy
489
        if(SenderOkay < 100)  // the rc-frame signal is not reveived or noisy
475
        {
490
        {
476
                if(!PcAccess) // if also no PC-Access via UART
491
                if(!PcAccess) // if also no PC-Access via UART
477
                {
492
                {
478
                        if(BeepModulation == 0xFFFF)
493
                        if(BeepModulation == 0xFFFF)
479
                        {
494
                        {
Line 498... Line 513...
498
                        PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] = 0;
513
                        PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] = 0;
499
                        PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
514
                        PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
500
                        PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0;
515
                        PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0;
501
                }
516
                }
502
                else MotorsOn = 0; // switch of all motors
517
                else MotorsOn = 0; // switch of all motors
503
        }
518
        } // eof SenderOkay < 100
504
        else
519
        else
505
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
520
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
506
// RC-signal is good
521
// RC-signal is good
507
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
522
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
508
        if(SenderOkay > 140)
523
        if(SenderOkay > 140)
Line 528... Line 543...
528
                        if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75)
543
                        if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75)
529
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
544
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
530
// calibrate the neutral readings of all attitude sensors
545
// calibrate the neutral readings of all attitude sensors
531
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
546
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
532
                        {
547
                        {
533
                        if(++delay_neutral > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
548
                                if(++delay_neutral > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
534
                        {
-
 
535
                                delay_neutral = 0;
-
 
536
                                GRN_OFF;
-
 
537
                                Modell_Is_Flying = 0;
-
 
538
                                // check roll/pitch stick position
-
 
539
                                // if pitch stick is topmost or roll stick is leftmost --> change parameter setting
-
 
540
                                // according to roll/pitch stick position
-
 
541
                                if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70)
-
 
542
                                {
549
                                {
-
 
550
                                        delay_neutral = 0;
-
 
551
                                        GRN_OFF;
-
 
552
                                        Modell_Is_Flying = 0;
-
 
553
                                        // check roll/pitch stick position
-
 
554
                                        // if pitch stick is topmost or roll stick is leftmost --> change parameter setting
-
 
555
                                        // according to roll/pitch stick position
-
 
556
                                        if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70)
-
 
557
                                        {
543
                                         uint8_t setting = 1; // default
558
                                                 uint8_t setting = 1; // default
544
                                         //  _________
559
                                                 //  _________
545
                                         // |2   3   4|
560
                                                 // |2   3   4|
546
                                         // |         |
561
                                                 // |         |
547
                                         // |1       5|
562
                                                 // |1       5|
548
                                         // |         |
563
                                                 // |         |
549
                                         // |_________|
564
                                                 // |_________|
550
                                         //
565
                                                 //
551
                                         // roll stick leftmost and pitch stick centered --> setting 1
566
                                                 // roll stick leftmost and pitch stick centered --> setting 1
552
                                         if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) setting = 1;
567
                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) setting = 1;
553
                                         // roll stick leftmost and pitch stick topmost --> setting 2
568
                                                 // roll stick leftmost and pitch stick topmost --> setting 2
554
                                         if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 2;
569
                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 2;
555
                                         // roll stick centered an pitch stick topmost --> setting 3
570
                                                 // roll stick centered an pitch stick topmost --> setting 3
556
                                         if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 3;
571
                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 3;
557
                                         // roll stick rightmost and pitch stick topmost --> setting 4
572
                                                 // roll stick rightmost and pitch stick topmost --> setting 4
558
                                         if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 4;
573
                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 4;
559
                                         // roll stick rightmost and pitch stick centered --> setting 5
574
                                                 // roll stick rightmost and pitch stick centered --> setting 5
560
                                         if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) setting = 5;
575
                                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) setting = 5;
561
                                         // update active parameter set in eeprom
576
                                                 // update active parameter set in eeprom
562
                                         SetActiveParamSet(setting);
577
                                                 SetActiveParamSet(setting);
563
                                }
578
                                        }
564
                                ParamSet_ReadFromEEProm(GetActiveParamSet());
579
                                        ParamSet_ReadFromEEProm(GetActiveParamSet());
565
                                SetNeutral();
580
                                        SetNeutral();
566
                                Beep(GetActiveParamSet());
581
                                        Beep(GetActiveParamSet());
567
                                }
582
                                }
568
                        }
583
                        }
569
                        // and if the yaw stick is in the rightmost position
584
                        // and if the yaw stick is in the rightmost position
570
                        // save the ACC neutral setting to eeprom
585
                        // save the ACC neutral setting to eeprom
571
            else if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75)
586
                        else if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75)
572
                        {
587
                        {
573
                        if(++delay_neutral > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
588
                        if(++delay_neutral > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
574
                                {
589
                                {
575
                                delay_neutral = 0;
590
                                delay_neutral = 0;
576
                                GRN_OFF;
591
                                GRN_OFF;
Line 582... Line 597...
582
                                SetParamWord(PID_ACC_ROLL,  (uint16_t)NeutralAccY);
597
                                SetParamWord(PID_ACC_ROLL,  (uint16_t)NeutralAccY);
583
                                SetParamWord(PID_ACC_Z,     (uint16_t)NeutralAccZ);
598
                                SetParamWord(PID_ACC_Z,     (uint16_t)NeutralAccZ);
584
                                Beep(GetActiveParamSet());
599
                                Beep(GetActiveParamSet());
585
                                }
600
                                }
586
                        }
601
                        }
587
            else delay_neutral = 0;
602
                        else delay_neutral = 0;
588
                }
603
                }
589
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
604
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
590
// thrust stick is down
605
// thrust stick is down
591
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
606
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
592
                if(PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < -85)
607
                if(PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < -85)
Line 622... Line 637...
622
                                if(++delay_stopmotors > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
637
                                if(++delay_stopmotors > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
623
                                {
638
                                {
624
                                        delay_stopmotors = 200; // do not repeat if once executed
639
                                        delay_stopmotors = 200; // do not repeat if once executed
625
                                        Modell_Is_Flying = 0;
640
                                        Modell_Is_Flying = 0;
626
                                        MotorsOn = 0;
641
                                        MotorsOn = 0;
627
 
-
 
-
 
642
                                        GPS_ClearHomePosition();
628
                                }
643
                                }
629
                        }
644
                        }
630
                        else delay_stopmotors = 0; // reset delay timer if sticks are not in this position
645
                        else delay_stopmotors = 0; // reset delay timer if sticks are not in this position
631
                        }
-
 
632
                }
646
                }
-
 
647
                        // remapping of paameters only if the signal rc-sigbnal conditions are good
-
 
648
        } // eof SenderOkay > 140
633
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
649
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
634
// new values from RC
650
// new values from RC
635
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
651
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
636
                if(!NewPpmData-- || EmergencyLanding) // NewData = 0 means new data from RC
652
        if(!NewPpmData-- || EmergencyLanding) // NewData = 0 means new data from RC
637
                {
653
        {
638
                        int tmp_int;
654
                int tmp_int;
639
                        ParameterMapping(); // remapping params (online poti replacement)
655
                ParameterMapping(); // remapping params (online poti replacement)
640
 
-
 
641
                        // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
656
                // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
642
                        StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4;
657
                StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4;
643
                        StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D;
658
                StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D;
644
                        StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4;
659
                StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4;
645
                        StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D;
660
                StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D;
646
 
661
 
647
                        // direct mapping of yaw and thrust
662
                // direct mapping of yaw and thrust
648
                        StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]];
663
                StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]];
649
                        StickThrust  = PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] + 120;// shift to positive numbers
664
                StickThrust  = PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] + 120;// shift to positive numbers
650
 
665
 
651
                        // update max stick positions for pitch, roll and yaw
666
                // update max stick positions for pitch, roll and yaw
652
                        if(abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) > MaxStickPitch)
667
                if(abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) > MaxStickPitch)
653
                                MaxStickPitch = abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]);
668
                        MaxStickPitch = abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]);
654
                        else MaxStickPitch--;
669
                else MaxStickPitch--;
655
                        if(abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > MaxStickRoll)
670
                if(abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > MaxStickRoll)
656
                                MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]);
671
                        MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]);
657
                        else MaxStickRoll--;
672
                else MaxStickRoll--;
658
                        if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > MaxStickYaw)
673
                if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > MaxStickYaw)
659
                                MaxStickYaw = abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]);
674
                        MaxStickYaw = abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]);
660
                        else MaxStickYaw--;
675
                else MaxStickYaw--;
Line 661... Line 676...
661
 
676
 
Line 662... Line 677...
662
                        // update gyro control loop factors
677
                // update gyro control loop factors
663
 
678
 
Line 664... Line 679...
664
                        Gyro_P_Factor = ((float) FCParam.Gyro_P + 10.0) / 256.0;
679
                Gyro_P_Factor = ((float) FCParam.Gyro_P + 10.0) / 256.0;
665
                        Gyro_I_Factor = ((float) FCParam.Gyro_I) / 44000;
680
                Gyro_I_Factor = ((float) FCParam.Gyro_I) / 44000;
666
 
681
 
Line 667... Line 682...
667
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
682
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
668
// Digital Control via DubWise
683
// Digital Control via DubWise
669
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
684
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
670
 
685
 
671
                        #define KEY_VALUE (FCParam.UserParam1 * 4) // step width
686
                #define KEY_VALUE (FCParam.UserParam1 * 4) // step width
672
                        if(DubWiseKeys[1]) BeepTime = 10;
687
                if(DubWiseKeys[1]) BeepTime = 10;
673
                        if(DubWiseKeys[1] & DUB_KEY_UP)  tmp_int = KEY_VALUE;
688
                if(DubWiseKeys[1] & DUB_KEY_UP)  tmp_int = KEY_VALUE;
674
                        else if(DubWiseKeys[1] & DUB_KEY_DOWN)  tmp_int = -KEY_VALUE;
689
                else if(DubWiseKeys[1] & DUB_KEY_DOWN)  tmp_int = -KEY_VALUE;
675
                        else tmp_int = 0;
690
                else tmp_int = 0;
676
                        ExternStickPitch = (ExternStickPitch * 7 + tmp_int) / 8;
691
                ExternStickPitch = (ExternStickPitch * 7 + tmp_int) / 8;
677
                        if(DubWiseKeys[1] & DUB_KEY_LEFT)  tmp_int = KEY_VALUE;
692
                if(DubWiseKeys[1] & DUB_KEY_LEFT)  tmp_int = KEY_VALUE;
678
                        else if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE;
693
                else if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE;
679
                        else tmp_int = 0;
694
                else tmp_int = 0;
680
                        ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;
695
                ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;
681
 
696
 
682
                        if(DubWiseKeys[0] & 8)  ExternStickYaw = 50;else
697
                if(DubWiseKeys[0] & 8)  ExternStickYaw = 50;else
683
                        if(DubWiseKeys[0] & 4)  ExternStickYaw =-50;else ExternStickYaw = 0;
698
                if(DubWiseKeys[0] & 4)  ExternStickYaw =-50;else ExternStickYaw = 0;
684
                        if(DubWiseKeys[0] & 2)  ExternHeightValue++;
699
                if(DubWiseKeys[0] & 2)  ExternHeightValue++;
685
                        if(DubWiseKeys[0] & 16) ExternHeightValue--;
700
                if(DubWiseKeys[0] & 16) ExternHeightValue--;
Line 686... Line 701...
686
 
701
 
687
                        StickPitch += ExternStickPitch / 8;
702
                StickPitch += ExternStickPitch / 8;
688
                        StickRoll += ExternStickRoll / 8;
703
                StickRoll += ExternStickRoll / 8;
Line 689... Line 704...
689
                        StickYaw += ExternStickYaw;
704
                StickYaw += ExternStickYaw;
690
 
705
 
691
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
706
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
692
//+ Analog control via serial communication
707
//+ Analog control via serial communication
693
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
708
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
694
 
709
 
695
                    if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128)
710
                if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128)
696
                        {
711
                {
697
                                 StickPitch += (int16_t) ExternControl.Pitch * (int16_t) ParamSet.Stick_P;
712
                         StickPitch += (int16_t) ExternControl.Pitch * (int16_t) ParamSet.Stick_P;
698
                                 StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P;
713
                         StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P;
699
                                 StickYaw += ExternControl.Yaw;
714
                         StickYaw += ExternControl.Yaw;
700
                                 ExternHeightValue =  (int16_t) ExternControl.Height * (int16_t)ParamSet.Height_Gain;
715
                         ExternHeightValue =  (int16_t) ExternControl.Height * (int16_t)ParamSet.Height_Gain;
701
                                 if(ExternControl.Thrust < StickThrust) StickThrust = ExternControl.Thrust;
716
                         if(ExternControl.Thrust < StickThrust) StickThrust = ExternControl.Thrust;
Line 702... Line 717...
702
                        }
717
                }
703
            // disable I part of gyro control feedback
718
                // disable I part of gyro control feedback
704
                        if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) Gyro_I_Factor =  0;
719
                if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) Gyro_I_Factor =  0;
Line 705... Line 720...
705
                        // avoid negative scaling factors
720
                // avoid negative scaling factors
706
                        if(Gyro_P_Factor < 0) Gyro_P_Factor = 0;
721
                if(Gyro_P_Factor < 0) Gyro_P_Factor = 0;
707
                        if(Gyro_I_Factor < 0) Gyro_I_Factor = 0;
722
                if(Gyro_I_Factor < 0) Gyro_I_Factor = 0;
708
 
723
 
709
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
724
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
710
// Looping?
725
// Looping?
711
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
726
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
712
 
727
 
713
                        if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LEFT)  Looping_Left = 1;
728
                if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LEFT)  Looping_Left = 1;
714
                        else
729
                else
715
                        {
730
                {
716
                         {
731
                 {
717
                          if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Left = 0;
732
                  if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Left = 0;
718
                         }
733
                 }
719
                        }
734
                }
Line 720... Line 735...
720
                        if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RIGHT) Looping_Right = 1;
735
                if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RIGHT) Looping_Right = 1;
721
                        else
736
                else
722
                        {
737
                {
723
                        if(Looping_Right) // Hysterese
738
                if(Looping_Right) // Hysterese
724
                         {
739
                 {
725
                          if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Right = 0;
740
                  if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Right = 0;
726
                         }
741
                 }
727
                        }
742
                }
728
 
743
 
729
                        if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UP) Looping_Top = 1;
744
                if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UP) Looping_Top = 1;
730
                        else
745
                else
731
                        {
746
                {
732
                        if(Looping_Top)  // Hysterese
747
                if(Looping_Top)  // Hysterese
733
                         {
748
                 {
734
                          if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Top = 0;
749
                  if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Top = 0;
735
                         }
750
                 }
Line 736... Line 751...
736
                        }
751
                }
737
                        if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_DOWN) Looping_Down = 1;
752
                if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_DOWN) Looping_Down = 1;
738
                        else
753
                else
Line 739... Line 754...
739
                        {
754
                {
740
                        if(Looping_Down) // Hysterese
755
                if(Looping_Down) // Hysterese
741
                         {
756
                 {
742
                          if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Down = 0;
757
                  if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Down = 0;
743
                         }
758
                 }
-
 
759
                }
-
 
760
 
-
 
761
                if(Looping_Left || Looping_Right)   Looping_Roll = 1; else Looping_Roll = 0;
-
 
762
                if(Looping_Top  || Looping_Down) {Looping_Pitch = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Pitch = 0;
-
 
763
        } // End of new RC-Values or Emergency Landing
-
 
764
 
-
 
765
 
-
 
766
        if(Looping_Roll) BeepTime = 100;
Line 744... Line 767...
744
                        }
767
        if(Looping_Roll || Looping_Pitch)
745
 
768
        {
746
                        if(Looping_Left || Looping_Right)   Looping_Roll = 1; else Looping_Roll = 0;
769
                if(ThrustMixFraction > ParamSet.LoopThrustLimit) ThrustMixFraction = ParamSet.LoopThrustLimit;
747
                        if(Looping_Top  || Looping_Down) {Looping_Pitch = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Pitch = 0;
770
        }
748
                } // End of new RC-Values or Emergency Landing
771
 
749
 
772
 
750
 
773
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
751
                if(Looping_Roll) BeepTime = 100;
774
//+ LED Control on J16/J17
752
                if(Looping_Roll || Looping_Pitch)
775
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
753
                {
776
        LED_OffTime = FCParam.UserParam7;
754
                if(ThrustMixFraction > ParamSet.LoopThrustLimit) ThrustMixFraction = ParamSet.LoopThrustLimit;
777
        LED_OnTime = FCParam.UserParam8;
755
                }
778
        LED_Update();
756
 
779
 
757
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
780
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
758
// in case of emergency landing
781
// in case of emergency landing
759
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
782
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
760
                // set all inputs to save values
783
        // set all inputs to save values
Line 761... Line 784...
761
                if(EmergencyLanding)
784
        if(EmergencyLanding)
762
                {
785
        {
763
                        StickYaw = 0;
786
                StickYaw = 0;
Line 764... Line 787...
764
                        StickPitch = 0;
787
                StickPitch = 0;
765
                        StickRoll = 0;
788
                StickRoll = 0;
766
                        Gyro_P_Factor  = 0.5;
789
                Gyro_P_Factor  = 0.5;
767
                        Gyro_I_Factor = 0.003;
790
                Gyro_I_Factor = 0.003;
Line 768... Line 791...
768
                        Looping_Roll = 0;
791
                Looping_Roll = 0;
769
                        Looping_Pitch = 0;
792
                Looping_Pitch = 0;
770
                        MaxStickPitch = 0;
793
                MaxStickPitch = 0;
771
                        MaxStickRoll = 0;
794
                MaxStickRoll = 0;
Line 772... Line 795...
772
                        MaxStickYaw = 0;
795
                MaxStickYaw = 0;
773
                }
796
        }
Line 774... Line 797...
774
 
797
 
775
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
798
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 776... Line 799...
776
// Trim Gyro-Integrals to ACC-Signals
799
// Trim Gyro-Integrals to ACC-Signals
777
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
800
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 778... Line 801...
778
 
801
 
779
                #define BALANCE_NUMBER 256L
802
        #define BALANCE_NUMBER 256L
-
 
803
        // sum for averaging
-
 
804
        MeanIntegralPitch  += IntegralPitch;
-
 
805
        MeanIntegralRoll  += IntegralRoll;
-
 
806
 
-
 
807
        if(Looping_Pitch || Looping_Roll) // if looping in any direction
-
 
808
        {
-
 
809
                // reset averaging for acc and gyro integral as well as gyro integral acc correction
-
 
810
                MeasurementCounter = 0;
-
 
811
 
-
 
812
                IntegralAccPitch = 0;
-
 
813
                IntegralAccRoll = 0;
-
 
814
 
-
 
815
                MeanIntegralPitch = 0;
-
 
816
                MeanIntegralRoll = 0;
-
 
817
 
-
 
818
                Reading_IntegralGyroPitch2 = Reading_IntegralGyroPitch;
-
 
819
                Reading_IntegralGyroRoll2 = Reading_IntegralGyroRoll;
-
 
820
 
-
 
821
                AttitudeCorrectionPitch = 0;
-
 
822
                AttitudeCorrectionRoll = 0;
-
 
823
        }
780
        // sum for averaging
824
 
Line -... Line 825...
-
 
825
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
826
        if(!Looping_Pitch && !Looping_Roll) // if not lopping in any direction
-
 
827
        {
-
 
828
                int32_t tmp_long, tmp_long2;
-
 
829
                // determine the deviation of gyro integral from averaged acceleration sensor
-
 
830
                tmp_long   =  (int32_t)(IntegralPitch / ParamSet.GyroAccFaktor - (int32_t)Mean_AccPitch);
-
 
831
                tmp_long  /= 16;
-
 
832
                tmp_long2  = (int32_t)(IntegralRoll   / ParamSet.GyroAccFaktor - (int32_t)Mean_AccRoll);
-
 
833
                tmp_long2 /= 16;
-
 
834
 
781
                MeanIntegralPitch  += IntegralPitch;
835
                if((MaxStickPitch > 15) || (MaxStickRoll > 15)) // reduce effect during stick commands
-
 
836
                {
-
 
837
                        tmp_long  /= 3;
-
 
838
                        tmp_long2 /= 3;
-
 
839
                }
-
 
840
                if(MaxStickYaw > 25) // reduce further is yaw stick is active
-
 
841
                {
-
 
842
                        tmp_long  /= 3;
-
 
843
                        tmp_long2 /= 3;
-
 
844
                }
782
                MeanIntegralRoll  += IntegralRoll;
845
 
783
 
846
                #define BALANCE 32
-
 
847
                // limit correction effect
-
 
848
                if(tmp_long >  BALANCE)  tmp_long  = BALANCE;
784
                if(Looping_Pitch || Looping_Roll) // if looping in any direction
849
                if(tmp_long < -BALANCE)  tmp_long  =-BALANCE;
-
 
850
                if(tmp_long2 > BALANCE)  tmp_long2 = BALANCE;
-
 
851
                if(tmp_long2 <-BALANCE)  tmp_long2 =-BALANCE;
-
 
852
                // correct current readings
-
 
853
                Reading_IntegralGyroPitch -= tmp_long;
-
 
854
                Reading_IntegralGyroRoll -= tmp_long2;
-
 
855
        }
785
                {
856
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
786
                        // reset averaging for acc and gyro integral as well as gyro integral acc correction
857
        // MeasurementCounter is incremented in the isr of analog.c
-
 
858
        if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached
787
                        MeasurementCounter = 0;
859
        {
-
 
860
                static int16_t cnt = 0;
-
 
861
                static int8_t last_n_p, last_n_n, last_r_p, last_r_n;
788
 
862
                static int32_t MeanIntegralPitch_old, MeanIntegralRoll_old;
-
 
863
 
789
                        IntegralAccPitch = 0;
864
                // if not lopping in any direction (this should be alwais the case,
Line 790... Line 865...
790
                        IntegralAccRoll = 0;
865
                // because the Measurement counter is reset to 0 if looping in any direction is active.)
791
 
866
                if(!Looping_Pitch && !Looping_Roll)
792
                        MeanIntegralPitch = 0;
867
                {
793
                        MeanIntegralRoll = 0;
868
                        // Calculate mean value of the gyro integrals
794
 
-
 
795
                        Reading_IntegralGyroPitch2 = Reading_IntegralGyroPitch;
-
 
796
                        Reading_IntegralGyroRoll2 = Reading_IntegralGyroRoll;
-
 
797
 
-
 
798
                        AttitudeCorrectionPitch = 0;
-
 
799
                        AttitudeCorrectionRoll = 0;
869
                        MeanIntegralPitch /= BALANCE_NUMBER;
Line 800... Line 870...
800
                }
870
                        MeanIntegralRoll  /= BALANCE_NUMBER;
801
 
871
 
802
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
872
                        // Calculate mean of the acceleration values
803
                if(!Looping_Pitch && !Looping_Roll) // if not lopping in any direction
873
                        IntegralAccPitch = (ParamSet.GyroAccFaktor * IntegralAccPitch) / BALANCE_NUMBER;
804
                {
874
                        IntegralAccRoll  = (ParamSet.GyroAccFaktor * IntegralAccRoll ) / BALANCE_NUMBER;
805
                        int32_t tmp_long, tmp_long2;
875
 
806
                        // determine the deviation of gyro integral from averaged acceleration sensor
876
                        // Pitch ++++++++++++++++++++++++++++++++++++++++++++++++
807
                        tmp_long   =  (int32_t)(IntegralPitch / ParamSet.GyroAccFaktor - (int32_t)Mean_AccPitch);
877
                        // Calculate deviation of the averaged gyro integral and the averaged acceleration integral
808
                        tmp_long  /= 16;
878
                        IntegralErrorPitch = (int32_t)(MeanIntegralPitch - (int32_t)IntegralAccPitch);
-
 
879
                        CorrectionPitch = IntegralErrorPitch / ParamSet.GyroAccTrim;
809
                        tmp_long2  = (int32_t)(IntegralRoll   / ParamSet.GyroAccFaktor - (int32_t)Mean_AccRoll);
880
                        AttitudeCorrectionPitch = CorrectionPitch / BALANCE_NUMBER;
810
                        tmp_long2 /= 16;
881
                        // Roll ++++++++++++++++++++++++++++++++++++++++++++++++
811
 
882
                        // Calculate deviation of the averaged gyro integral and the averaged acceleration integral
812
                        if((MaxStickPitch > 15) || (MaxStickRoll > 15)) // reduce effect during stick commands
883
                        IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll);
813
                        {
-
 
814
                                tmp_long  /= 3;
884
                        CorrectionRoll  = IntegralErrorRoll / ParamSet.GyroAccTrim;
815
                                tmp_long2 /= 3;
885
                        AttitudeCorrectionRoll  = CorrectionRoll  / BALANCE_NUMBER;
816
                        }
886
 
-
 
887
                        if((MaxStickPitch > 15) || (MaxStickRoll > 15) || (MaxStickYaw > 25))
-
 
888
                        {
-
 
889
                                AttitudeCorrectionPitch /= 2;
817
                        if(MaxStickYaw > 25) // reduce further is yaw stick is active
890
                                AttitudeCorrectionRoll /= 2;
-
 
891
                        }
-
 
892
 
-
 
893
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
818
                        {
894
        // Gyro-Drift ermitteln
819
                                tmp_long  /= 3;
895
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
820
                                tmp_long2 /= 3;
896
                        // deviation of gyro pitch integral (IntegralPitch is corrected by averaged acc sensor)
-
 
897
                        IntegralErrorPitch  = IntegralPitch2 - IntegralPitch;
821
                        }
898
                        Reading_IntegralGyroPitch2 -= IntegralErrorPitch;
822
 
-
 
823
                        #define BALANCE 32
899
                        // deviation of gyro pitch integral (IntegralPitch is corrected by averaged acc sensor)
824
                        // limit correction effect
-
 
825
                        if(tmp_long >  BALANCE)  tmp_long  = BALANCE;
-
 
826
                        if(tmp_long < -BALANCE)  tmp_long  =-BALANCE;
-
 
827
                        if(tmp_long2 > BALANCE)  tmp_long2 = BALANCE;
-
 
828
                        if(tmp_long2 <-BALANCE)  tmp_long2 =-BALANCE;
-
 
829
                        // correct current readings
-
 
830
                        Reading_IntegralGyroPitch -= tmp_long;
-
 
831
                        Reading_IntegralGyroRoll -= tmp_long2;
-
 
832
                }
-
 
833
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
834
        // MeasurementCounter is incremented in the isr of analog.c
-
 
835
                if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached
-
 
836
                {
-
 
837
                        static int16_t cnt = 0;
-
 
838
                        static int8_t last_n_p, last_n_n, last_r_p, last_r_n;
-
 
839
                        static int32_t MeanIntegralPitch_old, MeanIntegralRoll_old;
-
 
840
 
-
 
841
                        // if not lopping in any direction (this should be alwais the case,
-
 
842
                        // because the Measurement counter is reset to 0 if looping in any direction is active.)
900
                        IntegralErrorRoll = IntegralRoll2 - IntegralRoll;
843
                        if(!Looping_Pitch && !Looping_Roll)
-
 
844
                        {
-
 
845
                                // Calculate mean value of the gyro integrals
-
 
846
                                MeanIntegralPitch /= BALANCE_NUMBER;
-
 
847
                                MeanIntegralRoll  /= BALANCE_NUMBER;
-
 
848
 
-
 
849
                                // Calculate mean of the acceleration values
-
 
850
                                IntegralAccPitch = (ParamSet.GyroAccFaktor * IntegralAccPitch) / BALANCE_NUMBER;
-
 
851
                                IntegralAccRoll  = (ParamSet.GyroAccFaktor * IntegralAccRoll ) / BALANCE_NUMBER;
-
 
852
 
-
 
853
                                // Pitch ++++++++++++++++++++++++++++++++++++++++++++++++
-
 
854
                                // Calculate deviation of the averaged gyro integral and the averaged acceleration integral
-
 
855
                                IntegralErrorPitch = (int32_t)(MeanIntegralPitch - (int32_t)IntegralAccPitch);
-
 
856
                                CorrectionPitch = IntegralErrorPitch / ParamSet.GyroAccTrim;
-
 
857
                                AttitudeCorrectionPitch = CorrectionPitch / BALANCE_NUMBER;
-
 
858
                                // Roll ++++++++++++++++++++++++++++++++++++++++++++++++
-
 
859
                                // Calculate deviation of the averaged gyro integral and the averaged acceleration integral
-
 
860
                                IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll);
-
 
861
                                CorrectionRoll  = IntegralErrorRoll / ParamSet.GyroAccTrim;
-
 
862
                                AttitudeCorrectionRoll  = CorrectionRoll  / BALANCE_NUMBER;
-
 
863
 
-
 
864
                                if((MaxStickPitch > 15) || (MaxStickRoll > 15) || (MaxStickYaw > 25))
-
 
865
                                {
-
 
866
                                        AttitudeCorrectionPitch /= 2;
-
 
867
                                        AttitudeCorrectionRoll /= 2;
-
 
868
                                }
-
 
869
 
-
 
870
                // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
871
                // Gyro-Drift ermitteln
-
 
872
                // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
873
                                // deviation of gyro pitch integral (IntegralPitch is corrected by averaged acc sensor)
901
                        Reading_IntegralGyroRoll2 -= IntegralErrorRoll;
874
                                IntegralErrorPitch  = IntegralPitch2 - IntegralPitch;
-
 
875
                                Reading_IntegralGyroPitch2 -= IntegralErrorPitch;
-
 
876
                                // deviation of gyro pitch integral (IntegralPitch is corrected by averaged acc sensor)
-
 
877
                                IntegralErrorRoll = IntegralRoll2 - IntegralRoll;
902
 
878
                                Reading_IntegralGyroRoll2 -= IntegralErrorRoll;
-
 
879
 
-
 
880
 
903
 
881
                                DebugOut.Analog[17] = IntegralAccPitch / 26;
904
                        DebugOut.Analog[17] = IntegralAccPitch / 26;
882
                                DebugOut.Analog[18] = IntegralAccRoll / 26;
905
                        DebugOut.Analog[18] = IntegralAccRoll / 26;
883
                                DebugOut.Analog[19] = IntegralErrorPitch;// / 26;
906
                        DebugOut.Analog[19] = IntegralErrorPitch;// / 26;
884
                                DebugOut.Analog[20] = IntegralErrorRoll;// / 26;
-
 
885
                                DebugOut.Analog[21] = MeanIntegralPitch / 26;
-
 
886
                                DebugOut.Analog[22] = MeanIntegralRoll / 26;
907
                        DebugOut.Analog[20] = IntegralErrorRoll;// / 26;
-
 
908
                        DebugOut.Analog[21] = MeanIntegralPitch / 26;
-
 
909
                        DebugOut.Analog[22] = MeanIntegralRoll / 26;
887
                                //DebugOut.Analog[28] = CorrectionPitch;
910
                        //DebugOut.Analog[28] = CorrectionPitch;
888
                                DebugOut.Analog[29] = CorrectionRoll;
911
                        DebugOut.Analog[29] = CorrectionRoll;
-
 
912
                        DebugOut.Analog[30] = AttitudeCorrectionRoll * 10;
-
 
913
 
889
                                DebugOut.Analog[30] = AttitudeCorrectionRoll * 10;
914
                        #define ERROR_LIMIT  (BALANCE_NUMBER * 4)
890
 
-
 
891
                                #define ERROR_LIMIT  (BALANCE_NUMBER * 4)
-
 
892
                                #define ERROR_LIMIT2 (BALANCE_NUMBER * 16)
915
                        #define ERROR_LIMIT2 (BALANCE_NUMBER * 16)
893
                                #define MOVEMENT_LIMIT 20000
916
                        #define MOVEMENT_LIMIT 20000
894
                // Pitch +++++++++++++++++++++++++++++++++++++++++++++++++
917
        // Pitch +++++++++++++++++++++++++++++++++++++++++++++++++
895
                                cnt = 1;// + labs(IntegralErrorPitch) / 4096;
918
                        cnt = 1;// + labs(IntegralErrorPitch) / 4096;
896
                                CorrectionPitch = 0;
-
 
897
                                if(labs(MeanIntegralPitch_old - MeanIntegralPitch) < MOVEMENT_LIMIT)
-
 
898
                                {
919
                        CorrectionPitch = 0;
899
                                        if(IntegralErrorPitch >  ERROR_LIMIT2)
920
                        if(labs(MeanIntegralPitch_old - MeanIntegralPitch) < MOVEMENT_LIMIT)
900
                                        {
921
                        {
-
 
922
                                if(IntegralErrorPitch >  ERROR_LIMIT2)
-
 
923
                                {
901
                                                if(last_n_p)
924
                                        if(last_n_p)
902
                                                {
925
                                        {
903
                                                        cnt += labs(IntegralErrorPitch) / ERROR_LIMIT2;
926
                                                cnt += labs(IntegralErrorPitch) / ERROR_LIMIT2;
904
                                                        CorrectionPitch = IntegralErrorPitch / 8;
927
                                                CorrectionPitch = IntegralErrorPitch / 8;
905
                                                        if(CorrectionPitch > 5000) CorrectionPitch = 5000;
928
                                                if(CorrectionPitch > 5000) CorrectionPitch = 5000;
906
                                                        AttitudeCorrectionPitch += CorrectionPitch / BALANCE_NUMBER;
929
                                                AttitudeCorrectionPitch += CorrectionPitch / BALANCE_NUMBER;
907
                                                }
930
                                        }
908
                                                else last_n_p = 1;
931
                                        else last_n_p = 1;
909
                                        }
932
                                }
910
                                        else  last_n_p = 0;
933
                                else  last_n_p = 0;
-
 
934
                                if(IntegralErrorPitch < -ERROR_LIMIT2)
-
 
935
                                {
911
                                        if(IntegralErrorPitch < -ERROR_LIMIT2)
936
                                        if(last_n_n)
912
                                        {
937
                                        {
913
                                                if(last_n_n)
938
                                                cnt += labs(IntegralErrorPitch) / ERROR_LIMIT2;
914
                                                {
-
 
915
                                                        cnt += labs(IntegralErrorPitch) / ERROR_LIMIT2;
-
 
916
                                                        CorrectionPitch = IntegralErrorPitch / 8;
939
                                                CorrectionPitch = IntegralErrorPitch / 8;
917
                                                        if(CorrectionPitch < -5000) CorrectionPitch = -5000;
940
                                                if(CorrectionPitch < -5000) CorrectionPitch = -5000;
918
                                                        AttitudeCorrectionPitch += CorrectionPitch / BALANCE_NUMBER;
941
                                                AttitudeCorrectionPitch += CorrectionPitch / BALANCE_NUMBER;
919
                                                }
942
                                        }
920
                                                else last_n_n = 1;
-
 
921
                                        }
-
 
922
                                        else  last_n_n = 0;
943
                                        else last_n_n = 1;
-
 
944
                                }
-
 
945
                                else  last_n_n = 0;
923
                                }
946
                        }
924
                                else cnt = 0;
947
                        else cnt = 0;
-
 
948
                        if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
-
 
949
                        // correct Gyro Offsets
925
                                if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
950
                        if(IntegralErrorPitch >  ERROR_LIMIT)   AdNeutralPitch += cnt;
926
                                // correct Gyro Offsets
-
 
927
                                if(IntegralErrorPitch >  ERROR_LIMIT)   AdNeutralPitch += cnt;
-
 
928
                                if(IntegralErrorPitch < -ERROR_LIMIT)   AdNeutralPitch -= cnt;
951
                        if(IntegralErrorPitch < -ERROR_LIMIT)   AdNeutralPitch -= cnt;
929
 
952
 
930
                // Roll +++++++++++++++++++++++++++++++++++++++++++++++++
953
        // Roll +++++++++++++++++++++++++++++++++++++++++++++++++
931
                                cnt = 1;// + labs(IntegralErrorPitch) / 4096;
954
                        cnt = 1;// + labs(IntegralErrorPitch) / 4096;
932
                                CorrectionRoll = 0;
-
 
933
                                if(labs(MeanIntegralRoll_old - MeanIntegralRoll) < MOVEMENT_LIMIT)
-
 
934
                                {
955
                        CorrectionRoll = 0;
935
                                        if(IntegralErrorRoll >  ERROR_LIMIT2)
956
                        if(labs(MeanIntegralRoll_old - MeanIntegralRoll) < MOVEMENT_LIMIT)
936
                                        {
957
                        {
937
                                                if(last_r_p)
958
                                if(IntegralErrorRoll >  ERROR_LIMIT2)
938
                                                {
-
 
939
                                                        cnt += labs(IntegralErrorRoll) / ERROR_LIMIT2;
-
 
940
                                                        CorrectionRoll = IntegralErrorRoll / 8;
-
 
941
                                                        if(CorrectionRoll > 5000) CorrectionRoll = 5000;
-
 
942
                                                        AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER;
-
 
943
                                                }
-
 
944
                                                else last_r_p = 1;
-
 
945
                                        }
-
 
946
                                        else  last_r_p = 0;
-
 
947
                                        if(IntegralErrorRoll < -ERROR_LIMIT2)
-
 
948
                                        {
-
 
949
                                                if(last_r_n)
-
 
950
                                                {
-
 
951
                                                        cnt += labs(IntegralErrorRoll) / ERROR_LIMIT2;
959
                                {
-
 
960
                                        if(last_r_p)
-
 
961
                                        {
-
 
962
                                                cnt += labs(IntegralErrorRoll) / ERROR_LIMIT2;
-
 
963
                                                CorrectionRoll = IntegralErrorRoll / 8;
-
 
964
                                                if(CorrectionRoll > 5000) CorrectionRoll = 5000;
-
 
965
                                                AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER;
-
 
966
                                        }
-
 
967
                                        else last_r_p = 1;
-
 
968
                                }
-
 
969
                                else  last_r_p = 0;
-
 
970
                                if(IntegralErrorRoll < -ERROR_LIMIT2)
-
 
971
                                {
-
 
972
                                        if(last_r_n)
-
 
973
                                        {
-
 
974
                                                cnt += labs(IntegralErrorRoll) / ERROR_LIMIT2;
Line 952... Line 975...
952
                                                        CorrectionRoll = IntegralErrorRoll / 8;
975
                                                CorrectionRoll = IntegralErrorRoll / 8;
953
                                                        if(CorrectionRoll < -5000) CorrectionRoll = -5000;
976
                                                if(CorrectionRoll < -5000) CorrectionRoll = -5000;
954
                                                        AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER;
977
                                                AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER;
955
                                                }
978
                                        }
956
                                                else last_r_n = 1;
979
                                        else last_r_n = 1;
957
                                        }
980
                                }
958
                                        else  last_r_n = 0;
981
                                else  last_r_n = 0;
959
                                }
982
                        }
960
                                else cnt = 0;
983
                        else cnt = 0;
961
                                // correct Gyro Offsets
984
                        // correct Gyro Offsets
962
                                if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
985
                        if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
963
                                if(IntegralErrorRoll >  ERROR_LIMIT)   AdNeutralRoll += cnt;
986
                        if(IntegralErrorRoll >  ERROR_LIMIT)   AdNeutralRoll += cnt;
964
                                if(IntegralErrorRoll < -ERROR_LIMIT)   AdNeutralRoll -= cnt;
987
                        if(IntegralErrorRoll < -ERROR_LIMIT)   AdNeutralRoll -= cnt;
965
 
988
 
966
                                DebugOut.Analog[27] = CorrectionRoll;
989
                        DebugOut.Analog[27] = CorrectionRoll;
967
                                DebugOut.Analog[23] = AdNeutralPitch;//10*(AdNeutralPitch - StartNeutralPitch);
990
                        DebugOut.Analog[23] = AdNeutralPitch;//10*(AdNeutralPitch - StartNeutralPitch);
968
                                DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
991
                        DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
Line 969... Line 992...
969
                        }
992
                }
970
                        else // looping is active
993
                else // looping is active
971
                        {
994
                {
972
                                AttitudeCorrectionRoll  = 0;
995
                        AttitudeCorrectionRoll  = 0;
973
                                AttitudeCorrectionPitch = 0;
996
                        AttitudeCorrectionPitch = 0;
974
                        }
997
                }
975
 
998
 
976
                        // if Gyro_I_Faktor == 0 , for example at Heading Hold, ignore attitude correction
999
                // if Gyro_I_Faktor == 0 , for example at Heading Hold, ignore attitude correction
977
                        if(!Gyro_I_Factor)
1000
                if(!Gyro_I_Factor)
978
                        {
1001
                {
979
                                AttitudeCorrectionRoll  = 0;
1002
                        AttitudeCorrectionRoll  = 0;
980
                                AttitudeCorrectionPitch = 0;
1003
                        AttitudeCorrectionPitch = 0;
981
                        }
1004
                }
982
                // +++++++++++++++++++++++++++++++++++++++++++++++++++++
1005
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++
983
                        MeanIntegralPitch_old = MeanIntegralPitch;
1006
                MeanIntegralPitch_old = MeanIntegralPitch;
Line 984... Line 1007...
984
                        MeanIntegralRoll_old  = MeanIntegralRoll;
1007
                MeanIntegralRoll_old  = MeanIntegralRoll;
985
                // +++++++++++++++++++++++++++++++++++++++++++++++++++++
1008
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++
986
                        // reset variables used for averaging
1009
                // reset variables used for averaging
987
                        IntegralAccPitch = 0;
1010
                IntegralAccPitch = 0;
988
                        IntegralAccRoll = 0;
1011
                IntegralAccRoll = 0;
989
                        MeanIntegralPitch = 0;
1012
                MeanIntegralPitch = 0;
990
                        MeanIntegralRoll = 0;
1013
                MeanIntegralRoll = 0;
991
                        MeasurementCounter = 0;
-
 
992
                } // end of averaging
-
 
993
 
-
 
994
 
-
 
995
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
996
//  Yawing
-
 
997
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
998
                if(MaxStickYaw > 20) // yaw stick is activated
-
 
999
                {   // if not fixed compass course is set update compass course
-
 
1000
                        if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) StoreNewCompassCourse = 1;
-
 
1001
                }
-
 
Line 1002... Line 1014...
1002
                // exponential stick sensitivity in yawring rate
1014
                MeasurementCounter = 0;
1003
                tmp_int  = (int32_t) ParamSet.Yaw_P * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo  y = ax + bx²
1015
        } // end of averaging
1004
                tmp_int += (ParamSet.Yaw_P * StickYaw) / 4;
1016
 
1005
                SetPointYaw = tmp_int;
1017
 
-
 
1018
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1019
//  Yawing
1006
                Reading_IntegralGyroYaw -= tmp_int;
1020
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1007
                // limit the effect
1021
        if(MaxStickYaw > 20) // yaw stick is activated
1008
                if(Reading_IntegralGyroYaw > 50000) Reading_IntegralGyroYaw = 50000;
1022
        {   // if not fixed compass course is set update compass course
1009
                if(Reading_IntegralGyroYaw <-50000) Reading_IntegralGyroYaw =-50000;
1023
                if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) StoreNewCompassCourse = 1;
Line 1010... Line -...
1010
 
-
 
1011
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1012
//  Compass
-
 
1013
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1014
                if(ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE)
-
 
1015
                {
-
 
1016
                        int16_t w,v;
-
 
1017
                        static uint8_t updCompass = 0;
1024
        }
1018
 
-
 
1019
                        if (!updCompass--)
-
 
1020
                        {
-
 
1021
                                updCompass = 49; // update only at 2ms*50 = 100ms (10Hz)
-
 
1022
                                // get current compass heading (angule between MK head and magnetic north)
-
 
1023
                                #ifdef USE_MM3
1025
        // exponential stick sensitivity in yawring rate
1024
                                CompassHeading = MM3_Heading();
1026
        tmp_int  = (int32_t) ParamSet.Yaw_P * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo  y = ax + bx²
-
 
1027
        tmp_int += (ParamSet.Yaw_P * StickYaw) / 4;
1025
                                #endif
1028
        SetPointYaw = tmp_int;
-
 
1029
        Reading_IntegralGyroYaw -= tmp_int;
-
 
1030
        // limit the effect
1026
                                #ifdef USE_CMPS03
1031
        if(Reading_IntegralGyroYaw > 50000) Reading_IntegralGyroYaw = 50000;
-
 
1032
        if(Reading_IntegralGyroYaw <-50000) Reading_IntegralGyroYaw =-50000;
1027
                                CompassHeading = CMPS03_Heading();
1033
 
1028
                                #endif
1034
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1035
//  Compass
-
 
1036
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1029
 
1037
        if(ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE)
1030
                                if (CompassHeading < 0) // no compass data available
1038
        {
1031
                                {
1039
                int16_t w,v;
1032
                                        CompassOffCourse = 0;
1040
                static uint8_t updCompass = 0;
1033
                                        if(!BeepTime) BeepTime = 100; // make noise at 10 Hz to signal the compass problem
1041
 
1034
                                }
-
 
1035
                                else // calculate OffCourse (angular deviation from heading to course)
1042
                if (!updCompass--)
1036
                                CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
-
 
-
 
1043
                {
-
 
1044
                        updCompass = 49; // update only at 2ms*50 = 100ms (10Hz)
-
 
1045
                        // get current compass heading (angule between MK head and magnetic north)
1037
                        }
1046
                        #ifdef USE_MM3
1038
 
1047
                        CompassHeading = MM3_Heading();
1039
                        // reduce compass effect with increasing declination
1048
                        #endif
-
 
1049
                        #ifdef USE_CMPS03
-
 
1050
                        CompassHeading = CMPS03_Heading();
-
 
1051
                        #endif
-
 
1052
 
-
 
1053
                        if (CompassHeading < 0) // no compass data available
-
 
1054
                        {
-
 
1055
                                CompassOffCourse = 0;
-
 
1056
                                if(!BeepTime) BeepTime = 100; // make noise at 10 Hz to signal the compass problem
-
 
1057
                        }
-
 
1058
                        else // calculate OffCourse (angular deviation from heading to course)
-
 
1059
                        CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
-
 
1060
                }
-
 
1061
 
-
 
1062
                // reduce compass effect with increasing declination
-
 
1063
                w = abs(IntegralPitch / 512);
Line 1040... Line 1064...
1040
                        w = abs(IntegralPitch / 512);
1064
                v = abs(IntegralRoll  / 512);
1041
                        v = abs(IntegralRoll  / 512);
1065
                if(v > w) w = v; // get maximum declination
1042
                        if(v > w) w = v; // get maximum declination
1066
                // if declination is small enough update compass course if neccessary
1043
                        // if declination is small enough update compass course if neccessary
1067
                if(w < 35 && StoreNewCompassCourse)
1044
                        if(w < 35 && StoreNewCompassCourse)
1068
                {
1045
                        {
1069
                        CompassCourse = CompassHeading;
1046
                                CompassCourse = CompassHeading;
1070
                        StoreNewCompassCourse = 0;
1047
                                StoreNewCompassCourse = 0;
1071
                }
1048
                        }
1072
                w = (w * FCParam.CompassYawEffect) / 64;  // scale to parameter
1049
                        w = (w * FCParam.CompassYawEffect) / 64;  // scale to parameter
1073
                w = FCParam.CompassYawEffect - w; // reduce compass effect with increasing declination
1050
                        w = FCParam.CompassYawEffect - w; // reduce compass effect with increasing declination
1074
                if(w > 0) // if there is any compass effect (avoid negative compass feedback)
1051
                        if(w > 0) // if there is any compass effect (avoid negative compass feedback)
1075
                {
1052
                        {
1076
                        Reading_IntegralGyroYaw += (CompassOffCourse * w) / 32;
1053
                                Reading_IntegralGyroYaw += (CompassOffCourse * w) / 32;
1077
                }
1054
                        }
1078
        }
1055
                }
1079
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1056
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1080
//  GPS
1057
//  GPS
1081
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1058
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1082
        if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE)
1059
                if((ParamSet.GlobalConfig & CFG_GPS_ACTIVE) && !EmergencyLanding)
1083
        {
1060
                {
1084
                GPS_P_Factor = FCParam.UserParam5;
1061
                        GPS_P_Factor = FCParam.UserParam5;
1085
                GPS_D_Factor = FCParam.UserParam6;
1062
                        GPS_D_Factor = FCParam.UserParam6;
1086
                if(EmergencyLanding) GPS_Main(230); // enables Comming Home
1063
                        GPS_Main(); // updates GPS_Pitch and GPS_Roll on new GPS data
1087
                else GPS_Main(Poti3);
1064
                }
1088
        }
1065
                else
1089
        else
1066
                {
1090
        {
1067
                        GPS_Neutral();
1091
                GPS_Neutral();
1068
                }
1092
        }
1069
 
1093
 
1070
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1094
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1071
//  Debugwerte zuordnen
1095
//  Debugwerte zuordnen
1072
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1096
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1073
                if(!TimerDebugOut--)
1097
        if(!TimerDebugOut--)
1074
                {
1098
        {
1075
                        TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz)
1099
                TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz)
1076
                        DebugOut.Analog[0]  = IntegralPitch / ParamSet.GyroAccFaktor;
1100
                DebugOut.Analog[0]  = IntegralPitch / ParamSet.GyroAccFaktor;
1077
                        DebugOut.Analog[1]  = IntegralRoll / ParamSet.GyroAccFaktor;
1101
                DebugOut.Analog[1]  = IntegralRoll / ParamSet.GyroAccFaktor;
1078
                        DebugOut.Analog[2]  = Mean_AccPitch;
1102
                DebugOut.Analog[2]  = Mean_AccPitch;
1079
                        DebugOut.Analog[3]  = Mean_AccRoll;
1103
                DebugOut.Analog[3]  = Mean_AccRoll;
1080
                        DebugOut.Analog[4]  = Reading_GyroYaw;
1104
                DebugOut.Analog[4]  = Reading_GyroYaw;
1081
                        DebugOut.Analog[5]  = ReadingHeight;
1105
                DebugOut.Analog[5]  = ReadingHeight;
Line 1082... Line 1106...
1082
                        DebugOut.Analog[6]  = (Reading_Integral_Top / 512);
1106
                DebugOut.Analog[6]  = (Reading_Integral_Top / 512);
1083
                        DebugOut.Analog[8]  = CompassHeading;
1107
                DebugOut.Analog[8]  = CompassHeading;
1084
                        DebugOut.Analog[9]  = UBat;
1108
                DebugOut.Analog[9]  = UBat;
Line 1085... Line 1109...
1085
                        DebugOut.Analog[10] = SenderOkay;
1109
                DebugOut.Analog[10] = SenderOkay;
1086
                        DebugOut.Analog[16] = Mean_AccTop;
1110
                DebugOut.Analog[16] = Mean_AccTop;
1087
 
1111
 
1088
                        /*    DebugOut.Analog[16] = motor_rx[0];
1112
                /*    DebugOut.Analog[16] = motor_rx[0];
1089
                        DebugOut.Analog[17] = motor_rx[1];
1113
                DebugOut.Analog[17] = motor_rx[1];
1090
                        DebugOut.Analog[18] = motor_rx[2];
1114
                DebugOut.Analog[18] = motor_rx[2];
1091
                        DebugOut.Analog[19] = motor_rx[3];
1115
                DebugOut.Analog[19] = motor_rx[3];
1092
                        DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
1116
                DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
1093
                        DebugOut.Analog[20] /= 14;
1117
                DebugOut.Analog[20] /= 14;
1094
                        DebugOut.Analog[21] = motor_rx[4];
1118
                DebugOut.Analog[21] = motor_rx[4];
1095
                        DebugOut.Analog[22] = motor_rx[5];
1119
                DebugOut.Analog[22] = motor_rx[5];
1096
                        DebugOut.Analog[23] = motor_rx[6];
1120
                DebugOut.Analog[23] = motor_rx[6];
1097
                        DebugOut.Analog[24] = motor_rx[7];
1121
                DebugOut.Analog[24] = motor_rx[7];
1098
                        DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
1122
                DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
1099
 
1123
 
1100
                        DebugOut.Analog[9]  = Reading_GyroPitch;
1124
                DebugOut.Analog[9]  = Reading_GyroPitch;
1101
                        DebugOut.Analog[9]  = SetPointHeight;
1125
                DebugOut.Analog[9]  = SetPointHeight;
1102
                        DebugOut.Analog[10] = Reading_IntegralGyroYaw / 128;
1126
                DebugOut.Analog[10] = Reading_IntegralGyroYaw / 128;
Line 1103... Line 1127...
1103
                        DebugOut.Analog[11] = CompassCourse;
1127
                DebugOut.Analog[11] = CompassCourse;
1104
                        DebugOut.Analog[10] = FCParam.Gyro_I;
1128
                DebugOut.Analog[10] = FCParam.Gyro_I;
1105
                        DebugOut.Analog[10] = ParamSet.Gyro_I;
1129
                DebugOut.Analog[10] = ParamSet.Gyro_I;
1106
                        DebugOut.Analog[9]  = CompassOffCourse;
1130
                DebugOut.Analog[9]  = CompassOffCourse;
1107
                        DebugOut.Analog[10] = ThrustMixFraction;
1131
                DebugOut.Analog[10] = ThrustMixFraction;
1108
                        DebugOut.Analog[3]  = HeightD * 32;
1132
                DebugOut.Analog[3]  = HeightD * 32;
1109
                        DebugOut.Analog[4]  = HeightControlThrust;
1133
                DebugOut.Analog[4]  = HeightControlThrust;
1110
                        */
1134
                */
1111
                }
1135
        }
1112
 
1136
 
1113
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1137
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1114
//  calculate control feedback from angle (gyro integral) and agular velocity (gyro signal)
1138
//  calculate control feedback from angle (gyro integral) and agular velocity (gyro signal)
1115
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1116
 
-
 
1117
                if(Looping_Pitch) Reading_GyroPitch = Reading_GyroPitch * Gyro_P_Factor;
-
 
1118
                else Reading_GyroPitch = IntegralPitch * Gyro_I_Factor + Reading_GyroPitch * Gyro_P_Factor;
-
 
1119
                if(Looping_Roll) Reading_GyroRoll = Reading_GyroRoll * Gyro_P_Factor;
-
 
1120
                else Reading_GyroRoll = IntegralRoll * Gyro_I_Factor + Reading_GyroRoll * Gyro_P_Factor;
-
 
1121
                Reading_GyroYaw = Reading_GyroYaw * (2 * Gyro_P_Factor) + IntegralYaw * Gyro_I_Factor / 2;
-
 
1122
 
1139
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1123
                DebugOut.Analog[25] = IntegralRoll * Gyro_I_Factor;
1140
 
1124
                DebugOut.Analog[31] = StickRoll;// / (26*Gyro_I_Factor);
1141
        if(Looping_Pitch) Reading_GyroPitch = Reading_GyroPitch * Gyro_P_Factor;
1125
                DebugOut.Analog[28] = Reading_GyroRoll;
1142
        else Reading_GyroPitch = IntegralPitch * Gyro_I_Factor + Reading_GyroPitch * Gyro_P_Factor;
-
 
1143
        if(Looping_Roll) Reading_GyroRoll = Reading_GyroRoll * Gyro_P_Factor;
-
 
1144
        else Reading_GyroRoll = IntegralRoll * Gyro_I_Factor + Reading_GyroRoll * Gyro_P_Factor;
-
 
1145
        Reading_GyroYaw = Reading_GyroYaw * (2 * Gyro_P_Factor) + IntegralYaw * Gyro_I_Factor / 2;
-
 
1146
 
-
 
1147
        DebugOut.Analog[25] = IntegralRoll * Gyro_I_Factor;
-
 
1148
        DebugOut.Analog[31] = StickRoll;// / (26*Gyro_I_Factor);
-
 
1149
        DebugOut.Analog[28] = Reading_GyroRoll;
1126
 
1150
 
1127
                // limit control feedback
1151
        // limit control feedback
1128
                #define MAX_SENSOR  2048
1152
        #define MAX_SENSOR  2048
1129
                if(Reading_GyroPitch >  MAX_SENSOR) Reading_GyroPitch =  MAX_SENSOR;
1153
        if(Reading_GyroPitch >  MAX_SENSOR) Reading_GyroPitch =  MAX_SENSOR;
-
 
1154
        if(Reading_GyroPitch < -MAX_SENSOR) Reading_GyroPitch = -MAX_SENSOR;
-
 
1155
        if(Reading_GyroRoll  >  MAX_SENSOR) Reading_GyroRoll  =  MAX_SENSOR;
-
 
1156
        if(Reading_GyroRoll  < -MAX_SENSOR) Reading_GyroRoll  = -MAX_SENSOR;
-
 
1157
        if(Reading_GyroYaw   >  MAX_SENSOR) Reading_GyroYaw   =  MAX_SENSOR;
-
 
1158
        if(Reading_GyroYaw   < -MAX_SENSOR) Reading_GyroYaw   = -MAX_SENSOR;
-
 
1159
 
-
 
1160
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1161
// Height Control
-
 
1162
// The higth control algorithm reduces the thrust but does not increase the thrust.
-
 
1163
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1164
        // If hight control is activated and no emergency landing is active
-
 
1165
        if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) )
-
 
1166
        {
-
 
1167
                int tmp_int;
-
 
1168
                // if hight control is activated by an rc channel
1130
                if(Reading_GyroPitch < -MAX_SENSOR) Reading_GyroPitch = -MAX_SENSOR;
1169
                if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH)
1131
                if(Reading_GyroRoll  >  MAX_SENSOR) Reading_GyroRoll  =  MAX_SENSOR;
-
 
1132
                if(Reading_GyroRoll  < -MAX_SENSOR) Reading_GyroRoll  = -MAX_SENSOR;
-
 
1133
                if(Reading_GyroYaw   >  MAX_SENSOR) Reading_GyroYaw   =  MAX_SENSOR;
-
 
1134
                if(Reading_GyroYaw   < -MAX_SENSOR) Reading_GyroYaw   = -MAX_SENSOR;
-
 
1135
 
-
 
1136
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1137
// Height Control
-
 
1138
// The higth control algorithm reduces the thrust but does not increase the thrust.
-
 
1139
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1140
                // If hight control is activated and no emergency landing is active
-
 
1141
                if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) )
-
 
1142
                {
-
 
1143
                        int tmp_int;
-
 
1144
                        // if hight control is activated by an rc channel
-
 
1145
                        if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH)
-
 
1146
                        {       // check if parameter is less than activation threshold
1170
                {       // check if parameter is less than activation threshold
1147
                                if(FCParam.MaxHeight < 50)
1171
                        if(FCParam.MaxHeight < 50)
1148
                                {
1172
                        {
1149
                                        SetPointHeight = ReadingHeight - 20;  // update SetPoint with current reading
1173
                                SetPointHeight = ReadingHeight - 20;  // update SetPoint with current reading
1150
                                        HeightControlActive = 0; // disable hight control
1174
                                HeightControlActive = 0; // disable hight control
1151
                                }
1175
                        }
1152
                                else HeightControlActive = 1; // enable hight control
1176
                        else HeightControlActive = 1; // enable hight control
1153
                        }
-
 
1154
                        else // no switchable hight control
1177
                }
-
 
1178
                else // no switchable hight control
1155
                        {
1179
                {
1156
                                SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain - 20;
1180
                        SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain - 20;
1157
                                HeightControlActive = 1;
1181
                        HeightControlActive = 1;
1158
                        }
1182
                }
1159
                        // get current hight
1183
                // get current hight
1160
                        h = ReadingHeight;
1184
                h = ReadingHeight;
1161
                        // if current hight is above the setpoint reduce thrust
1185
                // if current hight is above the setpoint reduce thrust
1162
                        if((h > SetPointHeight) && HeightControlActive)
1186
                if((h > SetPointHeight) && HeightControlActive)
1163
                        {
1187
                {
1164
                                // hight difference -> P control part
1188
                        // hight difference -> P control part