Subversion Repositories NaviCtrl

Rev

Rev 521 | 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
516 holgerb 10
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool 
360 holgerb 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
516 holgerb 21
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand 
360 holgerb 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 -
516 holgerb 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 
360 holgerb 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>
516 holgerb 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
517 holgerb 79
s16 HeadingOld = -1;
41 ingob 80
 
296 killagreg 81
u8 WPActive = FALSE;
280 killagreg 82
 
83
u8 PointList_Init(void)
41 ingob 84
{
516 holgerb 85
        return PointList_Clear();
41 ingob 86
}
87
 
280 killagreg 88
u8 PointList_Clear(void)
41 ingob 89
{
90
        u8 i;
227 killagreg 91
        WPIndex = 0;    // real list position are 1 ,2, 3 ...
92
        POIIndex = 0;   // real list position are 1 ,2, 3 ...
280 killagreg 93
        WPCount = 0;    // no waypoints
516 holgerb 94
    POICount = 0;
280 killagreg 95
        PointCount = 0; // no contents
296 killagreg 96
        WPActive = FALSE;
280 killagreg 97
        NaviData.WaypointNumber = WPCount;
98
        NaviData.WaypointIndex = 0;
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();
516 holgerb 118
        return TRUE;           
41 ingob 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;
516 holgerb 152
 
295 killagreg 153
                                        case POINT_TYPE_POI:
154
                                                POICount++;
155
                                                PointCount++;
156
                                                break;
157
                                }
516 holgerb 158
                                break;
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;
516 holgerb 172
 
295 killagreg 173
                                        case POINT_TYPE_POI:
174
                                                POICount++;
175
                                                WPCount--;
176
                                                break;
177
                                }
178
                                break;
516 holgerb 179
 
295 killagreg 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;
516 holgerb 192
 
295 killagreg 193
                                        case POINT_TYPE_POI:
194
                                        default:
195
                                                // nothing to do
196
                                                break;
197
                                }
516 holgerb 198
                                break;         
277 killagreg 199
                }
516 holgerb 200
                memcpy(&PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry                                                                                
295 killagreg 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
516 holgerb 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);
516 holgerb 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
        {
516 holgerb 250
                POIIndex = 0;
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
{
516 holgerb 261
 
277 killagreg 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))
516 holgerb 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);
516 holgerb 283
                        else POIIndex = 0;     
277 killagreg 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))
516 holgerb 291
                                {      
295 killagreg 292
                                        POIIndex = MAX_LIST_LEN - i + 1;
277 killagreg 293
                                        break;
294
                                }
295
                        }
516 holgerb 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);
516 holgerb 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
        }
516 holgerb 334
        else
280 killagreg 335
        {  // no next wp found
516 holgerb 336
                NaviData.WaypointIndex = 0;    
277 killagreg 337
                POIIndex = 0;
338
                return(NULL);
339
        }
516 holgerb 340
}      
280 killagreg 341
 
342
void PointList_WPActive(u8 set)
343
{
295 killagreg 344
        if(set)
516 holgerb 345
        {      
295 killagreg 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
}
516 holgerb 355
 
280 killagreg 356
Point_t* PointList_GetPOI(void)
224 killagreg 357
{
516 holgerb 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
516 holgerb 377
                        if(!(overwride))
495 killagreg 378
                        {
379
                                UART1_PutString("Error: file exist!\r\n");
380
                                return(WPL_FILEEXIST);
516 holgerb 381
                        }      
495 killagreg 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);
516 holgerb 391
                sprintf(wpline, "Name=%s\r\n",  listname);
494 killagreg 392
                fputs_(wpline, fp);
516 holgerb 393
                sprintf(wpline, "FileVersion=%d\r\n", WP_FILE_VERSION_COMPATIBLE);
490 killagreg 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);
516 holgerb 459
                                fputs_(wpline, fp);    
460
                                // write prefix 
490 killagreg 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;
516 holgerb 473
                }                                
490 killagreg 474
        } // EOF if(Fat16_IsValid())
516 holgerb 475
        else
497 killagreg 476
        {
477
                UART1_PutString("no file system found!\r\n");
478
                retval = WPL_NO_SDCARD_FOUND;
479
        }
490 killagreg 480
        return(retval);
481
}
482
 
