Merge pull request #20 from cpihl/master

Gpx file altitude in meter

src/blackbox_decode.c

@@ -45,6 +45,7 @@ typedef struct decodeOptions_t {
 
     bool overrideSimCurrentMeterOffset, overrideSimCurrentMeterScale;
     int16_t simCurrentMeterOffset, simCurrentMeterScale;
+    float altOffset;
 
     Unit unitGPSSpeed, unitFrameTime, unitVbat, unitAmperage, unitHeight, unitAcceleration, unitRotation, unitFlags;
 } decodeOptions_t;
@@ -55,6 +56,7 @@ decodeOptions_t options = {
     .simulateIMU = false, .imuIgnoreMag = 0,
     .simulateCurrentMeter = false,
     .mergeGPS = 0,
+    .altOffset = 0,
 
     .overrideSimCurrentMeterOffset = false,
     .overrideSimCurrentMeterScale = false,
@@ -452,6 +454,13 @@ void outputGPSFields(flightLog_t *log, FILE *file, int64_t *frame)
     }
 }
 
+/**
+ * Get altitude in [m] from betaflight logged [cm], including optional user altitude offset.
+ */
+float getAltitude(flightLog_t *log, int64_t *frame) {
+	return frame[log->gpsFieldIndexes.GPS_altitude] / 100.0 + options.altOffset; //Change [cm] to [m] for gpx format
+}
+
 void outputGPSFrame(flightLog_t *log, int64_t *frame)
 {
     int64_t gpsFrameTime;
@@ -468,7 +477,7 @@ void outputGPSFrame(flightLog_t *log, int64_t *frame)
 	bool haveRequiredPrecision = log->gpsFieldIndexes.GPS_numSat == -1 || frame[log->gpsFieldIndexes.GPS_numSat] >= MIN_GPS_SATELLITES;
 
     if (haveRequiredFields && haveRequiredPrecision) {
-		gpxWriterAddPoint(gpx, gpsFrameTime, frame[log->gpsFieldIndexes.GPS_coord[0]], frame[log->gpsFieldIndexes.GPS_coord[1]], frame[log->gpsFieldIndexes.GPS_altitude]);
+		gpxWriterAddPoint(gpx, log->dateTime, gpsFrameTime, frame[log->gpsFieldIndexes.GPS_coord[0]], frame[log->gpsFieldIndexes.GPS_coord[1]], getAltitude(log, frame));
     }
 
     createGPSCSVFile(log);
@@ -643,7 +652,7 @@ void onFrameReadyMerge(flightLog_t *log, bool frameValid, int64_t *frame, uint8_
 				bool haveRequiredPrecision = log->gpsFieldIndexes.GPS_numSat == -1 || frame[log->gpsFieldIndexes.GPS_numSat] >= MIN_GPS_SATELLITES;
 
                 if (haveRequiredFields && haveRequiredPrecision) {
-                    gpxWriterAddPoint(gpx, gpsFrameTime, frame[log->gpsFieldIndexes.GPS_coord[0]], frame[log->gpsFieldIndexes.GPS_coord[1]], frame[log->gpsFieldIndexes.GPS_altitude]);
+                    gpxWriterAddPoint(gpx, log->dateTime, gpsFrameTime, frame[log->gpsFieldIndexes.GPS_coord[0]], frame[log->gpsFieldIndexes.GPS_coord[1]], getAltitude(log, frame));
                 }
             }
         break;
@@ -1195,6 +1204,7 @@ void printUsage(const char *argv0)
         "   --unit-acceleration <u>  Acceleration unit (raw|g|m/s2), default is raw\n"
         "   --unit-gps-speed <unit>  GPS speed unit (mps|kph|mph), default is mps (meters per second)\n"
         "   --unit-vbat <unit>       Vbat unit (raw|mV|V), default is V (volts)\n"
+	    "   --alt-offset             Altitude offset (meters), default is zero\n"
         "   --merge-gps              Merge GPS data into the main CSV log file instead of writing it separately\n"
         "   --simulate-current-meter Simulate a virtual current meter using throttle data\n"
         "   --sim-current-meter-scale   Override the FC's settings for the current meter simulation\n"
@@ -1238,6 +1248,7 @@ void parseCommandlineOptions(int argc, char **argv)
         SETTING_UNIT_ACCELERATION,
         SETTING_UNIT_FRAME_TIME,
         SETTING_UNIT_FLAGS,
+		SETTING_ALT_OFFSET,
     };
 
     while (1)
@@ -1266,6 +1277,7 @@ void parseCommandlineOptions(int argc, char **argv)
             {"unit-acceleration", required_argument, 0, SETTING_UNIT_ACCELERATION},
             {"unit-frame-time", required_argument, 0, SETTING_UNIT_FRAME_TIME},
             {"unit-flags", required_argument, 0, SETTING_UNIT_FLAGS},
+            {"alt-offset", required_argument, 0, SETTING_ALT_OFFSET},
             {0, 0, 0, 0}
         };
 
@@ -1347,6 +1359,9 @@ void parseCommandlineOptions(int argc, char **argv)
                 options.overrideSimCurrentMeterOffset = true;
                 options.simCurrentMeterOffset = atoi(optarg);
             break;
+            case SETTING_ALT_OFFSET:
+                options.altOffset = atof(optarg);
+            break;
             case '\0':
                 //Longopt which has set a flag
             break;

