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
782 killagreg 15
#define GPS_D_LIMIT_DIST        500             // for position deviations greater than 500cm the D-Part of the PD-Controller is limited
767 killagreg 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;
726 killagreg 22
 
746 killagreg 23
 
781 killagreg 24
 
726 killagreg 25
typedef struct
1 ingob 26
{
781 killagreg 27
        int32_t Longitude;
28
        int32_t Latitude;
726 killagreg 29
        uint8_t Status;
30
} GPS_Pos_t;
31
 
32
// GPS coordinates for hold position
741 killagreg 33
GPS_Pos_t HoldPosition  = {0,0, INVALID};
34
// GPS coordinates for home position
35
GPS_Pos_t HomePosition  = {0,0, INVALID};
726 killagreg 36
 
37
 
38
// ---------------------------------------------------------------------------------
39
 
781 killagreg 40
// set home position to current positon
746 killagreg 41
void GPS_SetHomePosition(void)
42
{
781 killagreg 43
        if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D)
44
        {
45
                HomePosition.Longitude = GPSInfo.longitude;
46
                HomePosition.Latitude = GPSInfo.latitude;
47
                HomePosition.Status = VALID;
48
                BeepTime = 1000; // signal if new home position was set
49
        }
50
        else
51
        {
52
                HomePosition.Status = INVALID;
53
        }
746 killagreg 54
}
726 killagreg 55
 
781 killagreg 56
// set hold position to current positon
57
void GPS_SetHoldPosition(void)
58
{
59
        if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D)
60
        {
61
                HoldPosition.Longitude = GPSInfo.longitude;
62
                HoldPosition.Latitude = GPSInfo.latitude;
63
                HoldPosition.Status = VALID;
64
        }
65
        else
66
        {
67
                HoldPosition.Status = INVALID;
68
        }
69
}
70
 
762 killagreg 71
// clear home position
72
void GPS_ClearHomePosition(void)
73
{
74
                HomePosition.Status = INVALID;
75
}
746 killagreg 76
 
747 killagreg 77
// disable GPS control sticks
746 killagreg 78
void GPS_Neutral(void)
1 ingob 79
{
746 killagreg 80
        GPS_Pitch = 0;
81
        GPS_Roll = 0;
82
}
83
 
