Subversion Repositories NaviCtrl

Rev

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

Rev Author Line No. Line
85 killagreg 1
/*#######################################################################################*/
2
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
3
/*#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + Copyright (c) 2008 Ingo Busker, Holger Buss
171 ingob 6
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
85 killagreg 7
// + FOR NON COMMERCIAL USE ONLY
8
// + www.MikroKopter.com
9
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
10
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
11
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
12
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
13
// + bzgl. der Nutzungsbedingungen aufzunehmen.
14
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
15
// + Verkauf von Luftbildaufnahmen, usw.
16
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
17
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
18
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
21
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
22
// + eindeutig als Ursprung verlinkt werden
23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
25
// + Benutzung auf eigene Gefahr
26
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
27
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
171 ingob 28
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
85 killagreg 29
// + mit unserer Zustimmung zulässig
30
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
32
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
33
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
34
// + this list of conditions and the following disclaimer.
35
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
36
// +     from this software without specific prior written permission.
37
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
38
// +     for non-commercial use (directly or indirectly)
39
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
40
// +     with our written permission
41
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
42
// +     clearly linked as origin
171 ingob 43
// +   * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
85 killagreg 44
//
45
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
46
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
47
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
48
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
49
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
50
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
51
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
52
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
53
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
54
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
55
// +  POSSIBILITY OF SUCH DAMAGE.
56
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
57
#include <stdlib.h>
58
#include <stdio.h>
59
#include "91x_lib.h"
60
#include "gpx.h"
61
#include "gpx_header.h"
119 killagreg 62
#include "timer1.h"
86 killagreg 63
#include "spi_slave.h"
64
#include "main.h"
65
#include "uart1.h"
338 holgerb 66
#include "compass.h"
67
#include "analog.h"
68
#include "main.h"
69
#include "led.h"
351 holgerb 70
#include "timer2.h"
85 killagreg 71
 
72
//________________________________________________________________________________________________________________________________________
73
// Function:    GPX_DocumentInit(GPX_Document_t *)
74
//
75
// Description: This function initializes the gpx-document for further use.
76
//
77
//
78
// Returnvalue: '1' if document was initialized
79
//________________________________________________________________________________________________________________________________________
80
 
81
u8 GPX_DocumentInit(GPX_Document_t *doc)
82
{
83
        if(doc->state != GPX_DOC_CLOSED) GPX_DocumentClose(doc);                                                // close file if it was opened before
84
        doc->state       = GPX_DOC_CLOSED;                                                                                                      // init state of the gpx-document
85
        doc->file        = NULL;
86
        return(1);
87
}
88
 
89
//________________________________________________________________________________________________________________________________________
90
// Function:    GPX_Document_Open(s8 *name, GPX_Document_t *doc);
91
//
92
// Description: This function opens a new gpx-document with the specified name and creates the document header within the file.
93
//
94
//
95
// Returnvalue: '1' if the gpx-file could be created.
96
//________________________________________________________________________________________________________________________________________
97
 
338 holgerb 98
 
85 killagreg 99
u8 GPX_DocumentOpen(s8 *name, GPX_Document_t *doc)
100
{
338 holgerb 101
        s8 string[60];
85 killagreg 102
        u8 retvalue = 0;
103
 
104
        if(doc == NULL) return(0);
105
        GPX_DocumentInit(doc);                                                                                                          // intialize the document with resetvalues
106
        doc->file = fopen_(name,'a');                                                                                           // open a new file with the specified filename on the memorycard.
107
 
108
        if(doc->file != NULL)                                                                                                           // could the file be opened?
109
        {
110
                retvalue = 1;                                                                                                                   // the document could be created on the drive.
111
                doc->state = GPX_DOC_OPENED;                                                                                            // change document state to opened. At next a placemark has to be opened.
338 holgerb 112
                fwrite_((void*)GPX_DOCUMENT_HEADER1, sizeof(GPX_DOCUMENT_HEADER1)-1,1,doc->file);// write the gpx-header to the document.
339 holgerb 113
                sprintf(string, "<desc>FC HW:%d.%d SW:%d.%d%c + NC HW:%d.%d SW:%d.%d%c</desc>\r\n", FC_Version.Hardware/10,FC_Version.Hardware%10, FC_Version.Major, FC_Version.Minor, 'a'+FC_Version.Patch, Version_HW/10, Version_HW%10, VERSION_MAJOR,  VERSION_MINOR, 'a'+ VERSION_PATCH);
338 holgerb 114
                fputs_(string, doc->file);
115
                fwrite_((void*)GPX_DOCUMENT_HEADER2, sizeof(GPX_DOCUMENT_HEADER2)-1,1,doc->file);// write the gpx-header to the document.
85 killagreg 116
        }
338 holgerb 117
        Logging_FCStatusFlags1 = 0;
118
        Logging_FCStatusFlags2 = 0;
85 killagreg 119
        return(retvalue);
120
}
121
 
122
//________________________________________________________________________________________________________________________________________
123
// Function:    DocumentClose(GPX_Document_t *doc);
124
//
125
// Description: This function closes the document specified by doc.
126
//
127
//
128
// Returnvalue: '1' if the gpx-file could be closed.
129
//________________________________________________________________________________________________________________________________________
130
 
131
u8 GPX_DocumentClose(GPX_Document_t *doc)
132
{
133
 
134
        u8 retvalue = 1;
135
 
136
        if(doc == NULL) return(0);
137
 
138
        while(doc->state != GPX_DOC_CLOSED)                                                             // close linestring, placemark and document before closing the file on the memorycard
94 killagreg 139
        {
85 killagreg 140
                switch(doc->state)
141
                {
142
                        case GPX_DOC_TRACKSEGMENT_OPENED:
143
                                GPX_TrackSegmentEnd(doc);                                               // write terminating tag to end tracksegment.
144
                                break;
145
 
104 killagreg 146
                        case GPX_DOC_TRACK_OPENED:                                                              // write terminating tag to close track.
85 killagreg 147
                                GPX_TrackEnd(doc);
148
                                break;
149
 
150
                        case GPX_DOC_OPENED:                                                                    // close the file on the memorycard
151
                                if(doc->file != NULL)
152
                                {
153
                                        fwrite_((void*)GPX_DOCUMENT_FOOTER, sizeof(GPX_DOCUMENT_FOOTER)-1,1,doc->file); // write the gpx-footer to the document.
154
                                        fclose_(doc->file);
155
                                        retvalue = 1;
156
                                }
157
                                doc->state = GPX_DOC_CLOSED;
158
                                break;
159
 
160
                        default:
161
                                doc->state = GPX_DOC_CLOSED;
162
                                break;
163
                }
164
        }
165
        return(retvalue);
166
}
167
 
338 holgerb 168
 
169
 
85 killagreg 170
//________________________________________________________________________________________________________________________________________
171
// Function:    u8 GPX_TrackBegin(GPX_Document_t);
172
//
173
// Description: This function adds a track to the document.
174
//
175
//
176
// Returnvalue: '1' if the track could be opened
177
//________________________________________________________________________________________________________________________________________
178
 
179
u8 GPX_TrackBegin(GPX_Document_t *doc)
180
{
181
 
182
        u8 retvalue = 0;
183
        if(doc->state == GPX_DOC_OPENED)
184
        {
185
                if(doc->file != NULL)
186
                {
187
                        doc->state = GPX_DOC_TRACK_OPENED;
188
                        retvalue = 1;
189
                        fwrite_((void*)GPX_TRACK_HEADER, sizeof(GPX_TRACK_HEADER)-1,1,doc->file);
190
                }
191
        }
192
        return(retvalue);
193
}
194
 
338 holgerb 195
 
85 killagreg 196
//________________________________________________________________________________________________________________________________________
197
// Function:    u8 GPX_TrackEnd(KML_Document_t *doc)
198
//
199
// Description: This function ends the track opened before.
200
//
201
//
202
// Returnvalue: 1' if the track could be closed
203
//________________________________________________________________________________________________________________________________________
204
 
205
u8 GPX_TrackEnd(GPX_Document_t *doc)
206
{
207
 
208
        u8 retvalue = 0;
209
 
210
        if(doc->state == GPX_DOC_TRACK_OPENED)
211
        {
212
                if(doc->file != NULL)
213
                {
214
                        doc->state = GPX_DOC_OPENED;
215
                        fwrite_((void*)GPX_TRACK_FOOTER, sizeof(GPX_TRACK_FOOTER)-1,1,doc->file);
216
                        retvalue = 1;
217
                }
218
        }
219
 
220
        return(retvalue);
221
}
222
 
223
//________________________________________________________________________________________________________________________________________
224
// Function:    u8 GPX_TrackSegmentBegin(GPX_Document_t *doc);
225
//
226
// Description: This function starts a track segment.
227
//
228
//
229
// Returnvalue: '1' if the track segement could be started
230
//________________________________________________________________________________________________________________________________________
231
 
232
u8 GPX_TrackSegmentBegin(GPX_Document_t *doc)
233
{
234
 
235
        u8 retvalue = 0;
236
 
237
        if(doc->state == GPX_DOC_TRACK_OPENED)
238
        {
239
                if(doc->file != NULL)
240
                {
241
                        doc->state = GPX_DOC_TRACKSEGMENT_OPENED;
242
                        fwrite_((void*)GPX_TRACKSEGMENT_HEADER, sizeof(GPX_TRACKSEGMENT_HEADER)-1,1,doc->file);
243
                        retvalue = 1;
244
                }
245
        }
246
        return(retvalue);
247
}
248
 
249
//________________________________________________________________________________________________________________________________________
250
// Function:    u8 GPX_TrackSegmentEnd(GPX_Document_t *doc);
251
//
252
// Description: This function ends the tracksegment opened before.
253
//
254
//
255
// Returnvalue: '1' if the track segment could be terminated
256
//________________________________________________________________________________________________________________________________________
257
 
258
u8 GPX_TrackSegmentEnd(GPX_Document_t *doc)
259
{
260
 
261
        u8 retvalue = 0;
262
        if(doc->state == GPX_DOC_TRACKSEGMENT_OPENED)
263
        {
264
                if(doc->file != NULL)
265
                {
266
                        doc->state = GPX_DOC_TRACK_OPENED;
267
                        fwrite_((void*)GPX_TRACKSEGMENT_FOOTER, sizeof(GPX_TRACKSEGMENT_FOOTER)-1,1,doc->file);
268
                        retvalue = 1;
269
                }
270
        }
271
        return(retvalue);
272
}
273
 
274
//________________________________________________________________________________________________________________________________________
275
// Function:    u8 GPX_TrackSegementAddPoint(GPS_Pos_t * pGPS_Position ,GPX_Document_t *doc);
276
//
277
// Description: This function adds a pointof a track segement to the specified document.
278
//
279
//
280
// Returnvalue: '1' if a point was added
281
//________________________________________________________________________________________________________________________________________
282
 
345 holgerb 283
u8 GPX_TrackSegementAddPoint(GPX_Document_t *doc,unsigned char part)
85 killagreg 284
{
285
 
286
        u8 retvalue = 0;
338 holgerb 287
        s8 string[100];
85 killagreg 288
 
289
        if(doc == NULL) return(0);
290
 
215 killagreg 291
        if((GPSData.Position.Status != INVALID) && (NaviData.HomePosition.Status != INVALID))
85 killagreg 292
        {
293
                if(doc->state == GPX_DOC_TRACKSEGMENT_OPENED)
294
                {
295
                        if(doc->file != NULL)
296
                        {
86 killagreg 297
                                s32 i32_1, i32_2;
298
                                s16 i16_1;
299
                                u8 u8_1, u8_2;
85 killagreg 300
                                // write <trkpt> tag
345 holgerb 301
                        switch(part)
302
                          {
303
                           case 0:
86 killagreg 304
                                if(GPSData.Position.Latitude < 0) u8_1 = '-';
305
                                else u8_1 = '+';
306
                                i32_1 = abs(GPSData.Position.Latitude)/10000000L;
307
                                i32_2 = abs(GPSData.Position.Latitude)%10000000L;
308
                                sprintf(string, "<trkpt lat=\"%c%ld.%07ld\" ",u8_1, i32_1, i32_2);
85 killagreg 309
                                fputs_(string, doc->file);
86 killagreg 310
                                if(GPSData.Position.Longitude < 0) u8_1 = '-';
311
                                else u8_1 = '+';
312
                                i32_1 = abs(GPSData.Position.Longitude)/10000000L;
313
                                i32_2 = abs(GPSData.Position.Longitude)%10000000L;
314
                                sprintf(string, "lon=\"%c%ld.%07ld\">\r\n",u8_1, i32_1, i32_2);
85 killagreg 315
                                fputs_(string, doc->file);
345 holgerb 316
                           break;
317
                           case 1:
85 killagreg 318
                                // write <ele> taga
215 killagreg 319
                                i32_2 = GPSData.Position.Altitude - NaviData.HomePosition.Altitude;
86 killagreg 320
                                if(i32_2 < 0) i32_2 = 0; // avoid negative altitudes in log
321
                                i32_1 = i32_2/1000L;
322
                                i32_2 = i32_2%1000L;
323
                                sprintf(string,"<ele>%ld.%03ld</ele>\r\n",i32_1, i32_2);
85 killagreg 324
                                fputs_(string, doc->file);
325
                                // write <time> tag     only at a resolution of one second
326
                                sprintf(string, "<time>%04d-%02d-%02dT%02d:%02d:%02dZ</time>\r\n",SystemTime.Year, SystemTime.Month, SystemTime.Day, SystemTime.Hour, SystemTime.Min, SystemTime.Sec);
327
                                fputs_(string, doc->file);
86 killagreg 328
                                // write <sat> tag
329
                                sprintf(string, "<sat>%d</sat>\r\n", NaviData.SatsInUse);
330
                                fputs_(string, doc->file);
94 killagreg 331
                                // todo: add  <extensions> tag with additional data to be logged
86 killagreg 332
                                sprintf(string, "<extensions>\r\n");
333
                                fputs_(string, doc->file);
345 holgerb 334
                           break;
335
                           case 2:
125 killagreg 336
                                // Altimeter according to air pressure
338 holgerb 337
                                sprintf(string, "<Altimeter>%d,'%c'</Altimeter>\r\n", NaviData.Altimeter,FromFC_VarioCharacter);
126 killagreg 338
                                fputs_(string, doc->file);
140 killagreg 339
                                // Variometer according to air pressure
340
                                sprintf(string, "<Variometer>%d</Variometer>\r\n", NaviData.Variometer);
341
                                fputs_(string, doc->file);
92 killagreg 342
                                // Course in deg
86 killagreg 343
                                i16_1 = GPSData.Heading/100000L;
344
                                sprintf(string, "<Course>%03d</Course>\r\n", i16_1);
345
                                fputs_(string, doc->file);
92 killagreg 346
                                // Ground Speed in cm/s
86 killagreg 347
                                sprintf(string, "<GroundSpeed>%d</GroundSpeed>\r\n", NaviData.GroundSpeed);
348
                                fputs_(string, doc->file);
140 killagreg 349
                                // Vertical Speed in cm/s
350
                                sprintf(string, "<VerticalSpeed>%d</VerticalSpeed>\r\n", NaviData.TopSpeed);
351
                                fputs_(string, doc->file);
352
                                // Flight duration
101 holgerb 353
                                sprintf(string, "<FlightTime>%d</FlightTime>\r\n", NaviData.FlyingTime);
354
                                fputs_(string, doc->file);
345 holgerb 355
                           break;
356
                           case 3:
86 killagreg 357
                                // Ubat
358
                                u8_1 = NaviData.UBat / 10;
359
                                u8_2 = NaviData.UBat % 10;
360
                                sprintf(string, "<Voltage>%d.%01d</Voltage>\r\n", u8_1, u8_2);
361
                                fputs_(string, doc->file);
206 killagreg 362
                                // Current
363
                                u8_1 = NaviData.Current / 10;
364
                                u8_2 = NaviData.Current % 10;
365
                                sprintf(string, "<Current>%d.%01d</Current>\r\n", u8_1, u8_2);
366
                                fputs_(string, doc->file);
367
                                // Capacity
368
                                sprintf(string, "<Capacity>%d</Capacity>\r\n", NaviData.UsedCapacity);
369
                                fputs_(string, doc->file);
86 killagreg 370
                                // RC Quality
371
                                sprintf(string, "<RCQuality>%d</RCQuality>\r\n", FC.RC_Quality);
372
                                fputs_(string, doc->file);
125 killagreg 373
                                // RC Received Signal Strength Indication
345 holgerb 374
                           break;
375
                           case 4:
125 killagreg 376
                                sprintf(string, "<RCRSSI>%d</RCRSSI>\r\n", FC.RC_RSSI);
377
                                fputs_(string, doc->file);
92 killagreg 378
                                // Compassind deg
86 killagreg 379
                                i16_1 = FromFlightCtrl.GyroHeading / 10;
338 holgerb 380
                                sprintf(string, "<Compass>%03d,%03d</Compass>\r\n", i16_1,ToFlightCtrl.CompassHeading);
86 killagreg 381
                                fputs_(string, doc->file);
92 killagreg 382
                                // Nick Angle ind deg
86 killagreg 383
                                sprintf(string, "<NickAngle>%03d</NickAngle>\r\n", NaviData.AngleNick);
384
                                fputs_(string, doc->file);
92 killagreg 385
                                // Roll Angle in deg
86 killagreg 386
                                sprintf(string, "<RollAngle>%03d</RollAngle>\r\n", NaviData.AngleRoll);
387
                                fputs_(string, doc->file);
345 holgerb 388
                           break;
389
                           case 5:
338 holgerb 390
                                // magnetic field 
340 holgerb 391
                                sprintf(string, "<MagnetField>%03d</MagnetField>\r\n",(u16) (EarthMagneticFieldFiltered/5));
338 holgerb 392
                                fputs_(string, doc->file);
393
                                // magnetic inclination & error 
340 holgerb 394
                                sprintf(string, "<MagnetInclination>%02d,%02d</MagnetInclination>\r\n",(s16)EarthMagneticInclinationFiltered/10,(s16)(EarthMagneticInclinationFiltered/10 - EarthMagneticInclinationTheoretic));
338 holgerb 395
                                fputs_(string, doc->file);
396
                                // BL Information
397
                                sprintf(string, "<MotorCurrent>%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d</MotorCurrent>\r\n",MotorCurrent[0],MotorCurrent[1],MotorCurrent[2],MotorCurrent[3],MotorCurrent[4],MotorCurrent[5],MotorCurrent[6],MotorCurrent[7],MotorCurrent[8],MotorCurrent[9],MotorCurrent[10],MotorCurrent[11]);
398
                                fputs_(string, doc->file);
399
                                sprintf(string, "<BL_Temperature>%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d</BL_Temperature>\r\n",MotorTemperature[0],MotorTemperature[1],MotorTemperature[2],MotorTemperature[3],MotorTemperature[4],MotorTemperature[5],MotorTemperature[6],MotorTemperature[7],MotorTemperature[8],MotorTemperature[9],MotorTemperature[10],MotorTemperature[11]);
400
                                fputs_(string, doc->file);
345 holgerb 401
                           break;
402
                           case 6:
338 holgerb 403
                                sprintf(string, "<AvaiableMotorPower>%03d</AvaiableMotorPower>\r\n",BL_MinOfMaxPWM);
404
                                fputs_(string, doc->file);
405
                                sprintf(string, "<FC_I2C_ErrorCounter>%03d</FC_I2C_ErrorCounter>\r\n",(s16)FC_I2C_ErrorConter);
406
                                fputs_(string, doc->file);
407
                                // Analog inputs of the NC
408
                                sprintf(string, "<AnalogInputs>%d,%d,%d,%d</AnalogInputs>\r\n",AnalogData.Ch4,AnalogData.Ch5,AnalogData.Ch6,AnalogData.Ch7);
409
                                fputs_(string, doc->file);
92 killagreg 410
                                // NC Mode (contains the status)
338 holgerb 411
                                sprintf(string, "<NCFlag>0x%02X</NCFlag>\r\n", NCFlags);
86 killagreg 412
                                fputs_(string, doc->file);
351 holgerb 413
                                sprintf(string, "<Servo>%d,%d,%d</Servo>\r\n", ServoParams.NickControl,ServoParams.RollControl,POI_KameraNick/10);      // Raw Poti-Values of the Servo control and the POI_Nick in 1°
414
                                fputs_(string, doc->file);
415
                                sprintf(string, "<WP>%d,%d,%d</WP>\r\n",NaviData.WaypointIndex,NaviData.WaypointNumber,FC_WP_EventChannel);      // x of y Waypoints and the actual value of the Event Channel
416
                                fputs_(string, doc->file);
345 holgerb 417
                           break;
418
                           case 7:
338 holgerb 419
                                // Flags
420
                                sprintf(string, "<FCFlags2>0x%02x,0x%02x</FCFlags2>\r\n",Logging_FCStatusFlags1,Logging_FCStatusFlags2);
421
                                fputs_(string, doc->file);
422
                                Logging_FCStatusFlags1 = 0;
423
                                Logging_FCStatusFlags2 = 0;
233 holgerb 424
                                // Status of the complete MikroKopter
425
                                sprintf(string, "<ErrorCode>%03d</ErrorCode>\r\n",ErrorCode);
426
                                fputs_(string, doc->file);
92 killagreg 427
                                // Target Bearing in deg
86 killagreg 428
                                sprintf(string, "<TargetBearing>%03d</TargetBearing>\r\n", NaviData.TargetPositionDeviation.Bearing);
429
                                fputs_(string, doc->file);
92 killagreg 430
                                // Target Distance in dm
86 killagreg 431
                                sprintf(string, "<TargetDistance>%d</TargetDistance>\r\n", NaviData.TargetPositionDeviation.Distance);
432
                                fputs_(string, doc->file);
345 holgerb 433
                           break;
434
                           case 8:
101 holgerb 435
                                // RC Sticks as Nick/Roll/Yaw
338 holgerb 436
                                sprintf(string, "<RCSticks>%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d</RCSticks>\r\n", FC.StickNick,FC.StickRoll, FC.StickYaw, FC.StickGas,FC.Poti[0],FC.Poti[1],FC.Poti[2],FC.Poti[3],FC.Poti[4],FC.Poti[5],FC.Poti[6],FC.Poti[7]);
101 holgerb 437
                                fputs_(string, doc->file);
94 killagreg 438
                                // GPS Sticks as Nick/Roll/Yaw
338 holgerb 439
                                sprintf(string, "<GPSSticks>%d,%d,%d,'%c'</GPSSticks>\r\n", ToFlightCtrl.GPSStick.Nick, ToFlightCtrl.GPSStick.Roll, ToFlightCtrl.GPSStick.Yaw,NC_GPS_ModeCharacter);
94 killagreg 440
                                fputs_(string, doc->file);
86 killagreg 441
                                // eof extensions
442
                                sprintf(string, "</extensions>\r\n");
443
                                fputs_(string, doc->file);
85 killagreg 444
                                sprintf(string, "</trkpt>\r\n");
445
                                fputs_(string, doc->file);
345 holgerb 446
                           break;
447
                          }
448
                        retvalue = 1;
85 killagreg 449
                        }
450
                }
451
        }
452
        return(retvalue);
453
}
454
 
455
//________________________________________________________________________________________________________________________________________
456
// Function:    u8 KML_LoggGPSCoordinates(gps_data_t *, KML_Document_t *)
457
//
458
// Description: This function opens and adds gpscoordinates to an GPX-Document. The document will be opened, if not already done
459
//
460
//
461
// Returnvalue: '1' if an gps coordinate was logged
462
//________________________________________________________________________________________________________________________________________
463
 
345 holgerb 464
u8 GPX_LoggGPSCoordinates(GPX_Document_t *doc,unsigned char part)
85 killagreg 465
{
466
        u8 retval = 0;
467
        while(doc->state != GPX_DOC_TRACKSEGMENT_OPENED)                                // automatic create document with default filename on the card.
468
        {
469
                switch(doc->state)
470
                {
471
                        case GPX_DOC_CLOSED:                                                                    // document hasn't been opened yet therefore it will be initialized automatically
324 killagreg 472
                                retval = GPX_DocumentOpen("default.gpx",doc);           // open the gpx-document with a standardname.
85 killagreg 473
                        break;
474
 
475
                        case GPX_DOC_OPENED:                                                                    // if a document has been opened before but no track exists:
476
                                retval = GPX_TrackBegin(doc);
477
                        break;
478
 
479
                        case GPX_DOC_TRACK_OPENED:                                                              // add tracksegement to the track
480
                                retval = GPX_TrackSegmentBegin(doc);
481
                        break;
482
 
483
                        default:
484
                                retval = 0;
485
                        break;
486
 
487
                }
488
                if(retval != 1) return(retval); // stop on error
489
        }
490
 
491
        if(doc->state == GPX_DOC_TRACKSEGMENT_OPENED)                                           // if the document was opened add coordinates to the document.
492
        {
345 holgerb 493
                retval = GPX_TrackSegementAddPoint(doc,part);                                           // add a track segment point
85 killagreg 494
        }
495
        return(retval);
496
}
497