Subversion Repositories NaviCtrl

Rev

Rev 44 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1 ingob 1
/*#######################################################################################*/
41 ingob 2
/*#######################################################################################*/
3
 
4
// IMPORTANT NOTE:
5
 
6
// This is only a dummy implementation for errorfree compiling of the NaviCtrl sources.
7
 
8
// The GPS navigation routines are NOT included !
9
 
10
/*#######################################################################################*/
11
/*#######################################################################################*/
1 ingob 12
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
13
/*#######################################################################################*/
14
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
15
// + Copyright (c) 2008 Ingo Busker, Holger Buss
171 ingob 16
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
1 ingob 17
// + FOR NON COMMERCIAL USE ONLY
18
// + www.MikroKopter.com
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41 ingob 20
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
21
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
22
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
23
// + bzgl. der Nutzungsbedingungen aufzunehmen.
1 ingob 24
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
25
// + Verkauf von Luftbildaufnahmen, usw.
26
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41 ingob 27
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
1 ingob 28
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
29
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
30
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
31
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
32
// + eindeutig als Ursprung verlinkt werden
33
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
34
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
35
// + Benutzung auf eigene Gefahr
36
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
37
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
171 ingob 38
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
1 ingob 39
// + mit unserer Zustimmung zulässig
40
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
42
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41 ingob 43
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
1 ingob 44
// + this list of conditions and the following disclaimer.
45
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
46
// +     from this software without specific prior written permission.
41 ingob 47
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
1 ingob 48
// +     for non-commercial use (directly or indirectly)
41 ingob 49
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
1 ingob 50
// +     with our written permission
41 ingob 51
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
52
// +     clearly linked as origin
171 ingob 53
// +   * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
1 ingob 54
//
55
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
56
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
57
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
58
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
59
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
60
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
61
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
62
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
63
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
64
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
41 ingob 65
// +  POSSIBILITY OF SUCH DAMAGE.
1 ingob 66
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41 ingob 67
#include <stdio.h>
68
#include <stdlib.h>
69
#include <math.h>
70
#include "91x_lib.h"
1 ingob 71
#include "main.h"
41 ingob 72
#include "uart1.h"
73
#include "GPS.h"
74
#include "timer.h"
75
#include "spi_slave.h"
76
#include "waypoints.h"
77
#include "i2c.h"
1 ingob 78
 
79
 
41 ingob 80
#define M_PI_180        (M_PI / 180.0f)
81
#define GPS_UPDATETIME_MS 200           // 200ms is 5 Hz
82
typedef enum
83
{
84
        GPS_FLIGHT_MODE_UNDEF,
85
        GPS_FLIGHT_MODE_FREE,
86
        GPS_FLIGHT_MODE_AID,
87
        GPS_FLIGHT_MODE_WAYPOINT
88
} GPS_FlightMode_t;
1 ingob 89
 
41 ingob 90
typedef struct
91
{
92
        float Gain;
93
        float P;
94
        float I;
95
        float D;
96
        float A;
97
        float ACC;
98
        u8 MinSat;
99
        s8 StickThreshold;
100
        float WindCorrection;
101
        s32 OperatingRadius;
102
        u32 NaviAngleLimitation;
103
        GPS_FlightMode_t  FlightMode;
104
} __attribute__((packed)) GPS_Parameter_t;
1 ingob 105
 
41 ingob 106
typedef struct
107
{
108
        Status_t Status; // invalid, newdata, processed
109
        s32 North;              // in cm
110
        s32 East;               // in cm
111
        s32 Bearing;    // in deg
112
        s32 Distance;   // in cm
113
} __attribute__((packed)) GPS_Deviation_t;
114
GPS_Deviation_t TargetDeviation;
115
 
116
GPS_Stick_t             GPS_Stick;
117
GPS_Parameter_t GPS_Parameter;
118
 
119
// the gps reference positions
120
GPS_Pos_t GPS_HoldPosition      = {0,0,0, INVALID};                     // the hold position
121
GPS_Pos_t GPS_HomePosition      = {0,0,0, INVALID};                     // the home position
122
GPS_Pos_t * GPS_pTargetPosition = NULL;                             // pointer to the actual target position
123
Waypoint_t* GPS_pWaypoint = NULL;                                               // pointer to the actual waypoint
124
 