762 killagreg 84
// calculates the GPS control sticks values from the deviation to target position
85
void GPS_PDController(GPS_Pos_t *TargetPos)
746 killagreg 86
{
726 killagreg 87
        int32_t coscompass, sincompass;
781 killagreg 88
        int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
726 killagreg 89
        int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
90
        int32_t PD_North = 0, PD_East = 0;
781 killagreg 91
        static int32_t cos_target_latitude = 1;
746 killagreg 92
 
781 killagreg 93
 
94
        if( (TargetPos->Status != INVALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D))
762 killagreg 95
        {
781 killagreg 96
                // calculate position deviation from latitude and longitude differences
97
                GPSPosDev_North = (GPSInfo.latitude - TargetPos->Latitude); // to calculate real cm we would need *111/100 additionally
98
                GPSPosDev_East  = (GPSInfo.longitude - TargetPos->Longitude); // to calculate real cm we would need *111/100 additionally
99
                // recalculate the target latitude projection only if the target data are updated
100
                // to save time
101
                if (TargetPos->Status != PROCESSED)
102
                {
103
                        cos_target_latitude = (int32_t)c_cos_8192((int16_t)(TargetPos->Latitude/10000000L));
104
                        TargetPos->Status = PROCESSED;
105
                }
106
                // calculate latitude projection
107
                GPSPosDev_East  *= cos_target_latitude;
108
                GPSPosDev_East /= 8192;
746 killagreg 109
 
762 killagreg 110
                DebugOut.Analog[12] = GPSPosDev_North;
111
                DebugOut.Analog[13] = GPSPosDev_East;
746 killagreg 112
 
767 killagreg 113
                //DebugOut.Analog[12] = GPSInfo.velnorth;
114
                //DebugOut.Analog[13] = GPSInfo.veleast;
115
 
762 killagreg 116
                //Calculate PD-components of the controller (negative sign for compensation)
117
                P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
118
                D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
119
                P_East =  -(GPS_P_Factor * GPSPosDev_East)/2048;
120
                D_East =  -(GPS_D_Factor * GPSInfo.veleast)/512;
746 killagreg 121
 
764 killagreg 122
                if( (abs(GPSPosDev_North) > GPS_D_LIMIT_DIST) ||  (abs(GPSPosDev_East) > GPS_D_LIMIT_DIST) )
123
                {
124
                        if (D_North > GPS_D_LIMIT) D_North = GPS_D_LIMIT;
125
                        else if (D_North < -GPS_D_LIMIT) D_North = -GPS_D_LIMIT;
126
                        if (D_East > GPS_D_LIMIT)  D_East  = GPS_D_LIMIT;
127
                        else if (D_East  < -GPS_D_LIMIT) D_East  = -GPS_D_LIMIT;
128
                }
129
 
762 killagreg 130
                // PD-controller
131
                PD_North = P_North + D_North;
132
                PD_East  = P_East  + D_East;
746 killagreg 133
 
762 killagreg 134
                // GPS to pitch and roll settings
746 killagreg 135
 
762 killagreg 136
                //A positive pitch angle moves head downwards (flying forward).
137
                //A positive roll angle tilts left side downwards (flying left).
138
 
139
                // If compass heading is 0 the head of the copter is in north direction.
140
                // A positive pitch angle will fly to north and a positive roll angle will fly to west.
141
                // In case of a positive north deviation/velocity the
142
                // copter should fly to south (negative pitch).
143
                // In case of a positive east position deviation and a positive east velocity the
144
                // copter should fly to west (positive roll).
145
 
146
                // The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
147
                // in the fc.c. Therefore a positive north deviation/velocity should result in a positive
148
                // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
149
 
150
                // rotation transformation to compass heading to match any copter orientation
151
                if (CompassHeading < 0) // no valid compass data
152
                { // disable GPS
153
                        GPS_Neutral();
154
                }
155
                else
156
                {
157
                        coscompass = (int32_t)c_cos_8192(CompassHeading);
158
                        sincompass = (int32_t)c_sin_8192(CompassHeading);
159
                        GPS_Roll  =    (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);
160
                        GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192);
161
                }
162
                // limit GPS controls
163
                if (GPS_Pitch >  GPS_STICK_LIMIT) GPS_Pitch =  GPS_STICK_LIMIT;
764 killagreg 164
                else if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT;
762 killagreg 165
                if (GPS_Roll  >  GPS_STICK_LIMIT)  GPS_Roll =  GPS_STICK_LIMIT;
764 killagreg 166
                else if (GPS_Roll  < -GPS_STICK_LIMIT)  GPS_Roll = -GPS_STICK_LIMIT;
746 killagreg 167
        }
762 killagreg 168
        else // invalid input data
746 killagreg 169
        {
762 killagreg 170
                BeepTime = 50;
171
                GPS_Neutral();
746 killagreg 172
        }
173
}
174
 
175
 