519 holgerb 483
u8 PointList_Load(u8 * filename, u8* listname, u8 listnamelen, u8 use_preset_speed)
490 killagreg 484
{
485
        File_t *fp;
496 killagreg 486
        s8 wpline[LINE_MAX];
487
        u8 retval = WPL_ERROR;
516 holgerb 488
 
490 killagreg 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();
517 holgerb 498
        HeadingOld = -1; // updates the direction if the new direction is the same like last time
490 killagreg 499
        UART1_PutString("\n\r Read ");
516 holgerb 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
                                                        {
519 holgerb 596
                                                                if(use_preset_speed) PointList[IsPointSection-1].Speed = use_preset_speed;
597
                                                                else PointList[IsPointSection-1].Speed = (u8)atoi(value);
490 killagreg 598
                                                        }
599
                                                        else if(strcmp(name, "CAM-NICK") == 0)
600
                                                        {
601
                                                                PointList[IsPointSection-1].CamAngle = (u8)atoi(value);
602
                                                        }
603
                                                        else if(strcmp(name, "TYPE") == 0)
604
                                                        {
605
                                                                PointList[IsPointSection-1].Type = (u8)atoi(value);
606
                                                                if(PointList[IsPointSection-1].Type > 0) PointList[IsPointSection-1].Type--;  // index shift
607
                                                                else PointList[IsPointSection-1].Type = POINT_TYPE_INVALID;
516 holgerb 608
 
490 killagreg 609
                                                                switch(PointList[IsPointSection-1].Type)
610
                                                                {
611
                                                                        case POINT_TYPE_WP:
612
                                                                                // this works only if altitude key is set before point type key in WPL file     !!
516 holgerb 613
                                                                                PointList[IsPointSection-1].Position.Altitude /= 10; // dm only for WPs 
490 killagreg 614
                                                                                WPCount++;
615
                                                                                break;
616
 
617
                                                                        case POINT_TYPE_POI:
618
                                                                                POICount++;
619
                                                                                break;
620
                                                                }
621
                                                        }
622
                                                        else if(strcmp(name, "PREFIX") == 0)
623
                                                        {
624
                                                                strncpy(PointList[IsPointSection-1].Name, value, 4);
625
                                                                PointList[IsPointSection-1].Name[3] = 0; // Terminate string
626
                                                        }
627
                                                        else
628
                                                        {
629
                                                                UART1_PutString("Unknown key: ");
630
                                                                UART1_PutString(name);
631
                                                                UART1_PutString("\r\n");
516 holgerb 632
                                                        }      
490 killagreg 633
                                                } // EOF point section
634
                                                else if(IsGeneralSection)
635
                                                {
636
                                                        if(strcmp(name, "NUMBEROFWAYPOINTS") == 0)
516 holgerb 637
                                                        {      
490 killagreg 638
                                                                WPNumber = (u8)atoi(value);
639
                                                                if(!WPNumber) // no waypoints in file
640
                                                                {
516 holgerb 641
                                                                        return(WPL_NO_WAYPOINTS); // we are done here   
490 killagreg 642
                                                                }
643
                                                                else if(WPNumber > MAX_LIST_LEN) // number o points larger than ram list
644
                                                                {
514 holgerb 645
                                                                        UART1_PutString("To many points!");
497 killagreg 646
                                                                        return(WPL_ERROR);
516 holgerb 647
                                                                }
490 killagreg 648
                                                        }
649
                                                        else if (strcmp(name, "FILEVERSION") == 0)
650
                                                        {
651
                                                                if((u8)atoi(value) != WP_FILE_VERSION_COMPATIBLE)
652
                                                                {
496 killagreg 653
                                                                        PointList_Clear();
490 killagreg 654
                                                                        UART1_PutString("Bad file version!\r\n");
516 holgerb 655
                                                                        return(WPL_ERROR);     
490 killagreg 656
                                                                }
657
                                                        }
495 killagreg 658
                                                        else if (strcmp(name, "NAME") == 0)
659
                                                        {
513 killagreg 660
                                                                if(listname)
498 killagreg 661
                                                                {
513 killagreg 662
                                                                        u8 len = strlen(value);
663
                                                                        if(len)
664
                                                                        {
665
                                                                                if(value[len-1] == '\r')
666
                                                                                {
667
                                                                                        value[len-1] = 0;
668
                                                                                        len--;
669
                                                                                }
670
                                                                        }
671
                                                                        if(len > listnamelen) len = listnamelen;
672
                                                                        if(len)
673
                                                                        {
674
                                                                                value[len-1] = 0; // terminate string
675
                                                                                if(listname) memcpy(listname, value, len);
676
                                                                        }
677
                                                                        NewWPL_Name = 1;
498 killagreg 678
                                                                }
495 killagreg 679
                                                        }
490 killagreg 680
                                                        else
681
                                                        {
682
                                                                UART1_PutString("Unknown key: ");
683
                                                                UART1_PutString(name);
684
                                                                UART1_PutString("\r\n");
685
                                                        }
686
                                                } // EOF general section
687
                                        } // EOF valid key entry
688
                                } // EOF key entry line
689
                        } // valid line
690
                } // EOF loop over all lines
