fix bugs and add features to Computrainer3dpFile

Specifically:

1.  The previous code assumed the wrong units while extracting
    speed and distance from a .3dp file.  Computrainer stores
    speed in (miles per hour / 160), and distance in kilometers.
    This patch converts .3dp speed/distance data points into
    kph and km correctly.  As a side-effect, speed and distance
    are displayed correctly in GC windows and calculations.

2.  This patch adds code to extract altitude data from a .3dp
    file and include it in a ride.

3.  .3dp files do not have a consistent inter-datapoint time
    interval.  Since GC expects one, the earlier version of this
    code averaged 1000 data points from the middle of the ride to
    estimate this interval.  Unfortunately, this approach caused
    a bunch of problems for various calculations that GC does,
    such as calculating the riding time (vs. workout time),
    average speed, xPower, critical power plot and FTP, and so
    on.  [GC assumes that # data points * inter-datapoint-interval
    = workout time, but this isn't true when you used an estimated
    interval.]

    To fix this, this patch adds averaging and interpolation code
    to covert the data point sequence in the .3dp file to an
    averaged sequence with a data point every 250ms.  Since the
    inter-data-point interval is now fixed, these calculation bugs
    went away, and correct values are now calculated and displayed
    by GC.

4.  Fix (3.) has another useful side-effect:  the number of data
    points per ride given to GC goes down by 10x.  (Raw .3dp files
    have a data point every 30-50ms.  This averaging/smoothing
    code emits a data point every 250ms.)  Since the critical
    power calculation is an O(n^2) calculation, the time for
    this calculation is reduced by 100x.  Instead of an hour
    to do the calculation for a typical 2hr ride, it now takes
    less than a minute.

5.  The code was cleaned up in several regards:  comments
    were added to help document the .3dp format and explain
    the averaging/smoothing code, and types from boost/cstdint.hpp
    were used instead of native C types when using a variable
    of a specific size  (e.g., the code now uses uint16_t instead
    of unsigned short, etc.).

This patch was built by Steve Gribble and Daniel Stark.

src/Computrainer3dpFile.cpp

@@ -3,6 +3,10 @@
  *                     Justin F. Knotzke (jknotzke@shampoo.ca)
  * Copyright (c)  2009 Greg Lonnon (greg.lonnon@gmail.com)
  *
+ * Additional contributions from:
+ *     Steve Gribble  (gribble [at] cs.washington.edu)  [December 3, 2009]
+ *     Daniel Stark [December 3, 2009]
+ *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License as published by the Free
  * Software Foundation; either version 2 of the License, or (at your option)
@@ -19,19 +23,22 @@
  */
 
 #include "Computrainer3dpFile.h"
-#include <QRegExp>
-#include <QTextStream>
+
 #include <QDateTime>
-#include <QString>
-#include <algorithm>            // for std::sort
-#include <assert.h>
-#include "math.h"
-#include <iostream>
 #include <QDebug>
-#include "Units.h"
+#include <QRegExp>
+#include <QString>
+#include <QTextStream>
 #include <QVector>
 