125
//-------------------------------------------------------------
126
// Update GPSParamter
127
void GPS_UpdateParameter(void)
128
{
129
        static GPS_FlightMode_t FlightMode_Old = GPS_FLIGHT_MODE_UNDEF;
130
        // in case of bad receiving conditions
131
        if(FC.RC_Quality < 100)
132
        {       // set fixed parameter
133
                GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE;
134
                GPS_Parameter.Gain      = (float) 100;
135
                GPS_Parameter.P         = (float) 90;
136
                GPS_Parameter.I         = (float) 90;
137
                GPS_Parameter.D         = (float) 90;
138
                GPS_Parameter.A         = (float) 90;
139
                GPS_Parameter.ACC       = (float) 0;
140
                GPS_Parameter.MinSat = 6;
141
                GPS_Parameter.StickThreshold = 8;
142
                GPS_Parameter.WindCorrection = 0.0;
143
                GPS_Parameter.OperatingRadius = 0; // forces the aircraft to fly to home positon
144
                GPS_Parameter.NaviAngleLimitation = 125;
145
        }
146
        else
147
        {
148
                // update parameter from FC
149
                if(StopNavigation) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE;
150
                else
151
                {
152
                        if     (Parameter.NaviGpsModeControl <  50) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_AID;
153
                        else if(Parameter.NaviGpsModeControl < 180) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE;
154
                        else                                                    GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_WAYPOINT;
155
                }
156
                GPS_Parameter.Gain      = (float)Parameter.NaviGpsGain;
157
                GPS_Parameter.P         = (float)Parameter.NaviGpsP;
158
                GPS_Parameter.I         = (float)Parameter.NaviGpsI;
159
                GPS_Parameter.D         = (float)Parameter.NaviGpsD;
160
                GPS_Parameter.A         = (float)Parameter.NaviGpsD;
161
                GPS_Parameter.ACC       = (float)Parameter.NaviGpsACC;
162
                GPS_Parameter.MinSat = (u8)Parameter.NaviGpsMinSat;
163
                GPS_Parameter.StickThreshold = (s8)Parameter.NaviStickThreshold;
164
                GPS_Parameter.WindCorrection = (float)Parameter.NaviWindCorrection;
165
                GPS_Parameter.OperatingRadius = (s32)Parameter.NaviOperatingRadius * 100; // conversion of m to cm
166
                GPS_Parameter.NaviAngleLimitation = (u32) Parameter.NaviAngleLimitation * 2;
167
        }
168
        // FlightMode changed?
169
        if(GPS_Parameter.FlightMode != FlightMode_Old) BeepTime = 100; // beep to indicate that mode has switched
170
        FlightMode_Old = GPS_Parameter.FlightMode;
171
}
172
 
173
//-------------------------------------------------------------
174
// This function defines a good GPS signal condition
175
u8 GPS_IsSignalOK(void)
176
{
177
        if( (GPSData.Status != INVALID) && (GPSData.SatFix == SATFIX_3D) &&  (GPSData.NumOfSats >= GPS_Parameter.MinSat)) return(1);
178
        else return(0);
179
}
180
 
1 ingob 181
//------------------------------------------------------------
41 ingob 182
// Checks for manual control action
183
u8 GPS_IsManuallyControlled(void)
184
{
185
        if( ( (abs(FC.StickNick) > GPS_Parameter.StickThreshold) || (abs(FC.StickRoll) > GPS_Parameter.StickThreshold)) && (GPS_Parameter.StickThreshold > 0)) return 1;
186
        else return 0;
187
}
188
 
