Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
726 killagreg 1
#include <inttypes.h>
764 killagreg 2
#include <stdlib.h>
726 killagreg 3
#include "fc.h"
4
#include "ubx.h"
5
#include "mymath.h"
727 killagreg 6
#include "timer0.h"
741 killagreg 7
#include "uart.h"
701 killagreg 8
 
741 killagreg 9
#define TSK_IDLE                0
10
#define TSK_HOLD                1
11
#define TSK_HOME                2
12
 
767 killagreg 13
#define GPS_STICK_SENSE         20              // must be at least in a range where 90% of the trimming does not switch of the GPS function
14
#define GPS_STICK_LIMIT         45              // limit of gps stick control to avoid critical flight attitudes
15
#define GPS_D_LIMIT_DIST        2000    // for position deviations grater than 500 cm (20m) the D-Part of the PD-Controller is limited
16
#define GPS_D_LIMIT                     50              // PD-Controntroller D-Limit.
741 killagreg 17
 
726 killagreg 18
int16_t GPS_Pitch = 0;
19
int16_t GPS_Roll = 0;
20
 
746 killagreg 21
int32_t GPS_P_Factor = 0, GPS_D_Factor = 0;
22
int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;
726 killagreg 23
 
746 killagreg 24
 
726 killagreg 25
typedef struct
1 ingob 26
{
726 killagreg 27
        int32_t Northing;
28
        int32_t Easting;
29
        uint8_t Status;
1 ingob 30
 
726 killagreg 31
} GPS_Pos_t;
32
 
33
// GPS coordinates for hold position
741 killagreg 34
GPS_Pos_t HoldPosition  = {0,0, INVALID};
35
// GPS coordinates for home position
36
GPS_Pos_t HomePosition  = {0,0, INVALID};
726 killagreg 37
 
38
 
39
// ---------------------------------------------------------------------------------
40
 
747 killagreg 41
// set home position to current hold positon
746 killagreg 42
void GPS_SetHomePosition(void)
43
{
44
                HomePosition.Northing = HoldPosition.Northing;
45
                HomePosition.Easting = HoldPosition.Easting;
46
                HomePosition.Status = HoldPosition.Status;
47
                if(HomePosition.Status == VALID) BeepTime = 1000; // signal if new home position was set
48
}
726 killagreg 49
 
762 killagreg 50
// clear home position
51
void GPS_ClearHomePosition(void)
52
{
53
                HomePosition.Status = INVALID;
54
}
746 killagreg 55
 
747 killagreg 56
// disable GPS control sticks
746 killagreg 57
void GPS_Neutral(void)
1 ingob 58
{
746 killagreg 59
        GPS_Pitch = 0;
60
        GPS_Roll = 0;
61
}
62
 
