Subversion Repositories FlightCtrl

Rev

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

Rev 701 Rev 726
Line 1... Line -...
1 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
2 
// + Copyright (c) 04.2007 Holger Buss
 - 
 
3 
// + only for non-profit use
 - 
 
4 
// + www.MikroKopter.com
 - 
 
5 
// + see the File "License.txt" for further Informations
 - 
 
6 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
7 
#include "main.h"
 1 
#include <inttypes.h>
8 
 
 - 
 
9 
signed int GPS_Pitch = 0;
 - 
 
10 
signed int GPS_Roll = 0;
 - 
 
11 
long GpsReading_X = 0;
 - 
 
12 
long GpsReading_Y = 0;
 - 
 
13 
long GpsTarget_X = 0;
 - 
 
14 
long GpsTarget_Y = 0;
 - 
 
Line -... Line 2...
- 
 
 2 
 
- 
 
 3 
#include "fc.h"
15 
 
 4 
#include "ubx.h"
16 
void GPS_Neutral(void)
 5 
#include "mymath.h"
17 
{
 6 
 
18 
 GpsTarget_X = GpsReading_X;
 7 
int16_t GPS_Pitch = 0;
19 
 GpsTarget_Y = GpsReading_Y;
 - 
 
Line -... Line 8...
- 
 
 8 
int16_t GPS_Roll = 0;
20 
}
 9 
 
- 
 
 10 
uint8_t GPS_P_Factor = 0;
- 
 
 11 
uint8_t GPS_D_Factor = 0;
21 
 
 12 
 
- 
 
 13 
typedef struct
22 
void GPS_CalcTargetDirection(void)
 14 
{
23 
{
 15 
        int32_t Northing;
24 
 GPS_Pitch = 0;
 16 
        int32_t Easting;
- 
 
 17 
        uint8_t Status;
Line -... Line 18...
- 
 
 18 
 
- 
 
 19 
} GPS_Pos_t;
- 
 
 20 
 
- 
 
 21 
// GPS coordinates for hold position
Line -... Line 22...
- 
 
 22 
GPS_Pos_t GPSHoldPoint  = {0,0, INVALID};
- 
 
 23 
// GPS coordinates for flying home
- 
 
 24 
GPS_Pos_t GPSHomePoint = {0,0, INVALID};;
- 
 
 25 
 
- 
 
 26 
 
- 
 
 27 
 
- 
 
 28 
// ---------------------------------------------------------------------------------
- 
 
 29 
 
- 
 
 30 
 
- 
 
 31 
void GPS_Main(void)
- 
 
 32 
{
- 
 
 33 
        int32_t coscompass, sincompass;
- 
 
 34 
        int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
- 
 
 35 
        int32_t PD_North = 0, PD_East = 0;
- 
 
 36 
        int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;
- 
 
 37 
 
- 
 
 38 
        // poti2 enables the gps feature
- 
 
 39 
        if(Poti2 > 70) // run GPS function only if Poti 2 is larger than 70 (switch on)
- 
 
 40 
        {
- 
 
 41 
                switch(GPSInfo.status)
- 
 
 42 
                {
- 
 
 43 
                case INVALID:  // invalid gps data
- 
 
 44 
                        GPS_Pitch = 0;
- 
 
 45 
                        GPS_Roll = 0;
- 
 
 46 
                        break;
- 
 
 47 
                case PROCESSED: // if gps data are already processed
- 
 
 48 
                        // downcount timeout
- 
 
 49 
                        if(GPSTimeout) GPSTimeout--;
- 
 
 50 
                        // if no new data arrived within timeout set current data invalid
- 
 
 51 
                        // and therefore disable GPS
- 
 
 52 
                        else
- 
 
 53 
                        {
- 
 
 54 
                                GPS_Pitch = 0;
- 
 
 55 
                                GPS_Roll = 0;
- 
 
 56 
                                GPSInfo.status = INVALID;
- 
 
 57 
                        }
- 
 
 58 
                        break;
- 
 
 59 
                case VALID: // new valid data from gps device
- 
 
 60 
                        // if the gps data quality is sufficient
- 
 
 61 
                        if (GPSInfo.satfix == SATFIX_3D)
- 
 
 62 
                        {
- 
 
 63 
                                // if sticks are centered  and hold position is valid enable position hold
- 
 
 64 
                                if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (GPSHoldPoint.Status == VALID))
- 
 
 65 
                                {
- 
 
 66 
                                        coscompass = (int32_t)c_cos_8192(CompassHeading);
- 
 
 67 
                                        sincompass = (int32_t)c_sin_8192(CompassHeading);
- 
 
 68 
 
- 
 
 69 
                                        // Calculate deviation from hold position
- 
 
 70 
                                        GPSPosDev_North = GPSInfo.utmnorth - GPSHoldPoint.Northing;
- 
 
 71 
                                        GPSPosDev_East  = GPSInfo.utmeast  - GPSHoldPoint.Easting;
- 
 
 72 
 
- 
 
 73 
                                        //Calculate PD-components of the controller
- 
 
 74 
                                        P_North = (GPS_P_Factor * GPSPosDev_North)/2048;
- 
 
 75 
                                        D_North = (GPS_D_Factor * GPSInfo.velnorth)/255;
- 
 
 76 
                                        P_East =  (GPS_P_Factor * GPSPosDev_East)/2048;
- 
 
 77 
                                        D_East =  (GPS_D_Factor * GPSInfo.veleast)/255;
- 
 
 78 
 
- 
 
 79 
                                        // PD-controller
- 
 
 80 
                                        PD_North = (-P_North + D_North);
- 
 
 81 
                                        PD_East = (P_East - D_East);
- 
 
 82 
 
- 
 
 83 
                                        // GPS to pitch and roll
- 
 
 84 
                                        GPS_Pitch = (int16_t)((coscompass * PD_North - sincompass * PD_East) / 8192);
- 
 
 85 
                                        GPS_Roll  = (int16_t)((sincompass * PD_North + coscompass * PD_East) / 8192);
- 
 
 86 
 
- 
 
 87 
                                        // limit GPS controls
- 
 
 88 
                                        if (GPS_Pitch >  35) GPS_Pitch =  35;
- 
 
 89 
                                        if (GPS_Pitch < -35) GPS_Pitch = -35;
- 
 
 90 
                                        if (GPS_Roll >  35) GPS_Roll =  35;
- 
 
 91 
                                        if (GPS_Roll < -35) GPS_Roll = -35;
- 
 
 92 
                                }
- 
 
 93 
                                else // MK controlled by user
- 
 
 94 
                                {
- 
 
 95 
                                        // update hold point to current gps position
- 
 
 96 
                                        GPSHoldPoint.Northing = GPSInfo.utmnorth;
- 
 
 97 
                                        GPSHoldPoint.Easting  = GPSInfo.utmeast;
- 
 
 98 
                                        GPSHoldPoint.Status = VALID;
- 
 
 99 
                                        // disable gps control
- 
 
 100 
                                        GPS_Pitch = 0;
- 
 
 101 
                                        GPS_Roll = 0;
- 
 
 102 
                                }
- 
 
 103 
                        } // eof 3D-FIX
- 
 
 104 
                        else // no 3D-SATFIX
- 
 
 105 
                        {       // diable gps control
- 
 
 106 
                                GPS_Pitch = 0;
- 
 
 107 
                                GPS_Roll = 0;
- 
 
 108 
                        }
- 
 
 109 
                        // set current data as processed to avoid further calculations on the same gps data
- 
 
 110 
                        GPSInfo.status = PROCESSED;