Subversion Repositories FlightCtrl

Rev

Go to most recent revision | 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
 
792 killagreg 84
// calculates the GPS control stick values from the deviation to target position
85
// if the pointer to the target positin is NULL or is the target position invalid
86
// then the P part of the controller is deactivated.
87
void GPS_PDController(GPS_Pos_t *pTargetPos)
746 killagreg 88
{
726 killagreg 89
        int32_t coscompass, sincompass;
781 killagreg 90
        int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
726 killagreg 91
        int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
92
        int32_t PD_North = 0, PD_East = 0;
781 killagreg 93
        static int32_t cos_target_latitude = 1;
746 killagreg 94
 
792 killagreg 95
        // if GPS data and Compass are ok
96
        if((GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D) && (CompassHeading >= 0) )
97
        {
781 killagreg 98
 
792 killagreg 99
                if(pTargetPos != NULL) // if there is a target position
781 killagreg 100
                {
792 killagreg 101
                        if(pTargetPos->Status != INVALID) // and the position data are valid
102
                        { // calculate position deviation from latitude and longitude differences
103
                                GPSPosDev_North = (GPSInfo.latitude - pTargetPos->Latitude); // to calculate real cm we would need *111/100 additionally
104
                                GPSPosDev_East  = (GPSInfo.longitude - pTargetPos->Longitude); // to calculate real cm we would need *111/100 additionally
105
                                // recalculate the target latitude projection only if the target data are updated
106
                                // to save time
107
                                if (pTargetPos->Status != PROCESSED)
108
                                {
109
                                        cos_target_latitude = (int32_t)c_cos_8192((int16_t)(pTargetPos->Latitude/10000000L));
110
                                        pTargetPos->Status = PROCESSED;
111
                                }
112
                                // calculate latitude projection
113
                                GPSPosDev_East  *= cos_target_latitude;
114
                                GPSPosDev_East /= 8192;
746 killagreg 115
 
792 killagreg 116
                                DebugOut.Analog[12] = GPSPosDev_North;
117
                                DebugOut.Analog[13] = GPSPosDev_East;
118
                                //DebugOut.Analog[12] = GPSInfo.velnorth;
119
                                //DebugOut.Analog[13] = GPSInfo.veleast;
746 killagreg 120
 
792 killagreg 121
                                //Calculate P-components of the controller (negative sign for compensation)
122
                                P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
123
                                P_East =  -(GPS_P_Factor * GPSPosDev_East)/2048;
124
                        }
125
                        else // not valid target position available
126
                        {
127
                                GPSPosDev_North = 0;
128
                                GPSPosDev_East = 0;
129
                        }
130
                }
131
                else // not target position available
132
                {
133
                        GPSPosDev_North = 0;
134
                        GPSPosDev_East = 0;
135
                }
767 killagreg 136
 
762 killagreg 137
                //Calculate PD-components of the controller (negative sign for compensation)
138
                P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
792 killagreg 139
                P_East =  -(GPS_P_Factor * GPSPosDev_East)/2048;
762 killagreg 140
                D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
141
                D_East =  -(GPS_D_Factor * GPSInfo.veleast)/512;
792 killagreg 142
                // limit D-Part if position deviation is significant of the target position
143
                // this accelerates flying to the target position
764 killagreg 144
                if( (abs(GPSPosDev_North) > GPS_D_LIMIT_DIST) ||  (abs(GPSPosDev_East) > GPS_D_LIMIT_DIST) )
145
                {
146
                        if (D_North > GPS_D_LIMIT) D_North = GPS_D_LIMIT;
147
                        else if (D_North < -GPS_D_LIMIT) D_North = -GPS_D_LIMIT;
148
                        if (D_East > GPS_D_LIMIT)  D_East  = GPS_D_LIMIT;
149
                        else if (D_East  < -GPS_D_LIMIT) D_East  = -GPS_D_LIMIT;
150
                }
762 killagreg 151
                // PD-controller
152
                PD_North = P_North + D_North;
153
                PD_East  = P_East  + D_East;
746 killagreg 154
 
762 killagreg 155
                // GPS to pitch and roll settings
746 killagreg 156
 
792 killagreg 157
                // A positive pitch angle moves head downwards (flying forward).
158
                // A positive roll angle tilts left side downwards (flying left).
762 killagreg 159
                // If compass heading is 0 the head of the copter is in north direction.
160
                // A positive pitch angle will fly to north and a positive roll angle will fly to west.
161
                // In case of a positive north deviation/velocity the
162
                // copter should fly to south (negative pitch).
163
                // In case of a positive east position deviation and a positive east velocity the
164
                // copter should fly to west (positive roll).
165
                // The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
166
                // in the fc.c. Therefore a positive north deviation/velocity should result in a positive
167
                // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
168
 
792 killagreg 169
                coscompass = (int32_t)c_cos_8192(CompassHeading);
170
                sincompass = (int32_t)c_sin_8192(CompassHeading);
171
                GPS_Roll  =    (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);
172
                GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192);
173
 
762 killagreg 174
                // limit GPS controls
175
                if (GPS_Pitch >  GPS_STICK_LIMIT) GPS_Pitch =  GPS_STICK_LIMIT;
764 killagreg 176
                else if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT;
762 killagreg 177
                if (GPS_Roll  >  GPS_STICK_LIMIT)  GPS_Roll =  GPS_STICK_LIMIT;
764 killagreg 178
                else if (GPS_Roll  < -GPS_STICK_LIMIT)  GPS_Roll = -GPS_STICK_LIMIT;
746 killagreg 179
        }