189
//------------------------------------------------------------
190
// copy GPS position from source position to target position
191
u8 GPS_CopyPosition(GPS_Pos_t * pGPSPosSrc, GPS_Pos_t* pGPSPosTgt)
192
{
193
        u8 retval = 0;
194
        if((pGPSPosSrc == NULL) || (pGPSPosTgt == NULL)) return(retval);        // bad pointer
195
        // copy only valid positions
196
        if(pGPSPosSrc->Status != INVALID)
197
        {
198
                // if the source GPS position is not invalid
199
                pGPSPosTgt->Longitude   = pGPSPosSrc->Longitude;
200
                pGPSPosTgt->Latitude    = pGPSPosSrc->Latitude;
201
                pGPSPosTgt->Altitude    = pGPSPosSrc->Altitude;
202
                pGPSPosTgt->Status              = NEWDATA; // mark data in target position as new
203
                retval = 1;
204
        }
205
        return(retval);
206
}
207
 
208
//------------------------------------------------------------
209
// clear position data
210
u8 GPS_ClearPosition(GPS_Pos_t * pGPSPos)
211
{
212
        u8 retval = FALSE;
213
        if(pGPSPos == NULL) return(retval);     // bad pointer
214
        else
215
        {
216
                pGPSPos->Longitude      = 0;
217
                pGPSPos->Latitude       = 0;
218
                pGPSPos->Altitude       = 0;
219
                pGPSPos->Status         = INVALID;
220
                retval = TRUE;
221
        }
222
        return (retval);
223
}
224
 
225
 
226
//------------------------------------------------------------
227
void GPS_Neutral()
228
{
229
        GPS_Stick.Nick  = 0;
230
        GPS_Stick.Roll  = 0;
231
        GPS_Stick.Yaw   = 0;
232
}
233
 
234
//------------------------------------------------------------
1 ingob 235
void GPS_Init(void)
236
{
41 ingob 237
        SerialPutString("\r\n GPS init...");
238
        UBX_Init();
239
        GPS_Neutral();
240
        GPS_ClearPosition(&GPS_HoldPosition);
241
        GPS_ClearPosition(&GPS_HomePosition);
242
        GPS_pTargetPosition = NULL;
243
        WPList_Init();
244
        GPS_pWaypoint = WPList_Begin();
245
        GPS_UpdateParameter();
246
        SerialPutString("ok");
247
}
1 ingob 248
 
41 ingob 249
//------------------------------------------------------------
250
// calculate the bearing to target position from its deviation
251
s32 DirectionToTarget_N_E(float northdev, float eastdev)
252
{
253
        s32 bearing;
254
        bearing = (s32)(atan2(northdev, eastdev) / M_PI_180);
255
        bearing = (270L - bearing)%360L;
256
        return(bearing);
1 ingob 257
}
41 ingob 258
 
259
 
1 ingob 260
//------------------------------------------------------------
41 ingob 261
// Rescale xy-vector length if length limit is violated
262
void GPS_LimitXY(s32 *x, s32 *y, s32 limit)
263
{
264
        s32 dist;
265
        dist = (s32)hypot(*x,*y);       // the length of the vector
266
        if ((dist != 0L) && (dist > limit))
267
        // if vector length is larger than the given limit
268
        {       // scale vector compontents so that the length is cut off to limit
269
                *x = (*x * limit) / dist;
270
                *y = (*y * limit) / dist;
271
        }
272
}
1 ingob 273
 
41 ingob 274
//------------------------------------------------------------
275
// transform the integer deg into float radians
276
inline double RadiansFromGPS(s32 deg)
1 ingob 277
{
41 ingob 278
  return ((double)deg * 1e-7f * M_PI_180); // 1E-7 because deg is the value in ° * 1E7
279
}
1 ingob 280
 
41 ingob 281
//------------------------------------------------------------
282
// transform the integer deg into float deg
283
inline double DegFromGPS(s32 deg)
284
{
285
  return ((double)deg  * 1e-7f); // 1E-7 because deg is the value in ° * 1E7
286
}
1 ingob 287
 
