Subversion Repositories Projects

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
2136 - 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 <stdlib.h>
59
#include <avr/io.h>
60
#include <stdint.h>
61
#include <string.h>
62
#include <stdbool.h>
63
//#include "91x_lib.h"
64
#include "waypoints.h"
65
#include "../mk-data-structs.h"
66
#include "../uart/uart1.h"
67
#include "../eeprom/eeprom.h"
68
#include "../main.h"
69
 
70
#include <util/delay.h>
71
 
72
#ifdef USE_WAYPOINTS
73
 
74
// the waypoints list
75
 
76
 
77
 
78
NaviData_t *NaviData;
79
 
80
//Point_t PointList[MAX_LIST_LEN];
81
uint8_t WPIndex = 0;            // list index of GPS point representig the current WP, can be maximal WPCount
82
uint8_t POIIndex = 0;   // list index of GPS Point representing the current POI, can be maximal WPCount
83
uint8_t WPCount = 0;            // number of waypoints
84
uint8_t PointCount = 0;         // number of wp in the list can be maximal equal to MAX_LIST_LEN
85
uint8_t POICount = 0;
86
 
87
uint8_t WPActive = false;
88
 
89
uint8_t PointList_Init(void)
90
{
91
        return PointList_Clear();
92
}
93
 
94
uint8_t PointList_Clear(void)
95
{
96
        uint8_t i;
97
        WPIndex = 0;    // real list position are 1 ,2, 3 ...
98
        POIIndex = 0;   // real list position are 1 ,2, 3 ...
99
        WPCount = 0;    // no waypoints
100
        POICount = 0;
101
        PointCount = 0; // no contents
102
        WPActive = false;
103
        NaviData->WaypointNumber = WPCount;
104
        NaviData->WaypointIndex = 0;
105
 
106
        for(i = 0; i < MAX_LIST_LEN; i++)
107
        {
108
                Config.PointList[i].Position.Status = INVALID;
109
                Config.PointList[i].Position.Latitude = 0;
110
                Config.PointList[i].Position.Longitude = 0;
111
                Config.PointList[i].Position.Altitude = 0;
112
                Config.PointList[i].Heading = 361;              // invalid value
113
                Config.PointList[i].ToleranceRadius = 0;        // in meters, if the MK is within that range around the target, then the next target is triggered
114
                Config.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
115
                Config.PointList[i].Type = POINT_TYPE_INVALID;
116
                Config.PointList[i].Event_Flag = 0;             // future implementation
117
                Config.PointList[i].AltitudeRate = 0;           // no change of setpoint
118
        }
119
        return true;
120
}
121
 
122
uint8_t PointList_GetCount(void)
123
{
124
        return PointCount; // number of points in the list
125
}
126
 
127
Point_t* PointList_GetAt(uint8_t index)
128
{
129
        if((index > 0) && (index <= PointCount)) return(&(Config.PointList[index-1]));  // return pointer to this waypoint
130
        else return(NULL);
131
}
132
 
133
uint8_t PointList_SetAt(Point_t* pPoint)
134
{
135
        // if index is in range
136
        if((pPoint->Index > 0) && (pPoint->Index <= MAX_LIST_LEN))
137
        {
138
                // check list entry before update
139
                switch(Config.PointList[pPoint->Index-1].Type)
140
                {
141
                        case POINT_TYPE_INVALID: // was invalid
142
                                switch(pPoint->Type)
143
                                {
144
                                        default:
145
                                        case POINT_TYPE_INVALID:
146
                                                // nothing to do
147
                                                break;
148
 
149
                                        case POINT_TYPE_WP:
150
                                                WPCount++;
151
                                                PointCount++;
152
                                                break;
153
 
154
                                        case POINT_TYPE_POI:
155
                                                POICount++;
156
                                                PointCount++;
157
                                                break;
158
                                }
159
                                break;
160
 
161
                        case POINT_TYPE_WP: // was  a waypoint
162
                                switch(pPoint->Type)
163
                                {
164
                                        case POINT_TYPE_INVALID:
165
                                                WPCount--;
166
                                                PointCount--;
167
                                                break;
168
 
169
                                        default:
170
                                        case POINT_TYPE_WP:
171
                                                //nothing to do
172
                                                break;
173
 
174
                                        case POINT_TYPE_POI:
175
                                                POICount++;
176
                                                WPCount--;
177
                                                break;
178
                                }
179
                                break;
180
 
181
                        case POINT_TYPE_POI: // was a poi
182
                                switch(pPoint->Type)
183
                                {
184
                                        case POINT_TYPE_INVALID:
185
                                                POICount--;
186
                                                PointCount--;
187
                                                break;
188
 
189
                                        case POINT_TYPE_WP:
190
                                                WPCount++;
191
                                                POICount--;
192
                                                break;
193
 
194
                                        case POINT_TYPE_POI:
195
                                        default:
196
                                                // nothing to do
197
                                                break;
198
                                }
199
                                break;         
200
                }
201
                memcpy(&Config.PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry
202
                NaviData->WaypointNumber = WPCount;
203
                return pPoint->Index;
204
        }
205
        else return(0);
206
}
207
 
208
// returns the pointer to the first waypoint within the list
209
Point_t* PointList_WPBegin(void)
210
{
211
        uint8_t i;
212
        WPIndex = 0; // set list position invalid
213
 
214
        if(WPActive == false) return(NULL);
215
 
216
        POIIndex = 0; // set invalid POI
217
        if(PointCount > 0)
218
        {
219
                // search for first wp in list
220
                for(i = 0; i <MAX_LIST_LEN; i++)
221
                {
222
                        if((Config.PointList[i].Type == POINT_TYPE_WP) && (Config.PointList[i].Position.Status != INVALID))
223
                        {
224
                                WPIndex = i + 1;
225
                                break;
226
                        }
227
                }
228
                if(WPIndex) // found a WP in the list
229
                {
230
                        NaviData->WaypointIndex = 1;
231
                        // update index to POI
232
                        if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading);
233
                        else POIIndex = 0;                     
234
                }
235
                else // some points in the list but no WP found
236
                {
237
                        NaviData->WaypointIndex = 0;
238
                        //Check for an existing POI
239
                        for(i = 0; i < MAX_LIST_LEN; i++)
240
                        {
241
                                if((Config.PointList[i].Type == POINT_TYPE_POI) && (Config.PointList[i].Position.Status != INVALID))
242
                                {
243
                                        POIIndex = i + 1;
244
                                        break;
245
                                }
246
                        }
247
                }
248
        }
249
        else // no point in the list
250
        {
251
                POIIndex = 0;
252
                NaviData->WaypointIndex = 0;
253
        }
254
 
255
        if(WPIndex) return(&(Config.PointList[WPIndex-1]));
256
        else return(NULL);
257
}
258
 