792 killagreg 180
        else // invalid GPS data
746 killagreg 181
        {
792 killagreg 182
                GPS_Neutral(); // do nothing
746 killagreg 183
        }
184
}
185
 
186
 
762 killagreg 187
void GPS_Main(uint8_t ctrl)
746 killagreg 188
{
741 killagreg 189
        static uint8_t GPS_Task = TSK_IDLE;
792 killagreg 190
        static uint8_t GPS_P_Delay = 0;
752 killagreg 191
        int16_t satbeep;
1 ingob 192
 
762 killagreg 193
        // ctrl enables the gps feature
194
        if(ctrl < 70) GPS_Task = TSK_IDLE;
195
        else if (ctrl < 160) GPS_Task = TSK_HOLD;
196
        else GPS_Task = TSK_HOME; // ctrl >= 160
741 killagreg 197
 
198
 
199
        switch(GPSInfo.status)
726 killagreg 200
        {
741 killagreg 201
        case INVALID:  // invalid gps data
746 killagreg 202
                GPS_Neutral();
752 killagreg 203
                if(GPS_Task != TSK_IDLE)
204
                {
205
                        BeepTime = 100; // beep if signal is neccesary
206
                }
741 killagreg 207
                break;
781 killagreg 208
        case PROCESSED: // if gps data are already processed do nothing
741 killagreg 209
                // downcount timeout
210
                if(GPSTimeout) GPSTimeout--;
211
                // if no new data arrived within timeout set current data invalid
212
                // and therefore disable GPS
213
                else
726 killagreg 214
                {
746 killagreg 215
                        GPS_Neutral();
741 killagreg 216
                        GPSInfo.status = INVALID;
217
                }
218
                break;
219
        case VALID: // new valid data from gps device
781 killagreg 220
                // if the gps data quality is good
741 killagreg 221
                if (GPSInfo.satfix == SATFIX_3D)
222
                {
223
                        switch(GPS_Task) // check what's to do
726 killagreg 224
                        {
741 killagreg 225
                                case TSK_IDLE:
781 killagreg 226
                                        // update hold position to current gps position
227
                                        GPS_SetHoldPosition(); // can get invalid if gps signal is bad
741 killagreg 228
                                        // disable gps control
746 killagreg 229
                                        GPS_Neutral();
741 killagreg 230
                                        break; // eof TSK_IDLE
231
                                case TSK_HOLD:
781 killagreg 232
                                        if(HoldPosition.Status != INVALID)
741 killagreg 233
                                        {
762 killagreg 234
                                                // if sticks are centered (no manual control)
235
                                                if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
236
                                                {
792 killagreg 237
                                                        if(GPS_P_Delay<7)
238
                                                        { // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
239
                                                                GPS_P_Delay++;
240
                                                                GPS_SetHoldPosition();  // update hold point to current gps position
241
                                                                GPS_PDController(NULL); // activates only the D-Part
242
                                                        }
243
                                                        else GPS_PDController(&HoldPosition);// activates the P&D-Part
762 killagreg 244
                                                }
245
                                                else // MK controlled by user
246
                                                {
247
                                                        // update hold point to current gps position
781 killagreg 248
                                                        GPS_SetHoldPosition();
762 killagreg 249
                                                        // disable gps control
250
                                                        GPS_Neutral();
792 killagreg 251
                                                        GPS_P_Delay = 0;
762 killagreg 252
                                                }
741 killagreg 253
                                        }
781 killagreg 254
                                        else // invalid Hold Position
255
                                        {  // try to catch a valid hold position from gps data input
256
                                                GPS_SetHoldPosition();
257
                                                GPS_Neutral();
258
                                        }
741 killagreg 259
                                        break; // eof TSK_HOLD
746 killagreg 260
                                case TSK_HOME:
781 killagreg 261
                                        if(HomePosition.Status != INVALID)
746 killagreg 262
                                        {
263
                                                // update hold point to current gps position
762 killagreg 264
                                                // to avoid a flight back if home comming is deactivated
781 killagreg 265
                                                GPS_SetHoldPosition();
762 killagreg 266
                                                // if sticks are centered (no manual control)
267
                                                if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
268
                                                {
269
                                                        GPS_PDController(&HomePosition);
270
                                                }
767 killagreg 271
                                                else // manual control
272
                                                {
273
                                                        GPS_Neutral();
274
                                                }
746 killagreg 275
                                        }
276
                                        else // bad home position
277
                                        {
278
                                                BeepTime = 50; // signal invalid home position
762 killagreg 279
                                                // try to hold at least the position as a fallback option
781 killagreg 280
 
281
                                                if (HoldPosition.Status != INVALID)
762 killagreg 282
                                                {
781 killagreg 283
                                                        // if sticks are centered (no manual control)
284
                                                        if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
285
                                                        {
286
                                                                GPS_PDController(&HoldPosition);
287
                                                        }
288
                                                        else // manual control or no reference position
289
                                                        {
290
                                                                GPS_Neutral();
291
                                                        }
762 killagreg 292
                                                }
781 killagreg 293
                                                else
294
                                                { // try to catch a valid hold position
295
                                                        GPS_SetHoldPosition();
767 killagreg 296
                                                        GPS_Neutral();
297
                                                }
746 killagreg 298
                                        }
299
                                        break; // eof TSK_HOME
741 killagreg 300
                                default: // unhandled task
746 killagreg 301
                                        GPS_Neutral();
741 killagreg 302
                                        break; // eof default
303
                        } // eof switch GPS_Task
304
                } // eof 3D-FIX
305
                else // no 3D-SATFIX
306
                {       // disable gps control
746 killagreg 307
                        GPS_Neutral();
752 killagreg 308
                        if(GPS_Task != TSK_IDLE)
309
                        {
767 killagreg 310
                                satbeep = 1800 - (int16_t)GPSInfo.satnum * 225; // is zero at 8 sats
752 killagreg 311
                                if (satbeep < 0) satbeep = 0;
767 killagreg 312
                                BeepTime = 50 + (uint16_t)satbeep; // max 1850 * 0.1 ms =
752 killagreg 313
                        }
741 killagreg 314
                }
315
                // set current data as processed to avoid further calculations on the same gps data
316
                GPSInfo.status = PROCESSED;
317
                break;
318
        } // eof GPSInfo.status
319
        DebugOut.Analog[14] = GPS_Pitch;
320
        DebugOut.Analog[15] = GPS_Roll;
726 killagreg 321
}
322