Subversion Repositories FlightCtrl

Rev

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

Rev 716 Rev 740
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 "main.h"
Line 8... Line 2...
8 
 
 2 
 
9 
signed int GPS_Nick = 0;
 3 
int16_t GPS_Nick = 0;
10 
signed int GPS_Roll = 0;
 - 
 
11 
long GpsAktuell_X = 0;
 - 
 
12 
long GpsAktuell_Y = 0;
 - 
 
13 
long GpsZiel_X = 0;
 - 
 
14 
long GpsZiel_Y = 0;
 4 
int16_t GPS_Roll = 0;
15 
 
 5 
 
16 
void GPS_Neutral(void)
 - 
 
17 
{
 - 
 
18 
 GpsZiel_X = GpsAktuell_X;
 6 
uint8_t GPS_P_Factor = 0;
19 
 GpsZiel_Y = GpsAktuell_Y;
 - 
 
Line 20... Line 7...
20 
}
 7 
uint8_t GPS_D_Factor = 0;
- 
 
 8 
 
- 
 
 9 
uint8_t flashtimer;
21 
 
 10 
 
22 
void GPS_BerechneZielrichtung(void)
 11 
typedef struct
- 
 
 12 
{
23 
{
 13 
        int32_t Northing;
24 
 GPS_Nick = 0;
 14 
        int32_t Easting;
- 
 
 15 
        uint8_t Status;
Line -... Line 16...
- 
 
 16 
 
- 
 
 17 
} GPS_Pos_t;
Line -... Line 18...
- 
 
 18 
 
- 
 
 19 
// GPS coordinates for hold position
- 
 
 20 
GPS_Pos_t GPSHoldPoint  = {0,0, INVALID};
- 
 
 21 
 
- 
 
 22 
 
- 
 
 23 
 
- 
 
 24 
// ---------------------------------------------------------------------------------
- 
 
 25 
 
- 
 
 26 
 
- 
 
 27 
void GPS_Main(void)
- 
 
 28 
{
- 
 
 29 
        int32_t coscompass, sincompass;
- 
 
 30 
        int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
- 
 
 31 
        int32_t PD_North = 0, PD_East = 0;
- 
 
 32 
        int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;
- 
 
 33 
 
- 
 
 34 
        // poti2 enables the gps feature
- 
 
 35 
        if((Poti3 > 70) && (!Notlandung)) // run GPS function only if Poti 2 is larger than 70 (switch on)
- 
 
 36 
        {
- 
 
 37 
                switch(GPSInfo.status)
- 
 
 38 
                {
- 
 
 39 
                case INVALID:  // invalid gps data
- 
 
 40 
                        GPS_Nick = 0;
- 
 
 41 
                        GPS_Roll = 0;
- 
 
 42 
                        GRN_OFF;
- 
 
 43 
                        break;
- 
 
 44 
                case PROCESSED: // if gps data are already processed
- 
 
 45 
                        // downcount timeout
- 
 
 46 
                        if(GPSTimeout) GPSTimeout--;
- 
 
 47 
                        // if no new data arrived within timeout set current data invalid
- 
 
 48 
                        // and therefore disable GPS
- 
 
 49 
                        else
- 
 
 50 
                        {
- 
 
 51 
                                GPS_Nick = 0;
- 
 
 52 
                                GPS_Roll = 0;
- 
 
 53 
                                GPSInfo.status = INVALID;
- 
 
 54 
                        }
- 
 
 55 
                        break;
- 
 
 56 
                case VALID: // new valid data from gps device
- 
 
 57 
                        // if the gps data quality is sufficient
- 
 
 58 
                        if (GPSInfo.satfix == SATFIX_3D)
- 
 
 59 
                        {
- 
 
 60 
                                GRN_ON;
- 
 
 61 
                                // if sticks are centered  and hold position is valid enable position hold control
- 
 
 62 
                                if ((MaxStickNick < 20) && (MaxStickRoll < 20) && (GPSHoldPoint.Status == VALID))
- 
 
 63 
                                {
- 
 
 64 
                                        // Calculate deviation from hold position
- 
 
 65 
                                        GPSPosDev_North = GPSInfo.utmnorth - GPSHoldPoint.Northing;
- 
 
 66 
                                        GPSPosDev_East  = GPSInfo.utmeast  - GPSHoldPoint.Easting;
- 
 
 67 
 
- 
 
 68 
                                        //Calculate PD-components of the controller (negative sign for compensation)
- 
 
 69 
                                        P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
- 
 
 70 
                                        D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
- 
 
 71 
                                        P_East =  -(GPS_P_Factor * GPSPosDev_East)/2048;
- 
 
 72 
                                        D_East =  -(GPS_D_Factor * GPSInfo.veleast)/512;
- 
 
 73 
 
- 
 
 74 
                                        // PD-controller
- 
 
 75 
                                        PD_North = P_North + D_North;
- 
 
 76 
                                        PD_East  = P_East  + D_East;
- 
 
 77 
 
- 
 
 78 
                                        // GPS to pitch and roll settings
- 
 
 79 
 
- 
 
 80 
                                        //A positive pitch angle moves head downwards (flying forward).
- 
 
 81 
                                        //A positive roll angle tilts left side downwards (flying left).
- 
 
 82 
 
- 
 
 83 
                                        // If compass heading is 0 the head of the copter is in north direction.
- 
 
 84 
                                        // A positive pitch angle will fly to north and a positive roll angle will fly to west.
- 
 
 85 
                                        // In case of a positive north deviation/velocity the
- 
 
 86 
                                        // copter should fly to south (negative pitch).
- 
 
 87 
                                        // In case of a positive east position deviation and a positive east velocity the
- 
 
 88 
                                        // copter should fly to west (positive roll).
- 
 
 89 
 
- 
 
 90 
                                        // The influence of the GPS_Nick and GPS_Roll variable is contrarily to the stick values
- 
 
 91 
                                        // in the fc.c. Therefore a positive north deviation/velocity should result in a positive
- 
 
 92 
                                        // GPS_Nick and a positive east deviation/velocity should result in a negative GPS_Roll.
- 
 
 93 
 
- 
 
 94 
                                        // rotation transformation to compass heading to match any copter orientation
- 
 
 95 
                                        coscompass = (int32_t)cos_i(KompassValue);
- 
 
 96 
                                        sincompass = (int32_t)sin_i(KompassValue);
- 
 
 97 
 
- 
 
 98 
                                        GPS_Roll  =    (int16_t)((PD_East * coscompass - PD_North * sincompass) / 1024);
- 
 
 99 
                                        GPS_Nick = -1*(int16_t)((PD_East * sincompass + PD_North * coscompass) / 1024);
- 
 
 100 
 
- 
 
 101 
                                        // limit GPS controls
- 
 
 102 
                                        #define GPS_CTRL_LIMIT 35
- 
 
 103 
                                        if (GPS_Nick >  GPS_CTRL_LIMIT) GPS_Nick =  GPS_CTRL_LIMIT;
- 
 
 104 
                                        if (GPS_Nick < -GPS_CTRL_LIMIT) GPS_Nick = -GPS_CTRL_LIMIT;
- 
 
 105 
                                        if (GPS_Roll  >  GPS_CTRL_LIMIT)  GPS_Roll =  GPS_CTRL_LIMIT;
- 
 
 106 
                                        if (GPS_Roll  < -GPS_CTRL_LIMIT)  GPS_Roll = -GPS_CTRL_LIMIT;
- 
 
 107 
                                }
- 
 
 108 
                                else // MK controlled by user
- 
 
 109 
                                {
- 
 
 110 
                                        // update hold point to current gps position
- 
 
 111 
                                        GPSHoldPoint.Northing = GPSInfo.utmnorth;
- 
 
 112 
                                        GPSHoldPoint.Easting  = GPSInfo.utmeast;
- 
 
 113 
                                        GPSHoldPoint.Status = VALID;
- 
 
 114 
                                        // disable gps control
- 
 
 115 
                                        GPS_Nick = 0;
- 
 
 116 
                                        GPS_Roll = 0;
- 
 
 117 
                                }
- 
 
 118 
                        } // eof 3D-FIX
- 
 
 119 
                        else // no 3D-SATFIX
- 
 
 120 
                        {       // diable gps control
- 
 
 121 
                                GPS_Nick = 0;
- 
 
 122 
                                GPS_Roll = 0;
- 
 
 123 
                                if (!flashtimer--)
- 
 
 124 
                                {
- 
 
 125 
                                        GRN_FLASH;
- 
 
 126 
                                        flashtimer=5;
- 
 
 127 
                                }
- 
 
 128 
                        }
- 
 
 129 
                        // set current data as processed to avoid further calculations on the same gps data
- 
 
 130 
                        GPSInfo.status = PROCESSED;
- 
 
 131 
                        break;
- 
 
 132 
                } // eof GPSInfo.status