-typedef unsigned char ubyte;
+#include <algorithm>            // for std::sort
+#include <assert.h>
+#include <boost/cstdint.hpp>    // for int8_t, int16_t, etc.
+#include <iostream>
+
+#include "math.h"
+#include "Units.h"
+
 
 static int Computrainer3dpFileReaderRegistered =
     RideFileFactory::instance().registerReader("3dp",
@@ -44,71 +51,242 @@ RideFile *Computrainer3dpFileReader::openRideFile(QFile & file,
                                                   QStringList & errors)
     const
 {
+    // open up the .3dp file, prepare a little-endian-ordered
+    // QDataStream
     if (!file.open(QFile::ReadOnly)) {
         errors << ("Could not open ride file: \"" + file.fileName() +
                    "\"");
         return NULL;
     }
-
     RideFile *rideFile = new RideFile();
     QDataStream is(&file);
-
     is.setByteOrder(QDataStream::LittleEndian);
 
+    // start parsing the header
+
     // looks like the first part is a header... ignore it.
     is.skipRawData(4);
-    char perfStr[5];
 
+    // the next 4 bytes are the ASCII characters 'Perf'
+    char perfStr[5];
     is.readRawData(perfStr, 4);
     perfStr[4] = NULL;
+
+    // not sure what the next 8 bytes are; skip them
     is.skipRawData(0x8);
+
+    // the next 65 bytes are a null-terminated and padded
+    // ASCII user name string
     char userName[65];
     is.readRawData(userName, 65);
-    ubyte age;
+
+    // next is a single byte of user age, in years.  I guess
+    // Computrainer doesn't allow people to get older than 255
+    // years. ;)
+    uint8_t age;
     is >> age;
+
+    // not sure what the next 6 bytes are; skip them.
     is.skipRawData(6);
+
+    // next is a (4 byte) C-style floating point with weight in kg
     float weight;
     is >> weight;
-    int upperHR;
+
+    // next is the upper heart rate limit (4 byte int)
+    uint32_t upperHR;
     is >> upperHR;
-    int lowerHR;
+
+    // and then the resting heart rate (4 byte int)
+    uint32_t lowerHR;
     is >> lowerHR;
-    int year;
+
+    // then year, month, day, hour, minute the exercise started
+    // (4, 1, 1, 1, 1 bytes)
+    uint32_t year;
     is >> year;
-    ubyte month;
+    uint8_t month;
     is >> month;
-    ubyte day;
+    uint8_t day;
     is >> day;
-    ubyte hour;
+    uint8_t hour;
     is >> hour;
-    ubyte minute;
+    uint8_t minute;
     is >> minute;
-    int numberSamples;
+
+    // the number of exercise data points in the file (4 byte int)
+    uint32_t numberSamples;
     is >> numberSamples;
-    // go back to the start, and go to the start of the data samples
+
+    // go back to the start, and skip header to go to
+    // the start of the data samples.
     file.seek(0);
     is.skipRawData(0xf8);
+
+    // we'll keep track of the altitude over time.  since computrainer
+    // gives us slope, we can calculate change in altitude if we know
+    // change in distance traveled, so we also need to keep track of
+    // the previous sample's distance.
+    float altitude = 100.0;  // arbitrary starting altitude of 100m
+    float lastKM = 0;
+
+    // computrainer doesn't have a fixed inter-sample-interval; GC
+    // expects one, and estimating one by averaging causes problems
+    // for some calculations that GC does.  also, computrainer samples
+    // so frequently (once every 30-50ms) that the O(n^2) critical
+    // power plot calculation takes waaaaay too long.  to solve both
+    // problems at once, we smooth the file, emitting an averaged data
+    // point every 250 milliseconds.
+    //
+    // for HR, cadence, watts, and speed, we'll do time averaging to
+    // figure out the correct average since the last emitted point.
+    // for distance and altitude, we just need to interpolate from the
+    // last data point in the computrainer file itself.
+    float lastAltitude = 100.0;
+    uint32_t lastEmittedMS = 0;
+    uint32_t lastSampleMS = 0;
+    double hr_sum = 0.0, cad_sum=0.0, speed_sum=0.0, watts_sum=0.0;
+
+#define CT_EMIT_MS 250
+
+    // loop over each sample in the file, do the averaging, interpolation,
+    // and emit smoothed points every CT_EMIT_MS milliseconds
     for (; numberSamples; numberSamples--) {
-        ubyte hr;
+
+        // 1 byte heart rate, in BPM
+        uint8_t hr;
         is >> hr;
-        ubyte cad;
+
+        // 1 byte cadence, in RPM
+        uint8_t cad;
         is >> cad;
-        unsigned short watts;
+
+        // 2 unsigned bytes of watts
+        uint16_t watts;
         is >> watts;
+
+        // 4 bytes of floating point speed (in mph/160 !!)
         float speed;
         is >> speed;
-        speed = speed * KM_PER_MILE * 100;
-        int ms;
-        is >> ms;
-        is.skipRawData(4);
-        float miles;
-        is >> miles;
-        float km = miles * KM_PER_MILE;
-        is.skipRawData(0x1c);
-        rideFile->appendPoint((double) ms / 1000, (double) cad,
-                              (double) hr, km, speed, 0.0, watts, 0, 0, 0);
+        speed = speed * 160 * KM_PER_MILE;  // convert to kph
 
+        // 4 bytes of total elapsed time, in milliseconds
+        uint32_t ms;
+        is >> ms;
+
+        // 2 signed bytes of 100 * [percent grade]
+        // (i.e., grade == 100 * 100 * rise/run !!)
+        int16_t grade;
+        is >> grade;
+
+        // not sure what the next 2 bytes are
+        is.skipRawData(2);
+
+        // 4 bytes of floating point total distance traveled, in KM
+        float km;
+        is >> km;
+
+        // calculate change in altitude over the past interval.
+        // first, calculate grade measured as rise/run.
+        float floatGrade;
+        floatGrade = 0.01 * 0.01 * grade;  // floatgrade = rise/run
+        // then, convert grade to angle (in radians).
+        float angle = atan(floatGrade);
+        // calculate distance traveled over past interval
+        float delta_distance_meters = (1000.0) * (km - lastKM);
+        // change in altitude is:
+        //     sin(angle) * (distance traveled in past interval).
+        altitude = altitude + delta_distance_meters*sin(angle);
+
+        // not sure what the next 28 bytes are.
+        is.skipRawData(0x1c);
+
+        // OK -- we've pulled the next data point out of the ride
+        // file.  let's figure out if it's time to emit the next
+        // CT_EMIT_MS interval(s).  if so, emit it(them), and reset
+        // the averaging sums.
+        if (ms == 0) {
+          // special case first data point
+          rideFile->appendPoint((double) ms/1000, (double) cad,
+                                (double) hr, km, speed, 0.0, watts,
+                                altitude, 0, 0);
+        }
+        // while loop since an interval in the .3dp file might
+        // span more than one CT_EMIT_MS interval
+        while ((ms - lastEmittedMS) >= CT_EMIT_MS) {
+          uint32_t sum_interval_ms;
+          float interpol_km, interpol_alt, interpol_fraction;
+
+          // figure out the averaging sum update interval (i.e., time
+          // since we last added to the averaging sums).  it's either
+          // the time since the last sample from the CT file, or
+          // CT_EMIT_MS, depending on whether we've gone through this
+          // while loop already, or this is the first loop through.
+          if (lastSampleMS > lastEmittedMS)
+            sum_interval_ms = (lastEmittedMS + CT_EMIT_MS) - lastSampleMS;
+          else
+            sum_interval_ms = CT_EMIT_MS;
+
+          // update averaging sums with final bit of this sampling interval
+          hr_sum += ((double) hr) * ((double) sum_interval_ms);
+          cad_sum += ((double) cad) * ((double) sum_interval_ms);
+          speed_sum += ((double) speed) * ((double) sum_interval_ms);
+          watts_sum += ((double) watts) * ((double) sum_interval_ms);
+
+          // figure out interpolation points based on time from previous
+          // sample from the computrainer file
+          interpol_fraction =
+            ((float) ((lastEmittedMS + CT_EMIT_MS) - lastSampleMS)) /
+            ((float) (ms - lastSampleMS));
+          interpol_km = lastKM + (km - lastKM) * interpol_fraction;
+          interpol_alt = lastAltitude +
+            (altitude - lastAltitude) * interpol_fraction;
+
+          // update last sample emit time
+          lastEmittedMS = lastEmittedMS + CT_EMIT_MS;
+
+          // emit averages and interpolated distance/altitude
+          rideFile->appendPoint(
+                                ((double) lastEmittedMS) / 1000,
+                                ((double) cad_sum) / CT_EMIT_MS,
+                                ((double) hr_sum) / CT_EMIT_MS,
+                                interpol_km,
+                                ((double) speed_sum) / CT_EMIT_MS,
+                                0.0,
+                                ((double) watts_sum) / CT_EMIT_MS,
+                                interpol_alt,
+                                0,
+                                0);
+
+          // reset averaging sums
+          hr_sum = cad_sum = speed_sum = watts_sum = 0.0;
+        }
+
+        // update averaging sums with interval to current
+        // data point in .3dp file
+        if (ms > lastEmittedMS) {
+          uint32_t sum_interval_ms;
+
+          if (lastSampleMS > lastEmittedMS)
+            sum_interval_ms = ms - lastSampleMS;
+          else
+            sum_interval_ms = ms - lastEmittedMS;
+          hr_sum += ((double) hr) * ((double) sum_interval_ms);
+          cad_sum += ((double) cad) * ((double) sum_interval_ms);
+          speed_sum += ((double) speed) * ((double) sum_interval_ms);
+          watts_sum += ((double) watts) * ((double) sum_interval_ms);
+        }
+
+        // stash away distance, altitude, and time at end this
+        // interval so can calculate distance traveled over next
+        // interval in next loop iteration, and so we can interpolate.
+        lastSampleMS = ms;
+        lastKM = km;
+        lastAltitude = altitude;
     }
+
+    // convert the start time we parsed from the header into
+    // what GC wants.
     QDateTime dateTime;
     QDate date;
     QTime time;
@@ -116,24 +294,14 @@ RideFile *Computrainer3dpFileReader::openRideFile(QFile & file,
     time.setHMS(hour, minute, 0, 0);
     dateTime.setDate(date);
     dateTime.setTime(time);
-
     rideFile->setStartTime(dateTime);
 
-    int n = rideFile->dataPoints().size();
-    n = qMin(n, 1000);
-    if (n >= 2) {
-        QVector < double >secs(n - 1);
-        for (int i = 0; i < n - 1; ++i) {
-            double now = rideFile->dataPoints()[i]->secs;
-            double then = rideFile->dataPoints()[i + 1]->secs;
-            secs[i] = then - now;
-        }
-        std::sort(secs.begin(), secs.end());
-        int mid = n / 2 - 1;
-        double recint = round(secs[mid] * 1000.0) / 1000.0;
-        rideFile->setRecIntSecs(recint);
-    }
+    rideFile->setRecIntSecs(((double) CT_EMIT_MS) / 1000.0);
+
+    // tell GC what kind of device a computrainer is
     rideFile->setDeviceType("Computrainer 3DP");
+
+    // all done!  close up.
     file.close();
     return rideFile;
 }