41 ingob 288
//------------------------------------------------------------
289
// calculate the deviation from the current position to the target position
290
u8 GPS_CalculateDeviation(GPS_Pos_t * pCurrentPos, GPS_Pos_t * pTargetPos)
291
{
292
        double temp1, temp2;
293
        // if given pointer is NULL
294
        if((pCurrentPos == NULL) || (pTargetPos == NULL)) goto baddata;
295
        // if positions are invalid
296
        if((pCurrentPos->Status == INVALID) || (pTargetPos->Status == INVALID)) goto baddata;
1 ingob 297
 
41 ingob 298
        // The deviation from the current to the target position along north and east direction is
299
        // simple the lat/lon difference. To convert that angular deviation into an
300
        // arc length the spherical projection has to be considered.
301
        // The mean earth radius is 6371km. Therfore the arc length per latitude degree
302
        // is always 6371km * 2 * Pi / 360deg =  111.2 km/deg.
303
        // The arc length per longitude degree depends on the correspondig latitude and
304
        // is 111.2km * cos(latitude).
1 ingob 305
 
41 ingob 306
        // calculate the shortest longitude deviation from target
307
        temp1 = DegFromGPS(pCurrentPos->Longitude) - DegFromGPS(pTargetPos->Longitude);
308
        // outside an angular difference of -180 deg ... +180 deg its shorter to go the other way around
309
        // In our application we wont fly more than 20.000 km but along the date line this is important.
310
        if(temp1 > 180.0f) temp1 -= 360.0f;
311
        else if (temp1 < -180.0f) temp1 += 360.0f;
312
        temp1 *= cos(RadiansFromGPS(pTargetPos->Latitude));
313
        // calculate latitude deviation from target
314
        // this is allways within -180 deg ... 180 deg
315
        temp2 = DegFromGPS(pCurrentPos->Latitude) - DegFromGPS(pTargetPos->Latitude);
316
        // deviation from target position in cm
317
        // i.e. the distance to walk from the target in northern and eastern direction to reach the current position
318
 
319
        TargetDeviation.Status = INVALID;
320
        TargetDeviation.North = (s32)(11119492.7f * temp2);
321
        TargetDeviation.East  = (s32)(11119492.7f * temp1);
322
        // If the position deviation is small enough to neglect the earth curvature
323
        // (this is for our application always fulfilled) the distance to target
324
        // can be calculated by the pythagoras of north and east deviation.
325
        TargetDeviation.Distance = (s32)(11119492.7f * hypot(temp1, temp2));
326
        if (TargetDeviation.Distance == 0L) TargetDeviation.Bearing = 0L;
327
        else TargetDeviation.Bearing = DirectionToTarget_N_E(temp2, temp1);
328
        TargetDeviation.Status = NEWDATA;
329
        return TRUE;
330
 
331
        baddata:
332
        TargetDeviation.North           = 0L;
333
        TargetDeviation.East            = 0L;
334
        TargetDeviation.Distance        = 0L;
335
        TargetDeviation.Bearing         = 0L;
336
        TargetDeviation.Status = INVALID;
337
        return FALSE;
338
}
339
 
