Subversion Repositories FlightCtrl

Rev

Rev 826 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 826 Rev 844
Line 1023... Line 1023...
1023
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1023
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1024
    // compass code is used if Compass option or GPS option is selected
1024
    // compass code is used if Compass option or GPS option is selected
1025
        if((ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE) || (ParamSet.GlobalConfig & CFG_GPS_ACTIVE))
1025
        if((ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE) || (ParamSet.GlobalConfig & CFG_GPS_ACTIVE))
1026
        {
1026
        {
1027
                static uint8_t updCompass = 0;
1027
                static uint8_t updCompass = 0;
-
 
1028
                int16_t w,v;
Line 1028... Line 1029...
1028
 
1029
 
1029
                if (!updCompass--)
1030
                if (!updCompass--)
1030
                {
1031
                {
1031
                        updCompass = 49; // update only at 2ms*50 = 100ms (10Hz)
1032
                        updCompass = 49; // update only at 2ms*50 = 100ms (10Hz)
1032
                        // get current compass heading (angule between MK head and magnetic north)
1033
                        // get current compass heading (angle between MK head and magnetic north)
1033
                        #ifdef USE_MM3
1034
                        #ifdef USE_MM3
1034
                        CompassHeading = MM3_Heading();
1035
                        CompassHeading = MM3_Heading();
1035
                        #endif
1036
                        #endif
1036
                        #ifdef USE_CMPS03
1037
                        #ifdef USE_CMPS03
1037
                        CompassHeading = CMPS03_Heading();
1038
                        CompassHeading = CMPS03_Heading();
1038
                        #endif
1039
                        #endif
1039
                        if (CompassHeading < 0) CompassOffCourse = 0; // disable gyro compass correction on bad compass data
1040
                        if (CompassHeading < 0) CompassOffCourse = 0; // disable gyro compass correction on bad compass data
1040
                        else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; // calc course deviation
1041
                        else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; // calc course deviation
Line -... Line 1042...
-
 
1042
                }
-
 
1043
 
1041
                }
1044
                // get maximum attitude angle
-
 
1045
                w = abs(IntegralPitch/512);
-
 
1046
                v = abs(IntegralRoll /512);
1042
 
1047
                if(v > w) w = v;
1043
                if ((MaxStickPitch < 75) && (MaxStickRoll < 75))
1048
                if (w < 25)
1044
                {
1049
                {
1045
                        if(UpdateCompassCourse)
1050
                        if(UpdateCompassCourse)
1046
                        {
1051
                        {
1047
                                UpdateCompassCourse = 0;
1052
                                UpdateCompassCourse = 0;
1048
                                CompassCourse = CompassHeading;
1053
                                CompassCourse = CompassHeading;
-
 
1054
                                CompassOffCourse = 0;
-
 
1055
                        }
1049
                                CompassOffCourse = 0;
1056
                        w = (w * FCParam.CompassYawEffect) / 64;
1050
                        }
1057
                w = FCParam.CompassYawEffect - w;
1051
                        Reading_IntegralGyroYaw += (CompassOffCourse * FCParam.CompassYawEffect) / 16;
1058
                        if(w > 0) Reading_IntegralGyroYaw += (CompassOffCourse * w) / 32;
1052
                }
1059
                }
1053
        }
1060
        }
1054
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1061
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 1058... Line 1065...
1058
        {
1065
        {
1059
                GPS_I_Factor = FCParam.UserParam2;
1066
                GPS_I_Factor = FCParam.UserParam2;
1060
                GPS_P_Factor = FCParam.UserParam5;
1067
                GPS_P_Factor = FCParam.UserParam5;
1061
                GPS_D_Factor = FCParam.UserParam6;
1068
                GPS_D_Factor = FCParam.UserParam6;
1062
                if(EmergencyLanding) GPS_Main(230); // enables Comming Home
1069
                if(EmergencyLanding) GPS_Main(230); // enables Comming Home
1063
                else GPS_Main(Poti3);
1070
                else GPS_Main(Poti3);               // behavior controlled by Poti3
1064
        }
1071
        }
1065
        else
1072
        else
1066
        {
1073
        {
1067
                GPS_Neutral();
1074
                GPS_Neutral();
1068
        }
1075
        }