691
                fclose_(fp);
692
                NaviData.WaypointNumber = WPCount;
516 holgerb 693
                retval = WPL_OK;        
694
                UART1_PutString("ok\r\n");                               
490 killagreg 695
        } // EOF if(Fat16_IsValid())
516 holgerb 696
        else
497 killagreg 697
        {
698
                UART1_PutString("no file system found!\r\n");
699
                retval = WPL_NO_SDCARD_FOUND;
516 holgerb 700
        }      
490 killagreg 701
        return(retval);
491 killagreg 702
}
490 killagreg 703
 
513 killagreg 704
// load actual point list from SD card
705
u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store)
706
{
516 holgerb 707
        u8 filename[30];       
513 killagreg 708
 
709
        pWPL_Store->Name[0] = 0; // clear current list name
710
 
711
        // user absolute path, i.e. leading /
712
        if(pWPL_Store->Index == 0) // index 0 looks for a default WPL file in the root
713
        {
516 holgerb 714
                sprintf(filename, "/default.wpl");     
513 killagreg 715
        }
716
        else
717
        {
718
                sprintf(filename, "/WPL/list_%03d.wpl", pWPL_Store->Index);
719
        }
519 holgerb 720
        return PointList_Load(filename, pWPL_Store->Name, sizeof(pWPL_Store->Name),0);
513 killagreg 721
}
722
 
723
// save actual point list to SD card
724
u8 PointList_WriteToFile(WPL_Store_t * pWPL_Store)
725
{
726
        u8 filename[30];
516 holgerb 727
 
728
 
513 killagreg 729
        if(PointCount == 0) return(WPL_NO_WAYPOINTS);
516 holgerb 730
        // user absolute path, i.e. leading /   
513 killagreg 731
        if(pWPL_Store->Index == 0)
732
        {
733
                sprintf(filename, "/default.wpl");
734
        }
735
        else
736
        {
737
                sprintf(filename, "/WPL/list_%03d.wpl", pWPL_Store->Index);
738
        }
739
        return PointList_Save(filename, pWPL_Store->Name, pWPL_Store->OverwriteFile);
740
}
741
 
742
 
516 holgerb 743
// save actual gps positiin and heading to file
513 killagreg 744
u8 PointList_SaveSinglePoint(WPL_Store_t * pWPL_Store)
745
{
746
        u8 retval = WPL_ERROR;
747
        u8 filename[30];
748
        Point_t WP;
749
 
514 holgerb 750
        UART1_PutString("\n\r write single point\n\r");
516 holgerb 751
        if(GPSData.Position.Status == INVALID)
752
         {
514 holgerb 753
                UART1_PutString("ERROR: No GPS - Fix\n\r");
754
                return(retval);
516 holgerb 755
         }
513 killagreg 756
 
757
        // clear current point list
758
        PointList_Clear();
759
        // prepare WP at current position
519 holgerb 760
 
761
        if(NCFlags & NC_FLAG_FREE || NaviData.TargetPositionDeviation.Distance > 7*10)
762
        {  // take actual position
763
                GPSPos_Copy(&GPSData.Position, &(WP.Position));
764
        }
765
        else
766
        {  // take last target position
767
                GPSPos_Copy(&NaviData.TargetPosition, &(WP.Position));
768
        }
769
 
513 killagreg 770
        GPSPos_Copy(&GPSData.Position, &(WP.Position));
771
        // set heading
772
        WP.Heading = CompassSetpointCorrected/10;
773
        if(WP.Heading == 0) WP.Heading = 360;
519 holgerb 774
        WP.ToleranceRadius = 120; // 12m 
775
        WP.HoldTime  = 2;
513 killagreg 776
        WP.Index  = 1;
777
        WP.Type = POINT_TYPE_WP;
778
        WP.WP_EventChannelValue = 0;
519 holgerb 779
        if(FC.StatusFlags & FC_STATUS_FLY && (FromFC_VarioCharacter != ' ')) // only in flight and if the Altitude control is enabled
516 holgerb 780
         {
519 holgerb 781
          WP.AltitudeRate = 255;  // Auto
782
//        WP.Position.Altitude = NaviData.Altimeter / 2;
783
          WP.Position.Altitude = NaviData.SetpointAltitude / 2;
516 holgerb 784
         }
785
        else
514 holgerb 786
        {
516 holgerb 787
         WP.AltitudeRate = 0;
788
     WP.Position.Altitude = 0;
514 holgerb 789
        }
519 holgerb 790
        WP.Speed = 50; // beim Laden wird der Wert nochmal neu gesetzt
513 killagreg 791
        WP.CamAngle = 0;
521 holgerb 792
        WP.Name[0] = 'S';
793
        WP.Name[1] = 'P';
794
        WP.Name[2] = 'T';
795
        WP.Name[3] = 0;
513 killagreg 796
        // add this point to wp list
797
        PointList_SetAt(&WP);
514 holgerb 798
 
799
        sprintf(filename, "/POINT/POINT%03d.wpl", pWPL_Store->Index);
516 holgerb 800
        UART1_PutString(filename);
514 holgerb 801
        sprintf(pWPL_Store->Name, "POINT%03d ", pWPL_Store->Index);
513 killagreg 802
        retval = PointList_Save(filename, pWPL_Store->Name, 1);
514 holgerb 803
 
804
        // clear current point list
519 holgerb 805
        if((NC_GPS_ModeCharacter != 'w') && (NC_GPS_ModeCharacter != 'W')) PointList_Clear();
806
        else
807
        {
808
                if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
809
                GPS_pWaypoint = PointList_WPBegin(); // updates POI index
810
        }
811
 
513 killagreg 812
        return(retval);
813
}
814
// load target gps posititon and heading from file
815
u8 PointList_LoadSinglePoint(WPL_Store_t * pWPL_Store)
816
{
519 holgerb 817
        u8 filename[30], result = 0;
516 holgerb 818
 
514 holgerb 819
        sprintf(filename, "/POINT/POINT%03d.wpl", pWPL_Store->Index);
513 killagreg 820
        pWPL_Store->Name[0] = 0; // clear current list name
519 holgerb 821
        result = PointList_Load(filename, pWPL_Store->Name, sizeof(pWPL_Store->Name),Parameter.SingleWpSpeed);
822
        if(result) UART1_Request_ReadPoint = 1; // Sends Point 1 to the PC
823
        return(result);
513 killagreg 824
}
825
 
