Subversion Repositories Projects

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1515 - 1
/*#######################################################################################*/
2
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
3
/*#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + Copyright (c) 2008 Ingo Busker, Holger Buss
6
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
7
// + FOR NON COMMERCIAL USE ONLY
8
// + www.MikroKopter.com
9
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
10
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
11
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
12
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
13
// + bzgl. der Nutzungsbedingungen aufzunehmen.
14
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
15
// + Verkauf von Luftbildaufnahmen, usw.
16
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
17
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
18
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
21
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
22
// + eindeutig als Ursprung verlinkt werden
23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
25
// + Benutzung auf eigene Gefahr
26
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
27
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
29
// + mit unserer Zustimmung zulässig
30
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
32
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
33
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
34
// + this list of conditions and the following disclaimer.
35
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
36
// +     from this software without specific prior written permission.
37
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
38
// +     for non-commercial use (directly or indirectly)
39
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
40
// +     with our written permission
41
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
42
// +     clearly linked as origin
43
// +   * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
44
//
45
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
46
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
47
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
48
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
49
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
50
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
51
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
52
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
53
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
54
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
55
// +  POSSIBILITY OF SUCH DAMAGE.
56
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
57
 
58
#include <string.h>
59
#include "91x_lib.h"
60
#include "waypoints.h"
61
#include "uart1.h"
62
 
63
// the waypoints list
64
#define MAX_LIST_LEN 31
65
 
66
Point_t PointList[MAX_LIST_LEN];
67
u8 WPIndex = 0;         // list index of GPS point representig the current WP, can be maximal WPCount
68
u8 POIIndex = 0;        // list index of GPS Point representing the current POI, can be maximal WPCount
69
u8 WPCount = 0;         // number of waypoints
70
u8 PointCount = 0;              // number of wp in the list can be maximal equal to MAX_LIST_LEN
71
u8 POICount = 0;
72
 
73
u8 WPActive = FALSE;
74
 
75
u8 PointList_Init(void)
76
{
77
        return PointList_Clear();
78
}
79
 
80
u8 PointList_Clear(void)
81
{
82
        u8 i;
83
        WPIndex = 0;    // real list position are 1 ,2, 3 ...
84
        POIIndex = 0;   // real list position are 1 ,2, 3 ...
85
        WPCount = 0;    // no waypoints
86
    POICount = 0;
87
        PointCount = 0; // no contents
88
        WPActive = FALSE;
89
        NaviData.WaypointNumber = WPCount;
90
        NaviData.WaypointIndex = 0;
91
 
92
        for(i = 0; i < MAX_LIST_LEN; i++)
93
        {
94
                PointList[i].Position.Status = INVALID;
95
                PointList[i].Position.Latitude = 0;
96
                PointList[i].Position.Longitude = 0;
97
                PointList[i].Position.Altitude = 0;
98
                PointList[i].Heading = 361;             // invalid value
99
                PointList[i].ToleranceRadius = 0;       // in meters, if the MK is within that range around the target, then the next target is triggered
100
                PointList[i].HoldTime = 0;                      // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered
101
                PointList[i].Type = POINT_TYPE_INVALID;
102
                PointList[i].Event_Flag = 0;            // future implementation
103
                PointList[i].AltitudeRate = 0;          // no change of setpoint
104
        }
105
        return TRUE;           
106
}
107
 
108
u8 PointList_GetCount(void)
109
{
110
        return PointCount; // number of points in the list
111
}
112
 
113
Point_t* PointList_GetAt(u8 index)
114
{
115
        if((index > 0) && (index <= PointCount)) return(&(PointList[index-1])); // return pointer to this waypoint
116
        else return(NULL);
117
}
118
 
119
u8 PointList_SetAt(Point_t* pPoint)
120
{
121
        // if index is in range
122
        if((pPoint->Index > 0) && (pPoint->Index <= MAX_LIST_LEN))
123
        {
124
                // check list entry before update
125
                switch(PointList[pPoint->Index-1].Type)
126
                {
127
                        case POINT_TYPE_INVALID: // was invalid
128
                                switch(pPoint->Type)
129
                                {
130
                                        default:
131
                                        case POINT_TYPE_INVALID:
132
                                                // nothing to do
133
                                                break;
134
 
135
                                        case POINT_TYPE_WP:
136
                                                WPCount++;
137
                                                PointCount++;
138
                                                break;
139
 
140
                                        case POINT_TYPE_POI:
141
                                                POICount++;
142
                                                PointCount++;
143
                                                break;
144
                                }
145
                                break;
146
 
147
                        case POINT_TYPE_WP: // was  a waypoint
148
                                switch(pPoint->Type)
149
                                {
150
                                        case POINT_TYPE_INVALID:
151
                                                WPCount--;
152
                                                PointCount--;
153
                                                break;
154
 
155
                                        default:
156
                                        case POINT_TYPE_WP:
157
                                                //nothing to do
158
                                                break;
159
 
160
                                        case POINT_TYPE_POI:
161
                                                POICount++;
162
                                                WPCount--;
163
                                                break;
164
                                }
165
                                break;
166
 
167
                        case POINT_TYPE_POI: // was a poi
168
                                switch(pPoint->Type)
169
                                {
170
                                        case POINT_TYPE_INVALID:
171
                                                POICount--;
172
                                                PointCount--;
173
                                                break;
174
 
175
                                        case POINT_TYPE_WP:
176
                                                WPCount++;
177
                                                POICount--;
178
                                                break;
179
 
180
                                        case POINT_TYPE_POI:
181
                                        default:
182
                                                // nothing to do
183
                                                break;
184
                                }
185
                                break;         
186
                }
187
                memcpy(&PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry                                                                                
188
                NaviData.WaypointNumber = WPCount;
189
                return pPoint->Index;
190
        }
191
        else return(0);
192
}
193
 
194
// returns the pointer to the first waypoint within the list
195
Point_t* PointList_WPBegin(void)
196
{
197
        u8 i;
198
        WPIndex = 0; // set list position invalid
199
 
200
        if(WPActive == FALSE) return(NULL);
201
 
202
        POIIndex = 0; // set invalid POI
203
        if(PointCount > 0)
204
        {
205
                // search for first wp in list
206
                for(i = 0; i <MAX_LIST_LEN; i++)
207
                {
208
                        if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID))
209
                        {
210
                                WPIndex = i + 1;
211
                                break;
212
                        }
213
                }
214
                if(WPIndex) // found a WP in the list
215
                {
216
                        NaviData.WaypointIndex = 1;
217
                        // update index to POI
218
                        if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
219
                        else POIIndex = 0;                     
220
                }
221
                else // some points in the list but no WP found
222
                {
223
                        NaviData.WaypointIndex = 0;
224
                        //Check for an existing POI
225
                        for(i = 0; i < MAX_LIST_LEN; i++)
226
                        {
227
                                if((PointList[i].Type == POINT_TYPE_POI) && (PointList[i].Position.Status != INVALID))
228
                                {
229
                                        POIIndex = i + 1;
230
                                        break;
231
                                }
232
                        }
233
                }
234
        }
235
        else // no point in the list
236
        {
237
                POIIndex = 0;
238
                NaviData.WaypointIndex = 0;    
239
        }
240
 
241
        if(WPIndex) return(&(PointList[WPIndex-1]));
242
        else return(NULL);
243
}
244
 
245
// returns the last waypoint
246
Point_t* PointList_WPEnd(void)
247
{
248
 
249
        u8 i;
250
        WPIndex = 0; // set list position invalid
251
        POIIndex = 0; // set invalid
252
 
253
        if(WPActive == FALSE) return(NULL);
254
 
255
        if(PointCount > 0)
256
        {
257
                // search backward!
258
                for(i = 1; i <= MAX_LIST_LEN; i++)
259
                {
260
                        if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
261
                        {      
262
                                WPIndex = MAX_LIST_LEN - i + 1;
263
                                break;
264
                        }
265
                }
266
                if(WPIndex) // found a WP within the list
267
                {
268
                        NaviData.WaypointIndex = WPCount;
269
                        if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
270
                        else POIIndex = 0;     
271
                }
272
                else // list contains some points but no WP in the list
273
                {
274
                        // search backward for a POI!
275
                        for(i = 1; i <= MAX_LIST_LEN; i++)
276
                        {
277
                                if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
278
                                {      
279
                                        POIIndex = MAX_LIST_LEN - i + 1;
280
                                        break;
281
                                }
282
                        }
283
                        NaviData.WaypointIndex = 0;    
284
                }
285
        }
286
        else // no point in the list
287
        {
288
                POIIndex = 0;
289
                NaviData.WaypointIndex = 0;
290
        }
291
        if(WPIndex) return(&(PointList[WPIndex-1]));
292
        else return(NULL);
293
}
294
 
295
// returns a pointer to the next waypoint or NULL if the end of the list has been reached
296
Point_t* PointList_WPNext(void)
297
{
298
        u8 wp_found = 0;
299
        if(WPActive == FALSE) return(NULL);
300
 
301
        if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list
302
        {
303
                u8 i;
304
                for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry
305
                {
306
                        if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID)) // jump over POIs
307
                        {
308
                                wp_found = i+1;
309
                                break;
310
                        }
311
                }
312
        }
313
        if(wp_found)
314
        {
315
                WPIndex = wp_found; // update list position
316
                NaviData.WaypointIndex++;
317
                if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
318
                else POIIndex = 0;
319
                return(&(PointList[WPIndex-1]));        // return pointer to this waypoint
320
        }
321
        else
322
        {  // no next wp found
323
                NaviData.WaypointIndex = 0;    
324
                POIIndex = 0;
325
                return(NULL);
326
        }
327
}      
328
 
329
void PointList_WPActive(u8 set)
330
{
331
        if(set)
332
        {      
333
                WPActive = TRUE;
334
                PointList_WPBegin(); // uopdates POI index
335
        }
336
        else
337
        {
338
                WPActive = FALSE;
339
                POIIndex = 0;  // disable POI also
340
        }
341
}
342
 
343
Point_t* PointList_GetPOI(void)
344
{
345
        return PointList_GetAt(POIIndex);      
346
}
347