762 killagreg 176
void GPS_Main(uint8_t ctrl)
746 killagreg 177
{
741 killagreg 178
        static uint8_t GPS_Task = TSK_IDLE;
752 killagreg 179
        int16_t satbeep;
1 ingob 180
 
762 killagreg 181
        // ctrl enables the gps feature
182
        if(ctrl < 70) GPS_Task = TSK_IDLE;
183
        else if (ctrl < 160) GPS_Task = TSK_HOLD;
184
        else GPS_Task = TSK_HOME; // ctrl >= 160
741 killagreg 185
 
186
 
187
        switch(GPSInfo.status)
726 killagreg 188
        {
741 killagreg 189
        case INVALID:  // invalid gps data
746 killagreg 190
                GPS_Neutral();
752 killagreg 191
                if(GPS_Task != TSK_IDLE)
192
                {
193
                        BeepTime = 100; // beep if signal is neccesary
194
                }
741 killagreg 195
                break;
781 killagreg 196
        case PROCESSED: // if gps data are already processed do nothing
741 killagreg 197
                // downcount timeout
198
                if(GPSTimeout) GPSTimeout--;
199
                // if no new data arrived within timeout set current data invalid
200
                // and therefore disable GPS
201
                else
726 killagreg 202
                {
746 killagreg 203
                        GPS_Neutral();
741 killagreg 204
                        GPSInfo.status = INVALID;
205
                }
206
                break;
207
        case VALID: // new valid data from gps device
781 killagreg 208
                // if the gps data quality is good
741 killagreg 209
                if (GPSInfo.satfix == SATFIX_3D)
210
                {
211
                        switch(GPS_Task) // check what's to do
726 killagreg 212
                        {
741 killagreg 213
                                case TSK_IDLE:
781 killagreg 214
                                        // update hold position to current gps position
215
                                        GPS_SetHoldPosition(); // can get invalid if gps signal is bad
741 killagreg 216
                                        // disable gps control
746 killagreg 217
                                        GPS_Neutral();
741 killagreg 218
                                        break; // eof TSK_IDLE
219
                                case TSK_HOLD:
781 killagreg 220
                                        if(HoldPosition.Status != INVALID)
741 killagreg 221
                                        {
762 killagreg 222
                                                // if sticks are centered (no manual control)
223
                                                if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
224
                                                {
225
                                                        GPS_PDController(&HoldPosition);
226
                                                }
227
                                                else // MK controlled by user
228
                                                {
229
                                                        // update hold point to current gps position
781 killagreg 230
                                                        GPS_SetHoldPosition();
762 killagreg 231
                                                        // disable gps control
232
                                                        GPS_Neutral();
233
                                                }
741 killagreg 234
                                        }
781 killagreg 235
                                        else // invalid Hold Position
236
                                        {  // try to catch a valid hold position from gps data input
237
                                                GPS_SetHoldPosition();
238
                                                GPS_Neutral();
239
                                        }
741 killagreg 240
                                        break; // eof TSK_HOLD
746 killagreg 241
                                case TSK_HOME:
781 killagreg 242
                                        if(HomePosition.Status != INVALID)
746 killagreg 243
                                        {
244
                                                // update hold point to current gps position
762 killagreg 245
                                                // to avoid a flight back if home comming is deactivated
781 killagreg 246
                                                GPS_SetHoldPosition();
762 killagreg 247
                                                // if sticks are centered (no manual control)
248
                                                if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
249
                                                {
250
                                                        GPS_PDController(&HomePosition);
251
                                                }
767 killagreg 252
                                                else // manual control
253
                                                {
254
                                                        GPS_Neutral();
255
                                                }
746 killagreg 256
                                        }
257
                                        else // bad home position
258
                                        {
259
                                                BeepTime = 50; // signal invalid home position
762 killagreg 260
                                                // try to hold at least the position as a fallback option
781 killagreg 261
 
262
                                                if (HoldPosition.Status != INVALID)
762 killagreg 263
                                                {
781 killagreg 264
                                                        // if sticks are centered (no manual control)
265
                                                        if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
266
                                                        {
267
                                                                GPS_PDController(&HoldPosition);
268
                                                        }
269
                                                        else // manual control or no reference position
270
                                                        {
271
                                                                GPS_Neutral();
272
                                                        }
762 killagreg 273
                                                }
781 killagreg 274
                                                else
275
                                                { // try to catch a valid hold position
276
                                                        GPS_SetHoldPosition();
767 killagreg 277
                                                        GPS_Neutral();
278
                                                }
746 killagreg 279
                                        }
280
                                        break; // eof TSK_HOME
741 killagreg 281
                                default: // unhandled task
746 killagreg 282
                                        GPS_Neutral();
741 killagreg 283
                                        break; // eof default
284
                        } // eof switch GPS_Task
285
                } // eof 3D-FIX
286
                else // no 3D-SATFIX
287
                {       // disable gps control
746 killagreg 288
                        GPS_Neutral();
752 killagreg 289
                        if(GPS_Task != TSK_IDLE)
290
                        {
767 killagreg 291
                                satbeep = 1800 - (int16_t)GPSInfo.satnum * 225; // is zero at 8 sats
752 killagreg 292
                                if (satbeep < 0) satbeep = 0;
767 killagreg 293
                                BeepTime = 50 + (uint16_t)satbeep; // max 1850 * 0.1 ms =
752 killagreg 294
                        }
741 killagreg 295
                }
296
                // set current data as processed to avoid further calculations on the same gps data
297
                GPSInfo.status = PROCESSED;
298
                break;
299
        } // eof GPSInfo.status
300
        DebugOut.Analog[14] = GPS_Pitch;
301
        DebugOut.Analog[15] = GPS_Roll;
726 killagreg 302
}
303