826
 
516 holgerb 827
 
504 holgerb 828
void ClearWLP_Name(void)
829
{
513 killagreg 830
        u8 i;
831
        for(i=0; i<sizeof(WPL_Store.Name);i++) WPL_Store.Name[i] = 0;
832
        NewWPL_Name = 1;
504 holgerb 833
}
513 killagreg 834
 
491 killagreg 835
// move actual point list to ref pos., the point in the list marked by index gets the RefPos afterwards
503 holgerb 836
u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos, u16 RotationAngle)
491 killagreg 837
{
838
        u8 retval = 0;
513 killagreg 839
        s32 altitude;
840
        GPS_Pos_t OldRefPos;
491 killagreg 841
        GPS_Pos_Deviation_t RefDeviation;
516 holgerb 842
 
491 killagreg 843
        // check inputs for plausibility;
516 holgerb 844
        if((RefIndex == 0) || (RefIndex > PointCount)) return(retval); 
491 killagreg 845
        if(pRefPos == NULL) return(retval);
846
        if(pRefPos->Status == INVALID) return(retval);
847
 
513 killagreg 848
        if(GPSPos_Copy(&(PointList[RefIndex-1].Position), &OldRefPos)) // backup old reference position
491 killagreg 849
        {
850
                u8 i;
513 killagreg 851
                // iterate the position list
491 killagreg 852
                for(i = 0; i < PointCount; i++)
853
                {
854
                        retval = 0;
513 killagreg 855
                        // backup altitude of this point
856
                        altitude = PointList[i].Position.Altitude;
857
                        // calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position
858
                        if(!GPSPos_Deviation(&(PointList[i].Position), &OldRefPos, &RefDeviation)) break;
491 killagreg 859
                        // copy of the new reference position into this list place
860
                        if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break;
516 holgerb 861
                        // restore former altitude 
513 killagreg 862
                        PointList[i].Position.Altitude = altitude;
491 killagreg 863
                        // move new reference according to the deviation of the old reference
513 killagreg 864
                        if(RotationAngle > 0)
865
                        {
866
                                retval = GPSPos_ShiftGeodetic(&(PointList[i].Position), (RefDeviation.Bearing + 180 + RotationAngle)%360, RefDeviation.Distance );
867
                                // Now rotate the heading positions if they are fixed angles
868
                                if(PointList[i].Heading >= 0 && PointList[i].Heading <= 360) PointList[i].Heading = (PointList[i].Heading + RotationAngle) % 360;
869
                        }
870
                        else // no rotation
871
                        {
872
                                // move new reference according to the deviation of the old reference
873
                                retval = GPSPos_ShiftCartesian(&(PointList[i].Position), RefDeviation.North, RefDeviation.East);
874
                        }
516 holgerb 875
                        if(!retval) break;             
491 killagreg 876
                }
877
        } // else ref pos old not copied!
878
        if(!retval) PointList_Clear();
879
        return(retval);
880
}
881
 
516 holgerb 882
 
491 killagreg 883