340
//------------------------------------------------------------
341
void GPS_Navigation(void)
342
{
343
        static u32 beep_rythm;
344
        static u32 GPSDataTimeout = 0;
345
 
346
        // pointer to current target position
347
        static GPS_Pos_t * pTargetPositionOld = NULL;
348
        static Waypoint_t* GPS_pWaypointOld = NULL;
349
 
350
        static GPS_Pos_t RangedTargetPosition = {0,0,0, INVALID};               // the limited target position, this is derived from the target position with repect to the operating radius
351
        static s32 OperatingRadiusOld = -1;
352
        static u8 WPArrived = FALSE;
353
        static u32 WPTime = 0;
354
 
355
        //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
356
        //+ Check for new data from GPS-receiver
357
        //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
358
        switch(GPSData.Status)
359
        {
360
                case INVALID: // no gps data available
361
                        // do nothing
362
                        break;
363
 
364
                case PROCESSED: // the current data have been allready processed
365
                        // if no new data are available within the timeout switch to invalid state.
366
                        if(CheckDelay(GPSDataTimeout)) GPSData.Status = INVALID;
367
                        // wait for new gps data
368
                        break;
369
 
370
                case NEWDATA: // handle new gps data
371
 
372
                // update GPS Parameter from FC-Data via SPI interface
373
                GPS_UpdateParameter();
374
 
375
                        // wait maximum of 3 times the normal data update time before data timemout
376
                        GPSDataTimeout = SetDelay(3 * GPS_UPDATETIME_MS);
377
                        beep_rythm++;
378
 
379
                        // debug
380
                        DebugOut.Analog[21] = (u16)GPSData.Speed_North;
381
                        DebugOut.Analog[22] = (u16)GPSData.Speed_East;
382
                        DebugOut.Analog[31] = (u16)GPSData.NumOfSats;
383
 
384
                        // If GPS signal condition is sufficient for a reliable position measurement
385
                        if(GPS_IsSignalOK())
386
                        {
387
                                // if the MK is starting or the home position is invalid then store the home position
388
                                if((FC.MKFlags & MKFLAG_START) || (GPS_HomePosition.Status == INVALID))
389
                                {       // try to update the home position from the current position
390
                                        if(GPS_CopyPosition(&(GPSData.Position), &GPS_HomePosition))
391
                                        {
392
                                                BeepTime = 700; // beep on success
393
                                                GPS_CopyPosition(&GPS_HomePosition, &(NaviData.HomePosition));
394
                                        }
395
                                        GPS_pWaypoint = WPList_Begin(); // go to start of waypoint list, return NULL of the list is empty
396
                                }
397
 
398
                                /* The selected flight mode influences the target position pointer and therefore the behavior */
399
 
400
                                // check for current flight mode and set the target pointer GPS_pTargetPosition respectively
401
                                switch(GPS_Parameter.FlightMode)
402
                                {
403
                                        // the GPS control is deactived
404
                                        case GPS_FLIGHT_MODE_FREE:
405
                                                NaviData.NCFlags &= ~(NC_FLAG_PH | NC_FLAG_CH);
406
                                                NaviData.NCFlags |= NC_FLAG_FREE;
407
 
408
                                                // update hold position
409
                                                GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
410
                                                // no target position
411
                                                GPS_pTargetPosition = NULL;
412
                                                break;
413
 
414
                                        // the GPS supports the position hold, if the pilot takes no action
415
                                        case GPS_FLIGHT_MODE_AID:
416
                                                NaviData.NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_CH);
417
                                                NaviData.NCFlags |= NC_FLAG_PH;
418
                                                // reset WPList to begin
419
                                                GPS_pWaypoint = WPList_Begin();
420
 
421
                                                if(GPS_IsManuallyControlled())
422
                                                {
423
                                                    GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
424
                                                        GPS_pTargetPosition = NULL;
425
                                                }
426
                                                else
427
                                                {
428
                                                        GPS_pTargetPosition = &GPS_HoldPosition;
429
                                                }
430
                                                break;
431
 
432
                                        // the GPS control is directed to a target position
433
                                        // given by a waypoint or by the home position
434
                                        case GPS_FLIGHT_MODE_WAYPOINT:
435
                                                NaviData.NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_PH);
436
                                                NaviData.NCFlags |= NC_FLAG_CH;
437
 
438
                                                // waypoint trigger logic
439
                                                if(GPS_pWaypoint != NULL) // waypoint exist
440
                                                {
441
                                                        if(GPS_pWaypoint->Position.Status == INVALID) // should never happen
442
                                                        {
443
                                                                GPS_pWaypoint = WPList_Next(); // goto to next WP
444
                                                                WPArrived = FALSE;
445
                                                                BeepTime = 255;
446
                                                        }
447
                                                        else // waypoint position is valid
448
                                                        {
449
                                                                // check if the pointer to the waypoint has been changed or the data have been updated
450
                                                                if((GPS_pWaypoint != GPS_pWaypointOld) || (GPS_pWaypoint->Position.Status == NEWDATA))
451
                                                                {
452
                                                                        GPS_pWaypointOld = GPS_pWaypoint;
453
                                                                        // reset the arrived bit to break a pending HoldTime of the old WP
454
                                                                        WPArrived = FALSE;
455
                                                                }
456
 
457
                                                                if(TargetDeviation.Status != INVALID)
458
                                                                {       // if the waypoint was not catched and the target area has been reached
459
                                                                        if(!WPArrived && (TargetDeviation.Distance < (GPS_pWaypoint->ToleranceRadius * 100)))
460
                                                                        {
461
                                                                                WPArrived = TRUE;
462
                                                                                WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // set hold time stamp
463
                                                                        }
464
                                                                }
465
                                                                // if WP has been reached once, wait hold time before trigger to next one
466
                                                                if(WPArrived)
467
                                                                {
468
                                                                        /* ToDo: Adjust GPS_pWaypoint->Heading, GPS_pWaypoint->Event handling */
469
                                                                        if(CheckDelay(WPTime))
470
                                                                        {
471
                                                                                GPS_pWaypoint = WPList_Next(); // goto to next waypoint, return NULL if end of list has been reached
472
                                                                                WPArrived = FALSE; // which is not arrived
473
                                                                        }
474
                                                                } // EOF if(WPArrived)
475
                                                        }
476
                                                } // EOF waypoint trigger logic
477
 
478
                                                if(GPS_pWaypoint != NULL) // Waypoint exist
479
                                                {
480
                                                        // update the hold position
481
                                                        GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
482
                                                        GPS_pTargetPosition = &(GPS_pWaypoint->Position);
483
                                                }
484
                                                else // no waypoint info available, i.e. the WPList is empty or the end of the list has been reached
485
                                                {
486
                                                        // fly back to home postion
487
                                                        if(GPS_HomePosition.Status == INVALID)
488
                                                        {
489
                                                                GPS_pTargetPosition = &GPS_HoldPosition; // fall back to hold mode if home position is not available
490
                                                                BeepTime = 255; // beep to indicate missin home position
491
                                                        }
492
                                                        else // the home position is valid
493
                                                        {
494
                                                                // update the hold position
495
                                                                GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
496
                                                                // set target to home position
497
                                                                GPS_pTargetPosition = &GPS_HomePosition;
498
                                                        }
499
                                                }
500
                                                break;
501
 
502
                                        case GPS_FLIGHT_MODE_UNDEF:
503
                                        default:
504
                                                // update hold position
505
                                                GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
506
                                                // no target position
507
                                                GPS_pTargetPosition = NULL;
508
                                                break;
509
 
510
                                }// EOF GPS Mode Handling
511
 
512
 
513
                                /* Calculation of range target based on the real target */
514
 
515
                                // if no target position exist clear the ranged target position
516
                                if(GPS_pTargetPosition == NULL) GPS_ClearPosition(&RangedTargetPosition);
517
                                else
518
                                {       // if the target position has been changed or the value has been updated or the OperatingRadius has changed
519
                                        if((GPS_pTargetPosition != pTargetPositionOld)  || (GPS_pTargetPosition->Status == NEWDATA) || (GPS_Parameter.OperatingRadius != OperatingRadiusOld) )
520
                                        {
521
                                                // calculate deviation of new target position from home position
522
                                                if(GPS_CalculateDeviation(GPS_pTargetPosition, &GPS_HomePosition))
523
                                                {
524
                                                        // check distance from home position
525
                                                        if(TargetDeviation.Distance > GPS_Parameter.OperatingRadius)
526
                                                        {
527
                                                                //calculate ranged target position to be within the operation radius area
528
                                                                NaviData.NCFlags |= NC_FLAG_RANGE_LIMIT;
529
                                                            TargetDeviation.Distance = GPS_Parameter.OperatingRadius;
530
                                                                GPS_LimitXY(&(TargetDeviation.East), &(TargetDeviation.North), TargetDeviation.Distance);
531
 
532
                                                                RangedTargetPosition.Status = INVALID;
533
                                                                RangedTargetPosition.Latitude = GPS_HomePosition.Latitude;
534
                                                                RangedTargetPosition.Latitude += (s32)((float)TargetDeviation.North / 1.11194927f);
535
                                                                RangedTargetPosition.Longitude = GPS_HomePosition.Longitude;
536
                                                                RangedTargetPosition.Longitude += (s32)((float)TargetDeviation.East / (1.11194927f * cos(RadiansFromGPS(GPS_HomePosition.Latitude))) );
537
                                                                RangedTargetPosition.Altitude = GPS_pTargetPosition->Altitude;
538
                                                                RangedTargetPosition.Status = NEWDATA;
539
                                                        }
540
                                                        else
541
                                                        {       // the target is located within the operation radius area
542
                                                                // simple copy the loaction to the ranged target position
543
                                                                GPS_CopyPosition(GPS_pTargetPosition, &RangedTargetPosition);
544
                                                                NaviData.NCFlags &= ~NC_FLAG_RANGE_LIMIT;
545
                                                        }
546
                                                }
547
                                                else
548
                                                {       // deviation could not be determined
549
                                                        GPS_ClearPosition(&RangedTargetPosition);
550
                                                }
551
                                                GPS_pTargetPosition->Status = PROCESSED;        // mark current target as processed!
552
                                        }
553
                                }
554
                                OperatingRadiusOld = GPS_Parameter.OperatingRadius;
555
                                // remember last target position pointer
556
                                pTargetPositionOld = GPS_pTargetPosition;
557
 
558
                                /* Calculate position deviation from ranged target */
559
 
560
                                // calculate deviation of current position to ranged target position in cm
561
                                if(GPS_CalculateDeviation(&(GPSData.Position), &RangedTargetPosition))
562
                                {
563
                                        // implement your control code here based
564
                                        // in the info available in the TargetDeviation, GPSData and FromFlightCtrl.GyroHeading
565
                                        GPS_Stick.Nick = 0;
566
                                        GPS_Stick.Roll = 0;
567
                                        GPS_Stick.Yaw  = 0;
568
                                }
569
                                else // deviation could not be calculated
570
                                {   // do nothing on gps sticks!
571
                                        GPS_Neutral();
572
                                }
573
 
574
                        }// eof if GPSSignal is OK