762 killagreg 63
// calculates the GPS control sticks values from the deviation to target position
64
void GPS_PDController(GPS_Pos_t *TargetPos)
746 killagreg 65
{
726 killagreg 66
        int32_t coscompass, sincompass;
67
        int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
68
        int32_t PD_North = 0, PD_East = 0;
746 killagreg 69
 
762 killagreg 70
        if( (TargetPos->Status == VALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D))
71
        {
72
                GPSPosDev_North = GPSInfo.utmnorth - TargetPos->Northing;
73
                GPSPosDev_East  = GPSInfo.utmeast  - TargetPos->Easting;
746 killagreg 74
 
762 killagreg 75
                DebugOut.Analog[12] = GPSPosDev_North;
76
                DebugOut.Analog[13] = GPSPosDev_East;
746 killagreg 77
 
767 killagreg 78
                //DebugOut.Analog[12] = GPSInfo.velnorth;
79
                //DebugOut.Analog[13] = GPSInfo.veleast;
80
 
762 killagreg 81
                //Calculate PD-components of the controller (negative sign for compensation)
82
                P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
83
                D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
84
                P_East =  -(GPS_P_Factor * GPSPosDev_East)/2048;
85
                D_East =  -(GPS_D_Factor * GPSInfo.veleast)/512;
746 killagreg 86
 
764 killagreg 87
                if( (abs(GPSPosDev_North) > GPS_D_LIMIT_DIST) ||  (abs(GPSPosDev_East) > GPS_D_LIMIT_DIST) )
88
                {
89
                        if (D_North > GPS_D_LIMIT) D_North = GPS_D_LIMIT;
90
                        else if (D_North < -GPS_D_LIMIT) D_North = -GPS_D_LIMIT;
91
                        if (D_East > GPS_D_LIMIT)  D_East  = GPS_D_LIMIT;
92
                        else if (D_East  < -GPS_D_LIMIT) D_East  = -GPS_D_LIMIT;
93
                }
94
 
762 killagreg 95
                // PD-controller
96
                PD_North = P_North + D_North;
97
                PD_East  = P_East  + D_East;
746 killagreg 98
 
762 killagreg 99
                // GPS to pitch and roll settings
746 killagreg 100
 
762 killagreg 101
                //A positive pitch angle moves head downwards (flying forward).
102
                //A positive roll angle tilts left side downwards (flying left).
103
 
104
                // If compass heading is 0 the head of the copter is in north direction.
105
                // A positive pitch angle will fly to north and a positive roll angle will fly to west.
106
                // In case of a positive north deviation/velocity the
107
                // copter should fly to south (negative pitch).
108
                // In case of a positive east position deviation and a positive east velocity the
109
                // copter should fly to west (positive roll).
110
 
111
                // The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
112
                // in the fc.c. Therefore a positive north deviation/velocity should result in a positive
113
                // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
114
 
115
                // rotation transformation to compass heading to match any copter orientation
116
                if (CompassHeading < 0) // no valid compass data
117
                { // disable GPS
118
                        GPS_Neutral();
119
                }
120
                else
121
                {
122
                        coscompass = (int32_t)c_cos_8192(CompassHeading);
123
                        sincompass = (int32_t)c_sin_8192(CompassHeading);
124
                        GPS_Roll  =    (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);
125
                        GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192);
126
                }
127
                // limit GPS controls
128
                if (GPS_Pitch >  GPS_STICK_LIMIT) GPS_Pitch =  GPS_STICK_LIMIT;
764 killagreg 129
                else if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT;
762 killagreg 130
                if (GPS_Roll  >  GPS_STICK_LIMIT)  GPS_Roll =  GPS_STICK_LIMIT;
764 killagreg 131
                else if (GPS_Roll  < -GPS_STICK_LIMIT)  GPS_Roll = -GPS_STICK_LIMIT;
746 killagreg 132
        }
762 killagreg 133
        else // invalid input data
746 killagreg 134
        {
762 killagreg 135
                BeepTime = 50;
136
                GPS_Neutral();
746 killagreg 137
        }
138
}
139
 
140
 
