Subversion Repositories NaviCtrl

Rev

Rev 85 | Rev 92 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 85 Rev 86
Line 58... Line 58...
58 
#include <stdio.h>
 58 
#include <stdio.h>
59 
#include "91x_lib.h"
 59 
#include "91x_lib.h"
60 
#include "gpx.h"
 60 
#include "gpx.h"
61 
#include "gpx_header.h"
 61 
#include "gpx_header.h"
62 
#include "timer.h"
 62 
#include "timer.h"
- 
 
 63 
#include "spi_slave.h"
- 
 
 64 
#include "main.h"
- 
 
 65 
#include "uart1.h"
Line 63... Line 66...
63 
 
 66 
 
64 
//________________________________________________________________________________________________________________________________________
 67 
//________________________________________________________________________________________________________________________________________
65 
// Function:    GPX_DocumentInit(GPX_Document_t *)
 68 
// Function:    GPX_DocumentInit(GPX_Document_t *)
66 
//
 69 
//
Line 267... Line 270...
267 
u8 GPX_TrackSegementAddPoint(GPX_Document_t *doc)
 270 
u8 GPX_TrackSegementAddPoint(GPX_Document_t *doc)
268 
{
 271 
{
Line 269... Line 272...
269 
 
 272 
 
270 
        u8 retvalue = 0;
 273 
        u8 retvalue = 0;
271 
        s8 string[50];
 - 
 
Line 272... Line 274...
272 
        s32 rel_altitude = 0;
 274 
        s8 string[50];
Line 273... Line 275...
273 
 
 275 
 
274 
        if(doc == NULL) return(0);
 276 
        if(doc == NULL) return(0);
275 
 
 277 
 
276 
        if((GPSData.Position.Status != INVALID) && (GPS_HomePosition.Status != INVALID))
 278 
        if((GPSData.Position.Status != INVALID) && (GPS_HomePosition.Status != INVALID))
277 
        {
 279 
        {
278 
                if(doc->state == GPX_DOC_TRACKSEGMENT_OPENED)
 280 
                if(doc->state == GPX_DOC_TRACKSEGMENT_OPENED)
279 
                {
 281 
                {
280 
                        if(doc->file != NULL)
 282 
                        if(doc->file != NULL)
- 
 
 283 
                        {
281 
                        {
 284 
                                s32 i32_1, i32_2;
282 
                                s32 i1, i2;
 285 
                                s16 i16_1;
283 
                                u8 sign;
 286 
                                u8 u8_1, u8_2;
284 
                                // write <trkpt> tag
 287 
                                // write <trkpt> tag
285 
                                if(GPSData.Position.Latitude < 0) sign = '-';
 288 
                                if(GPSData.Position.Latitude < 0) u8_1 = '-';
286 
                                else sign = '+';
 289 
                                else u8_1 = '+';
287 
                                i1 = abs(GPSData.Position.Latitude)/10000000L;
 290 
                                i32_1 = abs(GPSData.Position.Latitude)/10000000L;
288 
                                i2 = abs(GPSData.Position.Latitude)%10000000L;
 291 
                                i32_2 = abs(GPSData.Position.Latitude)%10000000L;
289 
                                sprintf(string, "<trkpt lat=\"%c%ld.%07ld\" ",sign, i1, i2);
 292 
                                sprintf(string, "<trkpt lat=\"%c%ld.%07ld\" ",u8_1, i32_1, i32_2);
290 
                                fputs_(string, doc->file);
 293 
                                fputs_(string, doc->file);
291 
                                if(GPSData.Position.Longitude < 0) sign = '-';
 294 
                                if(GPSData.Position.Longitude < 0) u8_1 = '-';
292 
                                else sign = '+';
 295 
                                else u8_1 = '+';
293 
                                i1 = abs(GPSData.Position.Longitude)/10000000L;
 296 
                                i32_1 = abs(GPSData.Position.Longitude)/10000000L;
294 
                                i2 = abs(GPSData.Position.Longitude)%10000000L;
 297 
                                i32_2 = abs(GPSData.Position.Longitude)%10000000L;
295 
                                sprintf(string, "lon=\"%c%ld.%07ld\">\r\n",sign, i1, i2);
 298 
                                sprintf(string, "lon=\"%c%ld.%07ld\">\r\n",u8_1, i32_1, i32_2);
296 
                                fputs_(string, doc->file);
 299 
                                fputs_(string, doc->file);
297 
                                // write <ele> taga
 300 
                                // write <ele> taga
298 
                                rel_altitude = GPSData.Position.Altitude - GPS_HomePosition.Altitude;
 301 
                                i32_2 = GPSData.Position.Altitude - GPS_HomePosition.Altitude;
299 
                                if(rel_altitude < 0) rel_altitude = 0; // avoid negative altitudes in log
 302 
                                if(i32_2 < 0) i32_2 = 0; // avoid negative altitudes in log
300 
                                i1 = rel_altitude/1000L;
 303 
                                i32_1 = i32_2/1000L;
301 
                                i2 = rel_altitude%1000L;
 304 
                                i32_2 = i32_2%1000L;
302 
                                sprintf(string,"<ele>%ld.%03ld</ele>\r\n",i1, i2);
 305 
                                sprintf(string,"<ele>%ld.%03ld</ele>\r\n",i32_1, i32_2);
303 
                                fputs_(string, doc->file);
 306 
                                fputs_(string, doc->file);
304 
                                // write <time> tag     only at a resolution of one second
 - 
 
- 
 
 307 
                                // write <time> tag     only at a resolution of one second
- 
 
 308 
                                sprintf(string, "<time>%04d-%02d-%02dT%02d:%02d:%02dZ</time>\r\n",SystemTime.Year, SystemTime.Month, SystemTime.Day, SystemTime.Hour, SystemTime.Min, SystemTime.Sec);
- 
 
 309 
                                fputs_(string, doc->file);
305 
                                sprintf(string, "<time>%04d-%02d-%02dT%02d:%02d:%02dZ</time>\r\n",SystemTime.Year, SystemTime.Month, SystemTime.Day, SystemTime.Hour, SystemTime.Min, SystemTime.Sec);
 310 
                                // write <sat> tag
- 
 
 311 
                                sprintf(string, "<sat>%d</sat>\r\n", NaviData.SatsInUse);
- 
 
 312 
                                fputs_(string, doc->file);
- 
 
 313 
                                // todo: add  <extensions> tag with additional data to be logged
- 
 
 314 
                                sprintf(string, "<extensions>\r\n");
- 
 
 315 
                                fputs_(string, doc->file);
- 
 
 316 
                                // Course
- 
 
 317 
                                i16_1 = GPSData.Heading/100000L;
- 
 
 318 
                                sprintf(string, "<Course>%03d</Course>\r\n", i16_1);
- 
 
 319 
                                fputs_(string, doc->file);
306 
                                fputs_(string, doc->file);
 320 
                                // Ground Speed
- 
 
 321 
                                sprintf(string, "<GroundSpeed>%d</GroundSpeed>\r\n", NaviData.GroundSpeed);
- 
 
 322 
                                fputs_(string, doc->file);
- 
 
 323 
                                // Ubat
- 
 
 324 
                                u8_1 = NaviData.UBat / 10;
- 
 
 325 
                                u8_2 = NaviData.UBat % 10;
- 
 
 326 
                                sprintf(string, "<Voltage>%d.%01d</Voltage>\r\n", u8_1, u8_2);
- 
 
 327 
                                fputs_(string, doc->file);
- 
 
 328 
                                // RC Quality
- 
 
 329 
                                sprintf(string, "<RCQuality>%d</RCQuality>\r\n", FC.RC_Quality);
- 
 
 330 
                                fputs_(string, doc->file);
- 
 
 331 
                                // Compass
- 
 
 332 
                                i16_1 = FromFlightCtrl.GyroHeading / 10;
- 
 
 333 
                                sprintf(string, "<Compass>%03d</Compass>\r\n", i16_1);
- 
 
 334 
                                fputs_(string, doc->file);
- 
 
 335 
                                // Nick Angle
- 
 
 336 
                                sprintf(string, "<NickAngle>%03d</NickAngle>\r\n", NaviData.AngleNick);
- 
 
 337 
                                fputs_(string, doc->file);
- 
 
 338 
                                // Roll Angle
- 
 
 339 
                                sprintf(string, "<RollAngle>%03d</RollAngle>\r\n", NaviData.AngleRoll);
- 
 
 340 
                                fputs_(string, doc->file);
- 
 
 341 
                                // GPS Mode
- 
 
 342 
                                if(NaviData.NCFlags & NC_FLAG_FREE)     sprintf(string, "<GPS_Mode>FREE</GPS_Mode>\r\n");
- 
 
 343 
                                else if(NaviData.NCFlags & NC_FLAG_PH)  sprintf(string, "<GPS_Mode>PH</GPS_Mode>\r\n");
- 
 
 344 
                                else if(NaviData.NCFlags & NC_FLAG_CH)  sprintf(string, "<GPS_Mode>CH</GPS_Mode>\r\n");
- 
 
 345 
                                fputs_(string, doc->file);
- 
 
 346 
                                // Target Bearing
- 
 
 347 
                                sprintf(string, "<TargetBearing>%03d</TargetBearing>\r\n", NaviData.TargetPositionDeviation.Bearing);
- 
 
 348 
                                fputs_(string, doc->file);
- 
 
 349 
                                // Target Distance
- 
 
 350 
                                sprintf(string, "<TargetDistance>%d</TargetDistance>\r\n", NaviData.TargetPositionDeviation.Distance);
- 
 
 351 
                                fputs_(string, doc->file);
307 
 
 352 
                                // eof extensions
308 
                                // todo: add  <extensions> tag with additional data to be logged
353 
                                sprintf(string, "</extensions>\r\n");
309 
 
 354 
                                fputs_(string, doc->file);
310 
                                sprintf(string, "</trkpt>\r\n");
 355 
                                sprintf(string, "</trkpt>\r\n");
311 
                                fputs_(string, doc->file);
 356 
                                fputs_(string, doc->file);