Subversion Repositories NaviCtrl

Rev

Rev 99 | Rev 103 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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