762 killagreg 141
void GPS_Main(uint8_t ctrl)
746 killagreg 142
{
741 killagreg 143
        static uint8_t GPS_Task = TSK_IDLE;
752 killagreg 144
        int16_t satbeep;
1 ingob 145
 
762 killagreg 146
        // ctrl enables the gps feature
147
        if(ctrl < 70) GPS_Task = TSK_IDLE;
148
        else if (ctrl < 160) GPS_Task = TSK_HOLD;
149
        else GPS_Task = TSK_HOME; // ctrl >= 160
741 killagreg 150
 
151
 
152
        switch(GPSInfo.status)
726 killagreg 153
        {
741 killagreg 154
        case INVALID:  // invalid gps data
746 killagreg 155
                GPS_Neutral();
752 killagreg 156
                if(GPS_Task != TSK_IDLE)
157
                {
158
                        BeepTime = 100; // beep if signal is neccesary
159
                }
741 killagreg 160
                break;
161
        case PROCESSED: // if gps data are already processed
162
                // downcount timeout
163
                if(GPSTimeout) GPSTimeout--;
164
                // if no new data arrived within timeout set current data invalid
165
                // and therefore disable GPS
166
                else
726 killagreg 167
                {
746 killagreg 168
                        GPS_Neutral();
741 killagreg 169
                        GPSInfo.status = INVALID;
170
                }
171
                break;
172
        case VALID: // new valid data from gps device
173
                // if the gps data quality is sufficient
174
                if (GPSInfo.satfix == SATFIX_3D)
175
                {
176
                        switch(GPS_Task) // check what's to do
726 killagreg 177
                        {
741 killagreg 178
                                case TSK_IDLE:
179
                                        // update hold point to current gps position
180
                                        HoldPosition.Northing = GPSInfo.utmnorth;
181
                                        HoldPosition.Easting  = GPSInfo.utmeast;
182
                                        HoldPosition.Status = VALID;
183
                                        // disable gps control
746 killagreg 184
                                        GPS_Neutral();
741 killagreg 185
                                        break; // eof TSK_IDLE
186
                                case TSK_HOLD:
762 killagreg 187
                                        if(HoldPosition.Status == VALID)
741 killagreg 188
                                        {
762 killagreg 189
                                                // if sticks are centered (no manual control)
190
                                                if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
191
                                                {
192
                                                        GPS_PDController(&HoldPosition);
193
                                                }
194
                                                else // MK controlled by user
195
                                                {
196
                                                        // update hold point to current gps position
197
                                                        HoldPosition.Northing = GPSInfo.utmnorth;
198
                                                        HoldPosition.Easting  = GPSInfo.utmeast;
199
                                                        HoldPosition.Status   = GPSInfo.status;
200
                                                        // disable gps control
201
                                                        GPS_Neutral();
202
                                                }
741 killagreg 203
                                        }
204
                                        break; // eof TSK_HOLD
746 killagreg 205
                                case TSK_HOME:
206
                                        if(HomePosition.Status == VALID)
207
                                        {
208
                                                // update hold point to current gps position
762 killagreg 209
                                                // to avoid a flight back if home comming is deactivated
746 killagreg 210
                                                HoldPosition.Northing = GPSInfo.utmnorth;
211
                                                HoldPosition.Easting  = GPSInfo.utmeast;
762 killagreg 212
                                                HoldPosition.Status   = GPSInfo.status;
213
                                                // if sticks are centered (no manual control)
214
                                                if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
215
                                                {
216
                                                        GPS_PDController(&HomePosition);
217
                                                }
767 killagreg 218
                                                else // manual control
219
                                                {
220
                                                        GPS_Neutral();
221
                                                }
746 killagreg 222
                                        }
223
                                        else // bad home position
224
                                        {
225
                                                BeepTime = 50; // signal invalid home position
762 killagreg 226
                                                // try to hold at least the position as a fallback option
227
                                                // if sticks are centered (no manual control)
228
                                                if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE) && (HoldPosition.Status == VALID))
229
                                                {
230
                                                        GPS_PDController(&HoldPosition);
231
                                                }
767 killagreg 232
                                                else // manual control or no rteference position
233
                                                {
234
                                                        GPS_Neutral();
235
                                                }
746 killagreg 236
                                        }
237
                                        break; // eof TSK_HOME
741 killagreg 238
                                default: // unhandled task
746 killagreg 239
                                        GPS_Neutral();
741 killagreg 240
                                        break; // eof default
241
                        } // eof switch GPS_Task
242
                } // eof 3D-FIX
727 killagreg 243
 
741 killagreg 244
                else // no 3D-SATFIX
245
                {       // disable gps control
746 killagreg 246
                        GPS_Neutral();
752 killagreg 247
                        if(GPS_Task != TSK_IDLE)
248
                        {
767 killagreg 249
                                satbeep = 1800 - (int16_t)GPSInfo.satnum * 225; // is zero at 8 sats
752 killagreg 250
                                if (satbeep < 0) satbeep = 0;
767 killagreg 251
                                BeepTime = 50 + (uint16_t)satbeep; // max 1850 * 0.1 ms =
752 killagreg 252
                        }
741 killagreg 253
                }
254
                // set current data as processed to avoid further calculations on the same gps data
255
                GPSInfo.status = PROCESSED;
256
                break;
257
        } // eof GPSInfo.status
258
        DebugOut.Analog[14] = GPS_Pitch;
259
        DebugOut.Analog[15] = GPS_Roll;
726 killagreg 260
}
261