Subversion Repositories NaviCtrl

Rev

Rev 504 | Rev 514 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
41 ingob 1
/*#######################################################################################*/
2
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
3
/*#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + www.MikroKopter.com
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 7
// + Software Nutzungsbedingungen (english version: see below)
8
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
9
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
10
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool 
11
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
12
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
41 ingob 13
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 14
// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
15
// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
16
// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
17
// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
18
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
19
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
20
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
21
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand 
22
// + des Mitverschuldens offen.
23
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
24
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
25
// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
26
// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
27
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
28
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
29
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
30
// +  Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
41 ingob 31
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 32
// + Software LICENSING TERMS
41 ingob 33
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 34
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
35
// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware 
36
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
37
// + The Software may only be used with the Licensor's products.
38
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
39
// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
40
// + agreement shall be the property of the Licensor.
41
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
42
// + features that can be used to identify the program may not be altered or defaced by the customer.
43
// + The customer shall be responsible for taking reasonable precautions
44
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
45
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
46
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
47
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
48
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
49
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
50
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
51
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
52
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
53
// + #### END OF LICENSING TERMS ####
54
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
41 ingob 55
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
490 killagreg 56
#include <ctype.h>
57
#include <stdio.h>
58
#include <stdlib.h>      
41 ingob 59
#include <string.h>
60
#include "91x_lib.h"
61
#include "waypoints.h"
62
#include "uart1.h"
490 killagreg 63
#include "fat16.h"
504 holgerb 64
#include "main.h"
513 killagreg 65
#include "spi_slave.h"
41 ingob 66
 
496 killagreg 67
 
68
WPL_Store_t WPL_Store;
69
 
41 ingob 70
// the waypoints list
351 holgerb 71
#define MAX_LIST_LEN 101
227 killagreg 72
 
280 killagreg 73
Point_t PointList[MAX_LIST_LEN];
74
u8 WPIndex = 0;         // list index of GPS point representig the current WP, can be maximal WPCount
75
u8 POIIndex = 0;        // list index of GPS Point representing the current POI, can be maximal WPCount
76
u8 WPCount = 0;         // number of waypoints
491 killagreg 77
u8 PointCount = 0;      // number of points in the list can be maximal equal to MAX_LIST_LEN
78
u8 POICount = 0;        // number of point of interest in the list
41 ingob 79
 
296 killagreg 80
u8 WPActive = FALSE;
280 killagreg 81
 
82
u8 PointList_Init(void)
41 ingob 83
{
280 killagreg 84
        return PointList_Clear();
41 ingob 85
}
86
 
280 killagreg 87
u8 PointList_Clear(void)
41 ingob 88
{
89
        u8 i;
227 killagreg 90
        WPIndex = 0;    // real list position are 1 ,2, 3 ...
91
        POIIndex = 0;   // real list position are 1 ,2, 3 ...
280 killagreg 92
        WPCount = 0;    // no waypoints
283 holgerb 93
    POICount = 0;
280 killagreg 94
        PointCount = 0; // no contents
296 killagreg 95
        WPActive = FALSE;
280 killagreg 96
        NaviData.WaypointNumber = WPCount;
97
        NaviData.WaypointIndex = 0;
41 ingob 98
 
280 killagreg 99
        for(i = 0; i < MAX_LIST_LEN; i++)
41 ingob 100
        {
280 killagreg 101
                PointList[i].Position.Status = INVALID;
102
                PointList[i].Position.Latitude = 0;
103
                PointList[i].Position.Longitude = 0;
104
                PointList[i].Position.Altitude = 0;
105
                PointList[i].Heading = 361;             // invalid value
106
                PointList[i].ToleranceRadius = 0;       // in meters, if the MK is within that range around the target, then the next target is triggered
107
                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
490 killagreg 108
                PointList[i].Event_Flag = 0;            // future implementation
109
                PointList[i].Index = 0;
286 killagreg 110
                PointList[i].Type = POINT_TYPE_INVALID;
490 killagreg 111
                PointList[i].WP_EventChannelValue = 0;
286 killagreg 112
                PointList[i].AltitudeRate = 0;          // no change of setpoint
490 killagreg 113
                PointList[i].Speed = 0;
114
                PointList[i].CamAngle = 0;
115
                PointList[i].Name[0] = 0;
41 ingob 116
        }
504 holgerb 117
        ClearWLP_Name();
41 ingob 118
        return TRUE;           
119
}
120
 
280 killagreg 121
u8 PointList_GetCount(void)
41 ingob 122
{
280 killagreg 123
        return PointCount; // number of points in the list
41 ingob 124
}
125
 
295 killagreg 126
Point_t* PointList_GetAt(u8 index)
41 ingob 127
{
295 killagreg 128
        if((index > 0) && (index <= PointCount)) return(&(PointList[index-1])); // return pointer to this waypoint
129
        else return(NULL);
130
}
131
 
132
u8 PointList_SetAt(Point_t* pPoint)
133
{
134
        // if index is in range
296 killagreg 135
        if((pPoint->Index > 0) && (pPoint->Index <= MAX_LIST_LEN))
41 ingob 136
        {
295 killagreg 137
                // check list entry before update
138
                switch(PointList[pPoint->Index-1].Type)
277 killagreg 139
                {
295 killagreg 140
                        case POINT_TYPE_INVALID: // was invalid
141
                                switch(pPoint->Type)
142
                                {
143
                                        default:
144
                                        case POINT_TYPE_INVALID:
145
                                                // nothing to do
146
                                                break;
147
 
148
                                        case POINT_TYPE_WP:
149
                                                WPCount++;
150
                                                PointCount++;
151
                                                break;
152
 
153
                                        case POINT_TYPE_POI:
154
                                                POICount++;
155
                                                PointCount++;
156
                                                break;
157
                                }
158
                                break;
277 killagreg 159
 
490 killagreg 160
                        case POINT_TYPE_WP: // was a waypoint
295 killagreg 161
                                switch(pPoint->Type)
162
                                {
163
                                        case POINT_TYPE_INVALID:
164
                                                WPCount--;
165
                                                PointCount--;
166
                                                break;
277 killagreg 167
 
295 killagreg 168
                                        default:
169
                                        case POINT_TYPE_WP:
170
                                                //nothing to do
171
                                                break;
172
 
173
                                        case POINT_TYPE_POI:
174
                                                POICount++;
175
                                                WPCount--;
176
                                                break;
177
                                }
178
                                break;
179
 
180
                        case POINT_TYPE_POI: // was a poi
181
                                switch(pPoint->Type)
182
                                {
183
                                        case POINT_TYPE_INVALID:
184
                                                POICount--;
185
                                                PointCount--;
186
                                                break;
187
 
188
                                        case POINT_TYPE_WP:
189
                                                WPCount++;
190
                                                POICount--;
191
                                                break;
192
 
193
                                        case POINT_TYPE_POI:
194
                                        default:
195
                                                // nothing to do
196
                                                break;
197
                                }
198
                                break;         
277 killagreg 199
                }
295 killagreg 200
                memcpy(&PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry                                                                                
201
                NaviData.WaypointNumber = WPCount;
202
                return pPoint->Index;
41 ingob 203
        }
296 killagreg 204
        else return(0);
41 ingob 205
}
206
 
227 killagreg 207
// returns the pointer to the first waypoint within the list
280 killagreg 208
Point_t* PointList_WPBegin(void)
41 ingob 209
{
277 killagreg 210
        u8 i;
227 killagreg 211
        WPIndex = 0; // set list position invalid
295 killagreg 212
 
283 holgerb 213
        if(WPActive == FALSE) return(NULL);
295 killagreg 214
 
277 killagreg 215
        POIIndex = 0; // set invalid POI
280 killagreg 216
        if(PointCount > 0)
152 killagreg 217
        {
225 killagreg 218
                // search for first wp in list
295 killagreg 219
                for(i = 0; i <MAX_LIST_LEN; i++)
225 killagreg 220
                {
280 killagreg 221
                        if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID))
225 killagreg 222
                        {
227 killagreg 223
                                WPIndex = i + 1;
225 killagreg 224
                                break;
225
                        }
226
                }
277 killagreg 227
                if(WPIndex) // found a WP in the list
228
                {
280 killagreg 229
                        NaviData.WaypointIndex = 1;
277 killagreg 230
                        // update index to POI
280 killagreg 231
                        if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
296 killagreg 232
                        else POIIndex = 0;                     
277 killagreg 233
                }
234
                else // some points in the list but no WP found
235
                {
280 killagreg 236
                        NaviData.WaypointIndex = 0;
277 killagreg 237
                        //Check for an existing POI
295 killagreg 238
                        for(i = 0; i < MAX_LIST_LEN; i++)
277 killagreg 239
                        {
280 killagreg 240
                                if((PointList[i].Type == POINT_TYPE_POI) && (PointList[i].Position.Status != INVALID))
277 killagreg 241
                                {
242
                                        POIIndex = i + 1;
243
                                        break;
244
                                }
245
                        }
246
                }
152 killagreg 247
        }
277 killagreg 248
        else // no point in the list
152 killagreg 249
        {
277 killagreg 250
                POIIndex = 0;
295 killagreg 251
                NaviData.WaypointIndex = 0;    
227 killagreg 252
        }
295 killagreg 253
 
254
        if(WPIndex) return(&(PointList[WPIndex-1]));
255
        else return(NULL);
41 ingob 256
}
257
 
225 killagreg 258
// returns the last waypoint
280 killagreg 259
Point_t* PointList_WPEnd(void)
151 killagreg 260
{
277 killagreg 261
 
262
        u8 i;
227 killagreg 263
        WPIndex = 0; // set list position invalid
277 killagreg 264
        POIIndex = 0; // set invalid
283 holgerb 265
 
280 killagreg 266
        if(WPActive == FALSE) return(NULL);
277 killagreg 267
 
280 killagreg 268
        if(PointCount > 0)
151 killagreg 269
        {
227 killagreg 270
                // search backward!
295 killagreg 271
                for(i = 1; i <= MAX_LIST_LEN; i++)
225 killagreg 272
                {
295 killagreg 273
                        if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
225 killagreg 274
                        {      
295 killagreg 275
                                WPIndex = MAX_LIST_LEN - i + 1;
225 killagreg 276
                                break;
277
                        }
278
                }
277 killagreg 279
                if(WPIndex) // found a WP within the list
280
                {
280 killagreg 281
                        NaviData.WaypointIndex = WPCount;
282
                        if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
277 killagreg 283
                        else POIIndex = 0;     
284
                }
278 killagreg 285
                else // list contains some points but no WP in the list
277 killagreg 286
                {
287
                        // search backward for a POI!
295 killagreg 288
                        for(i = 1; i <= MAX_LIST_LEN; i++)
277 killagreg 289
                        {
295 killagreg 290
                                if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
277 killagreg 291
                                {      
295 killagreg 292
                                        POIIndex = MAX_LIST_LEN - i + 1;
277 killagreg 293
                                        break;
294
                                }
295
                        }
295 killagreg 296
                        NaviData.WaypointIndex = 0;    
277 killagreg 297
                }
151 killagreg 298
        }
277 killagreg 299
        else // no point in the list
227 killagreg 300
        {
280 killagreg 301
                POIIndex = 0;
302
                NaviData.WaypointIndex = 0;
227 killagreg 303
        }
295 killagreg 304
        if(WPIndex) return(&(PointList[WPIndex-1]));
305
        else return(NULL);
151 killagreg 306
}
307
 
41 ingob 308
// returns a pointer to the next waypoint or NULL if the end of the list has been reached
280 killagreg 309
Point_t* PointList_WPNext(void)
41 ingob 310
{
227 killagreg 311
        u8 wp_found = 0;
280 killagreg 312
        if(WPActive == FALSE) return(NULL);
227 killagreg 313
 
295 killagreg 314
        if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list
41 ingob 315
        {
227 killagreg 316
                u8 i;
295 killagreg 317
                for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry
225 killagreg 318
                {
280 killagreg 319
                        if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID)) // jump over POIs
225 killagreg 320
                        {
227 killagreg 321
                                wp_found = i+1;
225 killagreg 322
                                break;
323
                        }
324
                }
41 ingob 325
        }
227 killagreg 326
        if(wp_found)
327
        {
328
                WPIndex = wp_found; // update list position
280 killagreg 329
                NaviData.WaypointIndex++;
330
                if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
227 killagreg 331
                else POIIndex = 0;
280 killagreg 332
                return(&(PointList[WPIndex-1]));        // return pointer to this waypoint
227 killagreg 333
        }
277 killagreg 334
        else
280 killagreg 335
        {  // no next wp found
336
                NaviData.WaypointIndex = 0;    
277 killagreg 337
                POIIndex = 0;
338
                return(NULL);
339
        }
41 ingob 340
}      
280 killagreg 341
 
342
void PointList_WPActive(u8 set)
343
{
295 killagreg 344
        if(set)
345
        {      
346
                WPActive = TRUE;
491 killagreg 347
                PointList_WPBegin(); // updates POI index
295 killagreg 348
        }
349
        else
350
        {
351
                WPActive = FALSE;
352
                POIIndex = 0;  // disable POI also
353
        }
280 killagreg 354
}
41 ingob 355
 
280 killagreg 356
Point_t* PointList_GetPOI(void)
224 killagreg 357
{
280 killagreg 358
        return PointList_GetAt(POIIndex);      
224 killagreg 359
}
360
 
513 killagreg 361
 
490 killagreg 362
#define LINE_MAX 70
363
#define WP_FILE_VERSION_COMPATIBLE 3
513 killagreg 364
 
365
u8 PointList_Save(u8 * filename, u8* listname, u8 overwride)
490 killagreg 366
{
367
        File_t *fp;
495 killagreg 368
        s8 wpline[LINE_MAX];
369
        u8 retval = WPL_ERROR;
513 killagreg 370
 
490 killagreg 371
        UART1_PutString("\n\r Save WPL...");
372
 
373
        if(Fat16_IsValid())
374
        {       // check if wpl file is existing
513 killagreg 375
                if(fexist_(filename))
495 killagreg 376
                {       //file is existent
513 killagreg 377
                        if(!(overwride))
495 killagreg 378
                        {
379
                                UART1_PutString("Error: file exist!\r\n");
380
                                return(WPL_FILEEXIST);
381
                        }      
382
                }
513 killagreg 383
                fp = fopen_(filename, 'w');             // try to open the file
490 killagreg 384
                if(fp == NULL)
385
                {
386
                        UART1_PutString("ERROR: Creating waypoint file!\r\n");
387
                        return(retval);
388
                }
389
                // Create general section and key entries
390
                fputs_("[General]\r\n", fp);
513 killagreg 391
                sprintf(wpline, "Name=%s\r\n",  listname);
494 killagreg 392
                fputs_(wpline, fp);
490 killagreg 393
                sprintf(wpline, "FileVersion=%d\r\n", WP_FILE_VERSION_COMPATIBLE);
394
                fputs_(wpline, fp);
395
                sprintf(wpline, "NumberOfWaypoints=%d\r\n", PointCount);
396
                fputs_(wpline, fp);
397
                // dump all points if existent
398
                if(PointCount)
399
                {
400
                        u8 i, u8_1;
401
                        s32 i32_1, i32_2;
504 holgerb 402
                        NewWPL_Name = 1;
490 killagreg 403
                        for (i = 0; i < PointCount; i++)
404
                        {
405
                                sprintf(wpline, "[Point%d]\r\n",PointList[i].Index);
406
                                fputs_(wpline, fp);
407
                                // write latitude in deg
408
                                if(PointList[i].Position.Latitude < 0) u8_1 = '-';
409
                                else u8_1 = '+';
410
                                i32_1 = abs(PointList[i].Position.Latitude)/10000000L;
411
                                i32_2 = abs(PointList[i].Position.Latitude)%10000000L;
412
                                sprintf(wpline, "Latitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);
413
                                fputs_(wpline, fp);
414
                                // write longitude in deg
415
                                if(PointList[i].Position.Longitude < 0) u8_1 = '-';
416
                                else u8_1 = '+';
417
                                i32_1 = abs(PointList[i].Position.Longitude)/10000000L;
418
                                i32_2 = abs(PointList[i].Position.Longitude)%10000000L;
419
                                sprintf(wpline, "Longitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);
420
                                fputs_(wpline, fp);
421
                                // write tolerace radius in m
422
                                sprintf(wpline, "Radius=%d\r\n", PointList[i].ToleranceRadius);
423
                                fputs_(wpline, fp);
424
                                // write altitude in m
425
                                if(PointList[i].Position.Altitude < 0) u8_1 = '-';
426
                                else u8_1 = '+';
427
                                if(PointList[i].Type == POINT_TYPE_POI)
428
                                {
429
                                        i32_1 = abs(PointList[i].Position.Altitude)/100L;  // cm --> m
430
                                        i32_2 = abs(PointList[i].Position.Altitude)%100L;
431
                                }
432
                                else
433
                                {
434
                                        i32_1 = abs(PointList[i].Position.Altitude)/10L; // dm --> m
435
                                        i32_2 = abs(PointList[i].Position.Altitude)%10L;
436
                                }
437
                                sprintf(wpline, "Altitude=%c%ld.%01ld\r\n",  u8_1, i32_1, i32_2);
438
                                fputs_(wpline, fp);
439
                                // write climb rate in 0.1 m/s
440
                                sprintf(wpline, "ClimbRate=%d\r\n", PointList[i].AltitudeRate);
441
                                fputs_(wpline, fp);
442
                                // write hold time in s
443
                                sprintf(wpline, "DelayTime=%d\r\n", PointList[i].HoldTime);
444
                                fputs_(wpline, fp);
445
                                // write event channel value
446
                                sprintf(wpline, "WP_Event_Channel_Value=%d\r\n", PointList[i].WP_EventChannelValue);
447
                                fputs_(wpline, fp);
448
                                // write heading in deg (0= nothing, neg. values index to poi)
449
                                sprintf(wpline, "Heading=%d\r\n", PointList[i].Heading);
450
                                fputs_(wpline, fp);
451
                                // write speed in 0.1 m/s
452
                                sprintf(wpline, "Speed=%d\r\n", PointList[i].Speed);
453
                                fputs_(wpline, fp);
454
                                // write cam angle in degree (255 -> POI-Automatic)
455
                                sprintf(wpline, "CAM-Nick=%d\r\n", PointList[i].CamAngle);
456
                                fputs_(wpline, fp);
457
                                // write point type
458
                                sprintf(wpline, "Type=%d\r\n", PointList[i].Type + 1);
459
                                fputs_(wpline, fp);    
460
                                // write prefix 
461
                                sprintf(wpline, "Prefix=%s\r\n", PointList[i].Name);
462
                                fputs_(wpline, fp);
463
                        } // EOF loop over all points
464
                } // EOF if(PointCount)
465
                if(EOF == fclose_(fp))
466
                {
467
                        UART1_PutString("failed!\r\n");
468
                }
469
                else
470
                {
471
                        UART1_PutString("ok\r\n");
495 killagreg 472
                        retval = WPL_OK;
490 killagreg 473
                }                                
474
        } // EOF if(Fat16_IsValid())
497 killagreg 475
        else
476
        {
477
                UART1_PutString("no file system found!\r\n");
478
                retval = WPL_NO_SDCARD_FOUND;
479
        }
490 killagreg 480
        return(retval);
481
}
482
 
513 killagreg 483
u8 PointList_Load(u8 * filename, u8* listname, u8 listnamelen)
490 killagreg 484
{
485
        File_t *fp;
496 killagreg 486
        s8 wpline[LINE_MAX];
487
        u8 retval = WPL_ERROR;
490 killagreg 488
 
489
        s8 *name, *value;
490
        u8 i;
491
 
492
        u8 IsGeneralSection = 0;
493
        u8 IsPointSection  = 0;
494
        u8 WPNumber = 0;
498 killagreg 495
 
496
        // clear point list first
497
        PointList_Clear();
498
 
490 killagreg 499
        UART1_PutString("\n\r Read ");
513 killagreg 500
        UART1_PutString(filename);
490 killagreg 501
        UART1_PutString("...");
502
 
503
        if(Fat16_IsValid())
504
        {       // check if wpl file is existing
513 killagreg 505
                fp = fopen_(filename, 'r');             // try to open the file
490 killagreg 506
                if(fp == NULL)
507
                {
508
                        UART1_PutString("ERROR: Reading waypoint file!\r\n");
496 killagreg 509
                        return(retval);
490 killagreg 510
                }
511
                // read all lines from file
512
                while(fgets_(wpline, LINE_MAX, fp) != 0)
513
                {
514
                        if ( // ignorelines starting with \r,\n,' ',';','#'
515
                                (wpline[0] != '\n') &&
516
                                (wpline[0] != '\r') &&
517
                                (wpline[0] != ' ' ) &&
518
                                (wpline[0] != ';' ) &&
519
                                (wpline[0] != '#' )
520
                                )
521
                        {
522
                                // check for section line found
523
                                if(wpline[0] == '[')
524
                                {
525
                                        // next section found
526
                                        IsGeneralSection = 0;
527
                                        IsPointSection = 0;
528
 
529
                                        name  = strtok(&wpline[1], "]");
530
                                        if(name != NULL)   // if section name
531
                                        {
532
                                                // check section type
533
                                                for(i=0; name[i]; i++) name[i] = toupper(name[i]); // convert to upper case
534
 
535
                                                if(strncmp(name, "POINT", 5) == 0)
536
                                                {
537
                                                        IsPointSection = (u8)atoi(&name[5]);
538
                                                        PointCount++;
539
                                                }
540
                                                else if(strcmp(name, "GENERAL") == 0)
541
                                                {
542
                                                        IsGeneralSection = 1;
543
                                                }
544
                                                else
545
                                                {
546
                                                        UART1_PutString("Unknown section: ");
547
                                                        UART1_PutString(name);
548
                                                        UART1_PutString("\r\n");
549
                                                }
550
                                        }
551
                                } // EOF section line
552
                                else
553
                                {       // look for key entrys of each sections
554
                                        name  = strtok(wpline, "="); // get name
555
                                        value = strtok(NULL, "="); // get value
556
                                        if ((name != NULL) && (value != NULL))
557
                                        {
558
                                                for(i=0; name[i]; i++) name[i] = toupper(name[i]); // convert to upper case
559
                                                if(IsPointSection &&  (IsPointSection <= WPNumber))
560
                                                {
561
                                                        if(strcmp(name, "LATITUDE") == 0)
562
                                                        {
563
                                                                PointList[IsPointSection-1].Position.Latitude = (s32)(atof(value) * 1E7);
564
                                                        }
565
                                                        else if(strcmp(name, "LONGITUDE") == 0)
566
                                                        {
567
                                                                PointList[IsPointSection-1].Position.Longitude = (s32)(atof(value) * 1E7);
568
                                                        }
569
                                                        else if(strcmp(name, "RADIUS") == 0)
570
                                                        {
571
                                                                PointList[IsPointSection-1].ToleranceRadius = (u8)atoi(value);
572
                                                        }
573
                                                        else if(strcmp(name, "ALTITUDE") == 0)
574
                                                        {
575
                                                                PointList[IsPointSection-1].Position.Altitude = (s32)(atof(value) * 100.0);      // in cm
576
                                                                PointList[IsPointSection-1].Position.Status = NEWDATA;
577
                                                        }
578
                                                        else if(strcmp(name, "CLIMBRATE") == 0)
579
                                                        {
580
                                                                PointList[IsPointSection-1].AltitudeRate = (u8)atoi(value);
581
                                                        }
582
                                                        else if(strcmp(name, "DELAYTIME") == 0)
583
                                                        {
584
                                                                PointList[IsPointSection-1].HoldTime = (u8)atoi(value);
585
                                                        }
586
                                                        else if(strcmp(name, "WP_EVENT_CHANNEL_VALUE") == 0)
587
                                                        {
588
                                                                PointList[IsPointSection-1].WP_EventChannelValue = (u8)atoi(value);
589
                                                        }
590
                                                        else if(strcmp(name, "HEADING") == 0)
591
                                                        {
592
                                                                PointList[IsPointSection-1].Heading = (s16)atoi(value);
593
                                                        }
594
                                                        else if(strcmp(name, "SPEED") == 0)
595
                                                        {
596
                                                                PointList[IsPointSection-1].Speed = (u8)atoi(value);
597
                                                        }
598
                                                        else if(strcmp(name, "CAM-NICK") == 0)
599
                                                        {
600
                                                                PointList[IsPointSection-1].CamAngle = (u8)atoi(value);
601
                                                        }
602
                                                        else if(strcmp(name, "TYPE") == 0)
603
                                                        {
604
                                                                PointList[IsPointSection-1].Type = (u8)atoi(value);
605
                                                                if(PointList[IsPointSection-1].Type > 0) PointList[IsPointSection-1].Type--;  // index shift
606
                                                                else PointList[IsPointSection-1].Type = POINT_TYPE_INVALID;
607
 
608
                                                                switch(PointList[IsPointSection-1].Type)
609
                                                                {
610
                                                                        case POINT_TYPE_WP:
611
                                                                                // this works only if altitude key is set before point type key in WPL file     !!
612
                                                                                PointList[IsPointSection-1].Position.Altitude /= 10; // dm only for WPs 
613
                                                                                WPCount++;
614
                                                                                break;
615
 
616
                                                                        case POINT_TYPE_POI:
617
                                                                                POICount++;
618
                                                                                break;
619
                                                                }
620
                                                        }
621
                                                        else if(strcmp(name, "PREFIX") == 0)
622
                                                        {
623
                                                                strncpy(PointList[IsPointSection-1].Name, value, 4);
624
                                                                PointList[IsPointSection-1].Name[3] = 0; // Terminate string
625
                                                        }
626
                                                        else
627
                                                        {
628
                                                                UART1_PutString("Unknown key: ");
629
                                                                UART1_PutString(name);
630
                                                                UART1_PutString("\r\n");
631
                                                        }      
632
                                                } // EOF point section
633
                                                else if(IsGeneralSection)
634
                                                {
635
                                                        if(strcmp(name, "NUMBEROFWAYPOINTS") == 0)
636
                                                        {      
637
                                                                WPNumber = (u8)atoi(value);
638
                                                                if(!WPNumber) // no waypoints in file
639
                                                                {
497 killagreg 640
                                                                        return(WPL_NO_WAYPOINTS); // we are done here   
490 killagreg 641
                                                                }
642
                                                                else if(WPNumber > MAX_LIST_LEN) // number o points larger than ram list
643
                                                                {
644
                                                                        UART1_PutString("To much points!");
497 killagreg 645
                                                                        return(WPL_ERROR);
490 killagreg 646
                                                                }
647
                                                        }
648
                                                        else if (strcmp(name, "FILEVERSION") == 0)
649
                                                        {
650
                                                                if((u8)atoi(value) != WP_FILE_VERSION_COMPATIBLE)
651
                                                                {
496 killagreg 652
                                                                        PointList_Clear();
490 killagreg 653
                                                                        UART1_PutString("Bad file version!\r\n");
496 killagreg 654
                                                                        return(WPL_ERROR);     
490 killagreg 655
                                                                }
656
                                                        }
495 killagreg 657
                                                        else if (strcmp(name, "NAME") == 0)
658
                                                        {
513 killagreg 659
                                                                if(listname)
498 killagreg 660
                                                                {
513 killagreg 661
                                                                        u8 len = strlen(value);
662
                                                                        if(len)
663
                                                                        {
664
                                                                                if(value[len-1] == '\r')
665
                                                                                {
666
                                                                                        value[len-1] = 0;
667
                                                                                        len--;
668
                                                                                }
669
                                                                        }
670
                                                                        if(len > listnamelen) len = listnamelen;
671
                                                                        if(len)
672
                                                                        {
673
                                                                                value[len-1] = 0; // terminate string
674
                                                                                if(listname) memcpy(listname, value, len);
675
                                                                        }
676
                                                                        NewWPL_Name = 1;
498 killagreg 677
                                                                }
495 killagreg 678
                                                        }
490 killagreg 679
                                                        else
680
                                                        {
681
                                                                UART1_PutString("Unknown key: ");
682
                                                                UART1_PutString(name);
683
                                                                UART1_PutString("\r\n");
684
                                                        }
685
                                                } // EOF general section
686
                                        } // EOF valid key entry
687
                                } // EOF key entry line
688
                        } // valid line
689
                } // EOF loop over all lines
690
                fclose_(fp);
691
                NaviData.WaypointNumber = WPCount;
497 killagreg 692
                retval = WPL_OK;        
693
                UART1_PutString("ok\r\n");                               
490 killagreg 694
        } // EOF if(Fat16_IsValid())
497 killagreg 695
        else
696
        {
697
                UART1_PutString("no file system found!\r\n");
698
                retval = WPL_NO_SDCARD_FOUND;
699
        }      
490 killagreg 700
        return(retval);
491 killagreg 701
}
490 killagreg 702
 
513 killagreg 703
// load actual point list from SD card
704
u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store)
705
{
706
        u8 filename[30];       
707
 
708
        pWPL_Store->Name[0] = 0; // clear current list name
709
 
710
        // user absolute path, i.e. leading /
711
        if(pWPL_Store->Index == 0) // index 0 looks for a default WPL file in the root
712
        {
713
                sprintf(filename, "/default.wpl");     
714
        }
715
        else
716
        {
717
                sprintf(filename, "/WPL/list_%03d.wpl", pWPL_Store->Index);
718
        }
719
        return PointList_Load(filename, pWPL_Store->Name, sizeof(pWPL_Store->Name));
720
}
721
 
722
// save actual point list to SD card
723
u8 PointList_WriteToFile(WPL_Store_t * pWPL_Store)
724
{
725
        u8 filename[30];
726
 
727
 
728
        if(PointCount == 0) return(WPL_NO_WAYPOINTS);
729
        // user absolute path, i.e. leading /   
730
        if(pWPL_Store->Index == 0)
731
        {
732
                sprintf(filename, "/default.wpl");
733
        }
734
        else
735
        {
736
                sprintf(filename, "/WPL/list_%03d.wpl", pWPL_Store->Index);
737
        }
738
        return PointList_Save(filename, pWPL_Store->Name, pWPL_Store->OverwriteFile);
739
}
740
 
741
 
742
// save actual gps positiin and heading to file
743
u8 PointList_SaveSinglePoint(WPL_Store_t * pWPL_Store)
744
{
745
        u8 retval = WPL_ERROR;
746
        u8 filename[30];
747
        Point_t WP;
748
 
749
        if(GPSData.Position.Status != INVALID) return(retval);
750
 
751
        // clear current point list
752
        PointList_Clear();
753
        // prepare WP at current position
754
        GPSPos_Copy(&GPSData.Position, &(WP.Position));
755
        // set heading
756
        WP.Heading = CompassSetpointCorrected/10;
757
        if(WP.Heading == 0) WP.Heading = 360;
758
        WP.ToleranceRadius = 0;
759
        WP.HoldTime  = 0;
760
        WP.Index  = 1;
761
        WP.Type = POINT_TYPE_WP;
762
        WP.WP_EventChannelValue = 0;
763
        WP.AltitudeRate = 0;
764
        WP.Speed = 0;
765
        WP.CamAngle = 0;
766
        WP.Name[0] = 'P';
767
        WP.Name[1] = 0;
768
        // add this point to wp list
769
        PointList_SetAt(&WP);
770
 
771
        sprintf(filename, "/WPL/point_%03d.wpl", pWPL_Store->Index);
772
        sprintf(pWPL_Store->Name, "POINT%03d", pWPL_Store->Index);
773
        retval = PointList_Save(filename, pWPL_Store->Name, 1);
774
        return(retval);
775
}
776
// load target gps posititon and heading from file
777
u8 PointList_LoadSinglePoint(WPL_Store_t * pWPL_Store)
778
{
779
        u8 filename[30];
780
 
781
        sprintf(filename, "/WPL/point_%03d.wpl", pWPL_Store->Index);
782
        pWPL_Store->Name[0] = 0; // clear current list name
783
        return PointList_Load(filename, pWPL_Store->Name, sizeof(pWPL_Store->Name));
784
}
785
 
786
 
787
 
504 holgerb 788
void ClearWLP_Name(void)
789
{
513 killagreg 790
        u8 i;
791
        for(i=0; i<sizeof(WPL_Store.Name);i++) WPL_Store.Name[i] = 0;
792
        NewWPL_Name = 1;
504 holgerb 793
}
513 killagreg 794
 
491 killagreg 795
// move actual point list to ref pos., the point in the list marked by index gets the RefPos afterwards
503 holgerb 796
u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos, u16 RotationAngle)
491 killagreg 797
{
798
        u8 retval = 0;
513 killagreg 799
        s32 altitude;
800
        GPS_Pos_t OldRefPos;
491 killagreg 801
        GPS_Pos_Deviation_t RefDeviation;
802
 
803
        // check inputs for plausibility;
804
        if((RefIndex == 0) || (RefIndex > PointCount)) return(retval); 
805
        if(pRefPos == NULL) return(retval);
806
        if(pRefPos->Status == INVALID) return(retval);
807
 
513 killagreg 808
        if(GPSPos_Copy(&(PointList[RefIndex-1].Position), &OldRefPos)) // backup old reference position
491 killagreg 809
        {
810
                u8 i;
513 killagreg 811
                // iterate the position list
491 killagreg 812
                for(i = 0; i < PointCount; i++)
813
                {
814
                        retval = 0;
513 killagreg 815
                        // backup altitude of this point
816
                        altitude = PointList[i].Position.Altitude;
817
                        // calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position
818
                        if(!GPSPos_Deviation(&(PointList[i].Position), &OldRefPos, &RefDeviation)) break;
491 killagreg 819
                        // copy of the new reference position into this list place
820
                        if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break;
513 killagreg 821
                        // restore former altitude 
822
                        PointList[i].Position.Altitude = altitude;
491 killagreg 823
                        // move new reference according to the deviation of the old reference
513 killagreg 824
                        if(RotationAngle > 0)
825
                        {
826
                                retval = GPSPos_ShiftGeodetic(&(PointList[i].Position), (RefDeviation.Bearing + 180 + RotationAngle)%360, RefDeviation.Distance );
827
                                // Now rotate the heading positions if they are fixed angles
828
                                if(PointList[i].Heading >= 0 && PointList[i].Heading <= 360) PointList[i].Heading = (PointList[i].Heading + RotationAngle) % 360;
829
                        }
830
                        else // no rotation
831
                        {
832
                                // move new reference according to the deviation of the old reference
833
                                retval = GPSPos_ShiftCartesian(&(PointList[i].Position), RefDeviation.North, RefDeviation.East);
834
                        }
491 killagreg 835
                        if(!retval) break;             
836
                }
837
        } // else ref pos old not copied!
838
        if(!retval) PointList_Clear();
839
        return(retval);
840
}
841
 
842
 
843