575
                        else // GPSSignal not OK
576
                        {
577
                                GPS_Neutral();
578
                                // beep if signal is not sufficient
579
                                if(GPS_Parameter.FlightMode != GPS_FLIGHT_MODE_FREE)
580
                                {
581
                                        if(!(GPSData.Flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100;
582
                                        else if (GPSData.NumOfSats < GPS_Parameter.MinSat && !(beep_rythm % 5)) BeepTime = 10;
583
                                }
584
                        }
585
                    GPSData.Status = PROCESSED; // mark as processed
586
                        break;
1 ingob 587
        }
41 ingob 588
 
589
        DebugOut.Analog[27] = (s16)TargetDeviation.North;
590
        DebugOut.Analog[28] = (s16)TargetDeviation.East;
591
        DebugOut.Analog[29] = GPS_Stick.Nick;
592
        DebugOut.Analog[30] = GPS_Stick.Roll;
593
 
594
        // update navi data, send back to ground station
595
        GPS_CopyPosition(&(GPSData.Position), &(NaviData.CurrentPosition));
596
        GPS_CopyPosition(&RangedTargetPosition, &(NaviData.TargetPosition));
597
        NaviData.SatsInUse = GPSData.NumOfSats;
598
        NaviData.TargetPositionDeviation.Distance = TargetDeviation.Distance;
599
        NaviData.TargetPositionDeviation.Bearing = TargetDeviation.Bearing;
600
        NaviData.UBat = FC.UBat;
601
        NaviData.GroundSpeed = (u16)GPSData.Speed_Ground;
602
        NaviData.Heading = (s16)GPSData.Heading;
603
        NaviData.CompassHeading = (s16)FromFlightCtrl.GyroHeading;
604
        NaviData.AngleNick = FromFlightCtrl.IntegralNick / 10;
605
        NaviData.AngleRoll = FromFlightCtrl.IntegralRoll / 10;
606
        NaviData.RC_Quality = (u8) FC.RC_Quality;
607
        NaviData.MKFlags = (u8)FC.MKFlags;
608
 
609
        //+++++++++++++++++++++++++++++++++++++++++++++++++++
610
  return;
1 ingob 611
}
612