259
// returns the last waypoint
260
Point_t* PointList_WPEnd(void)
261
{
262
 
263
        uint8_t i;
264
        WPIndex = 0; // set list position invalid
265
        POIIndex = 0; // set invalid
266
 
267
        if(WPActive == false) return(NULL);
268
 
269
        if(PointCount > 0)
270
        {
271
                // search backward!
272
                for(i = 1; i <= MAX_LIST_LEN; i++)
273
                {
274
                        if((Config.PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (Config.PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
275
                        {      
276
                                WPIndex = MAX_LIST_LEN - i + 1;
277
                                break;
278
                        }
279
                }
280
                if(WPIndex) // found a WP within the list
281
                {
282
                        NaviData->WaypointIndex = WPCount;
283
                        if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading);
284
                        else POIIndex = 0;     
285
                }
286
                else // list contains some points but no WP in the list
287
                {
288
                        // search backward for a POI!
289
                        for(i = 1; i <= MAX_LIST_LEN; i++)
290
                        {
291
                                if((Config.PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (Config.PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
292
                                {      
293
                                        POIIndex = MAX_LIST_LEN - i + 1;
294
                                        break;
295
                                }
296
                        }
297
                        NaviData->WaypointIndex = 0;
298
                }
299
        }
300
        else // no point in the list
301
        {
302
                POIIndex = 0;
303
                NaviData->WaypointIndex = 0;
304
        }
305
        if(WPIndex) return(&(Config.PointList[WPIndex-1]));
306
        else return(NULL);
307
}
308
 
309
// returns a pointer to the next waypoint or NULL if the end of the list has been reached
310
Point_t* PointList_WPNext(void)
311
{
312
        uint8_t wp_found = 0;
313
        if(WPActive == false) return(NULL);
314
 
315
        if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list
316
        {
317
                uint8_t i;
318
                for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry
319
                {
320
                        if((Config.PointList[i].Type == POINT_TYPE_WP) && (Config.PointList[i].Position.Status != INVALID)) // jump over POIs
321
                        {
322
                                wp_found = i+1;
323
                                break;
324
                        }
325
                }
326
        }
327
        if(wp_found)
328
        {
329
                WPIndex = wp_found; // update list position
330
                NaviData->WaypointIndex++;
331
                if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading);
332
                else POIIndex = 0;
333
                return(&(Config.PointList[WPIndex-1])); // return pointer to this waypoint
334
        }
335
        else
336
        {  // no next wp found
337
                NaviData->WaypointIndex = 0;
338
                POIIndex = 0;
339
                return(NULL);
340
        }
341
}      
342
 
343
void PointList_WPActive(uint8_t set)
344
{
345
        if(set)
346
        {      
347
                WPActive = true;
348
                PointList_WPBegin(); // uopdates POI index
349
        }
350
        else
351
        {
352
                WPActive = false;
353
                POIIndex = 0;  // disable POI also
354
        }
355
}
356
 
357
Point_t* PointList_GetPOI(void)
358
{
359
        return PointList_GetAt(POIIndex);      
360
}
361
 
362
#endif