src/gpxwriter.c

@@ -1,5 +1,6 @@
 #include <stdlib.h>
 #include <string.h>
+#include <time.h>
 
 #include "gpxwriter.h"
 
@@ -27,7 +28,7 @@ void gpxWriterAddPreamble(gpxWriter_t *gpx)
  * Time is in microseconds since device power-on. Lat and lon are degrees multiplied by GPS_DEGREES_DIVIDER. Altitude
  * is in meters.
  */
-void gpxWriterAddPoint(gpxWriter_t *gpx, int64_t time, int32_t lat, int32_t lon, int16_t altitude)
+void gpxWriterAddPoint(gpxWriter_t *gpx, time_t dateTime, int64_t time, int32_t lat, int32_t lon, float altitude)
 {
     char negSign[] = "-";
     char noSign[] = "";
@@ -52,22 +53,19 @@ void gpxWriterAddPoint(gpxWriter_t *gpx, int64_t time, int32_t lat, int32_t lon,
     char *latSign = ((lat < 0) && (latDegrees == 0)) ? negSign : noSign;
     char *lonSign = ((lon < 0) && (lonDegrees == 0)) ? negSign : noSign;
 
-    fprintf(gpx->file, "  <trkpt lat=\"%s%d.%07u\" lon=\"%s%d.%07u\"><ele>%d</ele>", latSign, latDegrees, latFracDegrees, lonSign, lonDegrees, lonFracDegrees, altitude);
+    fprintf(gpx->file, "  <trkpt lat=\"%s%d.%07u\" lon=\"%s%d.%07u\"><ele>%.2f</ele>", latSign, latDegrees, latFracDegrees, lonSign, lonDegrees, lonFracDegrees, altitude);
 
     if (time != -1) {
-        //We'll just assume that the timespan is less than 24 hours, and make up a date
-        uint32_t hours, mins, secs, frac;
+        //We'll just assume that the timespan is less than 24 hours
+        uint32_t secs, frac;
 
         frac = time % 1000000;
         secs = time / 1000000;
 
-        mins = secs / 60;
-        secs %= 60;
+    	time_t frameTime = dateTime+secs;
+    	struct tm *ftm = localtime (&frameTime);
 
-        hours = mins / 60;
-        mins %= 60;
-
-        fprintf(gpx->file, "<time>2000-01-01T%02u:%02u:%02u.%06uZ</time>", hours, mins, secs, frac);
+        fprintf(gpx->file, "<time>%04u-%02u-%02uT%02u:%02u:%02u.%06uZ</time>", ftm->tm_year + 1900, ftm->tm_mon + 1, ftm->tm_mday, ftm->tm_hour, ftm->tm_min, ftm->tm_sec, frac);
     }
     fprintf(gpx->file, "</trkpt>\n");
 }

src/gpxwriter.h

@@ -16,7 +16,7 @@ typedef struct gpxWriter_t {
     char *filename;
 } gpxWriter_t;
 
-void gpxWriterAddPoint(gpxWriter_t *gpx, int64_t time, int32_t lat, int32_t lon, int16_t altitude);
+void gpxWriterAddPoint(gpxWriter_t *gpx, time_t dateTime, int64_t time, int32_t lat, int32_t lon, float altitude);
 gpxWriter_t* gpxWriterCreate(const char *filename);
 void gpxWriterDestroy(gpxWriter_t* gpx);
 

src/parser.c

@@ -289,6 +289,20 @@ static void identifyFields(flightLog_t * log, uint8_t frameType, flightLogFrameD
     }
 }
 
+static time_t parseDateTime(char* fieldValue) {
+	int year, month, day;
+	struct tm parsedTime;
+	if(sscanf(fieldValue, "%d-%d-%dT%d:%d:%d", &year, &month, &day, &parsedTime.tm_hour, &parsedTime.tm_min, &parsedTime.tm_sec) != EOF){
+		// tm_year is years since 1900
+		parsedTime.tm_year = year - 1900;
+		// tm_months is months since january
+		parsedTime.tm_mon = month - 1;
+		parsedTime.tm_mday = day;
+		parsedTime.tm_isdst = 0; //Ignore daylight savings time for GPS time
+	}
+	return mktime(&parsedTime);;
+}
+
 static size_t parseHeaderLine(flightLog_t *log, mmapStream_t *stream, ParserState *parserState) {
 
     if (streamReadByte(stream) != 'H') {
@@ -426,7 +440,10 @@ static size_t parseHeaderLine(flightLog_t *log, mmapStream_t *stream, ParserStat
         parseCommaSeparatedIntegers(fieldValue, motorOutputs, 2);
         log->sysConfig.motorOutputLow = motorOutputs[0];
         log->sysConfig.motorOutputHigh = motorOutputs[1];
-     }
+     } else if (startsWith(fieldName,"Log start datetime"))  {
+    	log->dateTime = parseDateTime(fieldValue);
+ 	}
+
      return frameSize;
 }
 

src/parser.h

@@ -144,6 +144,7 @@ typedef struct flightLogFrameDef_t {
 struct flightLogPrivate_t;
 
 typedef struct flightLog_t {
+	time_t dateTime; //GPS start date and time
     flightLogStatistics_t stats;
 
     //Information about fields which we need to decode them properly