Subversion Repositories FlightCtrl

Rev

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