mirror of
https://github.com/tumic0/GPXSee.git
synced 2024-11-24 03:35:53 +01:00
Proper fix for Bryton Rider FIT files
Refactoring
This commit is contained in:
parent
3424b3e265
commit
366e84c9fc
@ -1,4 +1,3 @@
|
||||
#include <cstring>
|
||||
#include <QtEndian>
|
||||
#include "common/staticassert.h"
|
||||
#include "fitparser.h"
|
||||
@ -10,50 +9,64 @@
|
||||
#define EVENT_MESSAGE 21
|
||||
#define TIMESTAMP_FIELD 253
|
||||
|
||||
struct Event {
|
||||
class Event {
|
||||
public:
|
||||
Event() : id(0), type(0), data(0) {}
|
||||
|
||||
quint8 id;
|
||||
quint8 type;
|
||||
quint32 data;
|
||||
};
|
||||
|
||||
Event() : id(0), type(0), data(0) {}
|
||||
struct FileHeader {
|
||||
quint8 headerSize;
|
||||
quint8 protocolVersion;
|
||||
quint16 profileVersion;
|
||||
quint32 dataSize;
|
||||
quint32 magic;
|
||||
};
|
||||
|
||||
struct FITParser::Field {
|
||||
quint8 id;
|
||||
quint8 size;
|
||||
quint8 type;
|
||||
};
|
||||
|
||||
class FITParser::MessageDefinition {
|
||||
public:
|
||||
MessageDefinition() : endian(0), globalId(0), numFields(0), fields(0),
|
||||
numDevFields(0), devFields(0) {}
|
||||
~MessageDefinition() {delete[] fields; delete[] devFields;}
|
||||
|
||||
quint8 endian;
|
||||
quint16 globalId;
|
||||
quint8 numFields;
|
||||
FITParser::Field *fields;
|
||||
quint8 numDevFields;
|
||||
FITParser::Field *devFields;
|
||||
};
|
||||
|
||||
class FITParser::CTX {
|
||||
public:
|
||||
CTX(QFile *file) : file(file), len(0), endian(0), timestamp(0),
|
||||
lastWrite(0), ratio(NAN) {}
|
||||
|
||||
QFile *file;
|
||||
quint32 len;
|
||||
quint8 endian;
|
||||
quint32 timestamp, lastWrite;
|
||||
FITParser::MessageDefinition defs[16];
|
||||
qreal ratio;
|
||||
Trackpoint trackpoint;
|
||||
TrackData track;
|
||||
};
|
||||
|
||||
|
||||
FITParser::FITParser()
|
||||
{
|
||||
memset(_defs, 0, sizeof(_defs));
|
||||
|
||||
_device = 0;
|
||||
_endian = 0;
|
||||
_timestamp = 0;
|
||||
_last = 0;
|
||||
_len = 0;
|
||||
_ratio = NAN;
|
||||
}
|
||||
|
||||
void FITParser::clearDefinitions()
|
||||
{
|
||||
for (int i = 0; i < 16; i++) {
|
||||
if (_defs[i].fields)
|
||||
delete[] _defs[i].fields;
|
||||
if (_defs[i].devFields)
|
||||
delete[] _defs[i].devFields;
|
||||
}
|
||||
|
||||
memset(_defs, 0, sizeof(_defs));
|
||||
}
|
||||
|
||||
void FITParser::warning(const char *text) const
|
||||
{
|
||||
const QFile *file = static_cast<QFile *>(_device);
|
||||
qWarning("%s:%d: %s\n", qPrintable(file->fileName()), _len, text);
|
||||
}
|
||||
|
||||
bool FITParser::readData(char *data, size_t size)
|
||||
bool FITParser::readData(QFile *file, char *data, size_t size)
|
||||
{
|
||||
qint64 n;
|
||||
|
||||
n = _device->read(data, size);
|
||||
n = file->read(data, size);
|
||||
if (n < 0) {
|
||||
_errorString = "I/O error";
|
||||
return false;
|
||||
@ -65,17 +78,17 @@ bool FITParser::readData(char *data, size_t size)
|
||||
return true;
|
||||
}
|
||||
|
||||
template<class T> bool FITParser::readValue(T &val)
|
||||
template<class T> bool FITParser::readValue(CTX &ctx, T &val)
|
||||
{
|
||||
T data;
|
||||
|
||||
if (!readData((char*)&data, sizeof(T)))
|
||||
if (!readData(ctx.file, (char*)&data, sizeof(T)))
|
||||
return false;
|
||||
|
||||
_len -= sizeof(T);
|
||||
ctx.len -= sizeof(T);
|
||||
|
||||
if (sizeof(T) > 1) {
|
||||
if (_endian)
|
||||
if (ctx.endian)
|
||||
val = qFromBigEndian(data);
|
||||
else
|
||||
val = qFromLittleEndian(data);
|
||||
@ -85,22 +98,16 @@ template<class T> bool FITParser::readValue(T &val)
|
||||
return true;
|
||||
}
|
||||
|
||||
bool FITParser::skipValue(size_t size)
|
||||
bool FITParser::skipValue(CTX &ctx, size_t size)
|
||||
{
|
||||
size_t i;
|
||||
quint8 val;
|
||||
|
||||
for (i = 0; i < size; i++)
|
||||
if (!readValue(val))
|
||||
return false;
|
||||
|
||||
return true;
|
||||
ctx.len -= size;
|
||||
return ctx.file->seek(ctx.file->pos() + size);
|
||||
}
|
||||
|
||||
bool FITParser::parseDefinitionMessage(quint8 header)
|
||||
bool FITParser::parseDefinitionMessage(CTX &ctx, quint8 header)
|
||||
{
|
||||
int local_id = header & 0x0f;
|
||||
MessageDefinition *def = &_defs[local_id];
|
||||
MessageDefinition *def = &(ctx.defs[local_id]);
|
||||
quint8 i;
|
||||
|
||||
|
||||
@ -114,47 +121,48 @@ bool FITParser::parseDefinitionMessage(quint8 header)
|
||||
}
|
||||
|
||||
// reserved/unused
|
||||
if (!readValue(i))
|
||||
if (!readValue(ctx, i))
|
||||
return false;
|
||||
|
||||
// endianness
|
||||
if (!readValue(def->endian))
|
||||
if (!readValue(ctx, def->endian))
|
||||
return false;
|
||||
if (def->endian > 1) {
|
||||
_errorString = "Bad endian field";
|
||||
return false;
|
||||
}
|
||||
_endian = def->endian;
|
||||
ctx.endian = def->endian;
|
||||
|
||||
// global message number
|
||||
if (!readValue(def->globalId))
|
||||
if (!readValue(ctx, def->globalId))
|
||||
return false;
|
||||
|
||||
// number of records
|
||||
if (!readValue(def->numFields))
|
||||
if (!readValue(ctx, def->numFields))
|
||||
return false;
|
||||
|
||||
// definition records
|
||||
def->fields = new Field[def->numFields];
|
||||
for (i = 0; i < def->numFields; i++) {
|
||||
STATIC_ASSERT(sizeof(def->fields[i]) == 3);
|
||||
if (!readData((char*)&(def->fields[i]), sizeof(def->fields[i])))
|
||||
if (!readData(ctx.file, (char*)&(def->fields[i]),
|
||||
sizeof(def->fields[i])))
|
||||
return false;
|
||||
_len -= sizeof(def->fields[i]);
|
||||
ctx.len -= sizeof(def->fields[i]);
|
||||
}
|
||||
|
||||
// developer definition records
|
||||
if (header & 0x20) {
|
||||
if (!readValue(def->numDevFields))
|
||||
if (!readValue(ctx, def->numDevFields))
|
||||
return false;
|
||||
|
||||
def->devFields = new Field[def->numDevFields];
|
||||
for (i = 0; i < def->numDevFields; i++) {
|
||||
STATIC_ASSERT(sizeof(def->devFields[i]) == 3);
|
||||
if (!readData((char*)&(def->devFields[i]),
|
||||
if (!readData(ctx.file, (char*)&(def->devFields[i]),
|
||||
sizeof(def->devFields[i])))
|
||||
return false;
|
||||
_len -= sizeof(def->devFields[i]);
|
||||
ctx.len -= sizeof(def->devFields[i]);
|
||||
}
|
||||
} else
|
||||
def->numDevFields = 0;
|
||||
@ -162,7 +170,7 @@ bool FITParser::parseDefinitionMessage(quint8 header)
|
||||
return true;
|
||||
}
|
||||
|
||||
bool FITParser::readField(Field *f, quint32 &val)
|
||||
bool FITParser::readField(CTX &ctx, Field *field, quint32 &val)
|
||||
{
|
||||
quint8 v8 = (quint8)-1;
|
||||
quint16 v16 = (quint16)-1;
|
||||
@ -170,65 +178,44 @@ bool FITParser::readField(Field *f, quint32 &val)
|
||||
|
||||
val = (quint32)-1;
|
||||
|
||||
switch (f->type) {
|
||||
switch (field->type) {
|
||||
case 0: // enum
|
||||
case 1: // sint8
|
||||
case 2: // uint8
|
||||
if (f->size == 1) {
|
||||
ret = readValue(v8);
|
||||
if (field->size == 1) {
|
||||
ret = readValue(ctx, v8);
|
||||
val = v8;
|
||||
} else
|
||||
ret = skipValue(f->size);
|
||||
ret = skipValue(ctx, field->size);
|
||||
break;
|
||||
case 0x83: // sint16
|
||||
case 0x84: // uint16
|
||||
if (f->size == 2) {
|
||||
ret = readValue(v16);
|
||||
if (field->size == 2) {
|
||||
ret = readValue(ctx, v16);
|
||||
val = v16;
|
||||
} else
|
||||
ret = skipValue(f->size);
|
||||
ret = skipValue(ctx, field->size);
|
||||
break;
|
||||
case 0x85: // sint32
|
||||
case 0x86: // uint32
|
||||
if (f->size == 4)
|
||||
ret = readValue(val);
|
||||
if (field->size == 4)
|
||||
ret = readValue(ctx, val);
|
||||
else
|
||||
ret = skipValue(f->size);
|
||||
ret = skipValue(ctx, field->size);
|
||||
break;
|
||||
default:
|
||||
ret = skipValue(f->size);
|
||||
ret = skipValue(ctx, field->size);
|
||||
break;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool FITParser::addEntry(TrackData &track)
|
||||
{
|
||||
if (_trackpoint.coordinates().isValid()) {
|
||||
_trackpoint.setTimestamp(QDateTime::fromTime_t(_timestamp
|
||||
+ 631065600));
|
||||
_trackpoint.setRatio(_ratio);
|
||||
track.append(_trackpoint);
|
||||
} else {
|
||||
if (_trackpoint.coordinates().isNull())
|
||||
warning("Missing coordinates");
|
||||
else {
|
||||
_errorString = "Invalid coordinates";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool FITParser::parseData(TrackData &track, MessageDefinition *def,
|
||||
quint8 offset)
|
||||
bool FITParser::parseData(CTX &ctx, const MessageDefinition *def)
|
||||
{
|
||||
Field *field;
|
||||
Event event;
|
||||
quint32 val;
|
||||
int i;
|
||||
|
||||
|
||||
if (!def->fields && !def->devFields) {
|
||||
@ -236,58 +223,50 @@ bool FITParser::parseData(TrackData &track, MessageDefinition *def,
|
||||
return false;
|
||||
}
|
||||
|
||||
if (def->globalId == RECORD_MESSAGE && _last != _timestamp) {
|
||||
if (!addEntry(track))
|
||||
return false;
|
||||
_last = _timestamp;
|
||||
_trackpoint = Trackpoint();
|
||||
}
|
||||
ctx.endian = def->endian;
|
||||
|
||||
_endian = def->endian;
|
||||
_timestamp += offset;
|
||||
|
||||
for (i = 0; i < def->numFields; i++) {
|
||||
for (int i = 0; i < def->numFields; i++) {
|
||||
field = &def->fields[i];
|
||||
if (!readField(field, val))
|
||||
if (!readField(ctx, field, val))
|
||||
return false;
|
||||
|
||||
if (field->id == TIMESTAMP_FIELD)
|
||||
_timestamp = val;
|
||||
ctx.timestamp = val;
|
||||
else if (def->globalId == RECORD_MESSAGE) {
|
||||
switch (field->id) {
|
||||
case 0:
|
||||
if (val != 0x7fffffff)
|
||||
_trackpoint.rcoordinates().setLat(
|
||||
ctx.trackpoint.rcoordinates().setLat(
|
||||
((qint32)val / (double)0x7fffffff) * 180);
|
||||
break;
|
||||
case 1:
|
||||
if (val != 0x7fffffff)
|
||||
_trackpoint.rcoordinates().setLon(
|
||||
ctx.trackpoint.rcoordinates().setLon(
|
||||
((qint32)val / (double)0x7fffffff) * 180);
|
||||
break;
|
||||
case 2:
|
||||
if (val != 0xffff)
|
||||
_trackpoint.setElevation((val / 5.0) - 500);
|
||||
ctx.trackpoint.setElevation((val / 5.0) - 500);
|
||||
break;
|
||||
case 3:
|
||||
if (val != 0xff)
|
||||
_trackpoint.setHeartRate(val);
|
||||
ctx.trackpoint.setHeartRate(val);
|
||||
break;
|
||||
case 4:
|
||||
if (val != 0xff)
|
||||
_trackpoint.setCadence(val);
|
||||
ctx.trackpoint.setCadence(val);
|
||||
break;
|
||||
case 6:
|
||||
if (val != 0xffff)
|
||||
_trackpoint.setSpeed(val / 1000.0f);
|
||||
ctx.trackpoint.setSpeed(val / 1000.0f);
|
||||
break;
|
||||
case 7:
|
||||
if (val != 0xffff)
|
||||
_trackpoint.setPower(val);
|
||||
ctx.trackpoint.setPower(val);
|
||||
break;
|
||||
case 13:
|
||||
if (val != 0x7f)
|
||||
_trackpoint.setTemperature((qint8)val);
|
||||
ctx.trackpoint.setTemperature((qint8)val);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
@ -308,73 +287,85 @@ bool FITParser::parseData(TrackData &track, MessageDefinition *def,
|
||||
}
|
||||
}
|
||||
|
||||
for (i = 0; i < def->numDevFields; i++) {
|
||||
for (int i = 0; i < def->numDevFields; i++) {
|
||||
field = &def->devFields[i];
|
||||
if (!readField(field, val))
|
||||
if (!readField(ctx, field, val))
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
if (def->globalId == EVENT_MESSAGE) {
|
||||
if ((event.id == 42 || event.id == 43) && event.type == 3) {
|
||||
quint32 front = ((event.data & 0xFF000000) >> 24);
|
||||
quint32 rear = ((event.data & 0x0000FF00) >> 8);
|
||||
_ratio = ((qreal)front / (qreal)rear);
|
||||
ctx.ratio = ((qreal)front / (qreal)rear);
|
||||
}
|
||||
} else if (def->globalId == RECORD_MESSAGE) {
|
||||
if (ctx.timestamp > ctx.lastWrite
|
||||
&& ctx.trackpoint.coordinates().isValid()) {
|
||||
ctx.trackpoint.setTimestamp(QDateTime::fromTime_t(ctx.timestamp
|
||||
+ 631065600));
|
||||
ctx.trackpoint.setRatio(ctx.ratio);
|
||||
ctx.track.append(ctx.trackpoint);
|
||||
ctx.trackpoint = Trackpoint();
|
||||
ctx.lastWrite = ctx.timestamp;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool FITParser::parseDataMessage(TrackData &track, quint8 header)
|
||||
bool FITParser::parseDataMessage(CTX &ctx, quint8 header)
|
||||
{
|
||||
int local_id = header & 0xf;
|
||||
MessageDefinition *def = &_defs[local_id];
|
||||
return parseData(track, def, 0);
|
||||
MessageDefinition *def = &(ctx.defs[local_id]);
|
||||
return parseData(ctx, def);
|
||||
}
|
||||
|
||||
bool FITParser::parseCompressedMessage(TrackData &track, quint8 header)
|
||||
bool FITParser::parseCompressedMessage(CTX &ctx, quint8 header)
|
||||
{
|
||||
int local_id = (header >> 5) & 3;
|
||||
MessageDefinition *def = &_defs[local_id];
|
||||
return parseData(track, def, header & 0x1f);
|
||||
MessageDefinition *def = &(ctx.defs[local_id]);
|
||||
ctx.timestamp += header & 0x1f;
|
||||
return parseData(ctx, def);
|
||||
}
|
||||
|
||||
bool FITParser::parseRecord(TrackData &track)
|
||||
bool FITParser::parseRecord(CTX &ctx)
|
||||
{
|
||||
quint8 header;
|
||||
|
||||
if (!readValue(header))
|
||||
if (!readValue(ctx, header))
|
||||
return false;
|
||||
|
||||
if (header & 0x80)
|
||||
return parseCompressedMessage(track, header);
|
||||
return parseCompressedMessage(ctx, header);
|
||||
else if (header & 0x40)
|
||||
return parseDefinitionMessage(header);
|
||||
return parseDefinitionMessage(ctx, header);
|
||||
else
|
||||
return parseDataMessage(track, header);
|
||||
return parseDataMessage(ctx, header);
|
||||
}
|
||||
|
||||
bool FITParser::parseHeader()
|
||||
bool FITParser::parseHeader(CTX &ctx)
|
||||
{
|
||||
FileHeader hdr;
|
||||
quint16 crc;
|
||||
qint64 len;
|
||||
|
||||
STATIC_ASSERT(sizeof(hdr) == 12);
|
||||
len = _device->read((char*)&hdr, sizeof(hdr));
|
||||
len = ctx.file->read((char*)&hdr, sizeof(hdr));
|
||||
if (len < 0) {
|
||||
_errorString = "I/O error";
|
||||
return false;
|
||||
} else if ((size_t)len < sizeof(hdr)
|
||||
|| hdr.magic != qToLittleEndian(FIT_MAGIC)) {
|
||||
|| hdr.magic != qToLittleEndian((quint32)FIT_MAGIC)) {
|
||||
_errorString = "Not a FIT file";
|
||||
return false;
|
||||
}
|
||||
|
||||
_len = qFromLittleEndian(hdr.dataSize);
|
||||
ctx.len = qFromLittleEndian(hdr.dataSize);
|
||||
|
||||
if (hdr.headerSize > sizeof(hdr))
|
||||
if (!readData((char *)&crc, sizeof(crc)))
|
||||
if (!readData(ctx.file, (char *)&crc, sizeof(crc)))
|
||||
return false;
|
||||
|
||||
return true;
|
||||
@ -385,28 +376,17 @@ bool FITParser::parse(QFile *file, QList<TrackData> &tracks,
|
||||
{
|
||||
Q_UNUSED(routes);
|
||||
Q_UNUSED(waypoints);
|
||||
bool ret = true;
|
||||
CTX ctx(file);
|
||||
|
||||
_device = file;
|
||||
_endian = 0;
|
||||
_timestamp = 0;
|
||||
_last = 0;
|
||||
_ratio = NAN;
|
||||
_trackpoint = Trackpoint();
|
||||
|
||||
if (!parseHeader())
|
||||
if (!parseHeader(ctx))
|
||||
return false;
|
||||
|
||||
tracks.append(TrackData());
|
||||
TrackData &track = tracks.last();
|
||||
while (ctx.len)
|
||||
if (!parseRecord(ctx))
|
||||
return false;
|
||||
|
||||
while (_len)
|
||||
if (!(ret = parseRecord(track)))
|
||||
break;
|
||||
if (ret && _trackpoint.coordinates().isValid())
|
||||
ret = addEntry(track);
|
||||
tracks.append(ctx.track);
|
||||
|
||||
clearDefinitions();
|
||||
|
||||
return ret;
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -3,66 +3,34 @@
|
||||
|
||||
#include "parser.h"
|
||||
|
||||
class QFile;
|
||||
|
||||
class FITParser : public Parser
|
||||
{
|
||||
public:
|
||||
FITParser();
|
||||
|
||||
bool parse(QFile *file, QList<TrackData> &tracks, QList<RouteData> &routes,
|
||||
QList<Waypoint> &waypoints);
|
||||
QString errorString() const {return _errorString;}
|
||||
int errorLine() const {return 0;}
|
||||
|
||||
private:
|
||||
struct FileHeader {
|
||||
quint8 headerSize;
|
||||
quint8 protocolVersion;
|
||||
quint16 profileVersion;
|
||||
quint32 dataSize;
|
||||
quint32 magic;
|
||||
};
|
||||
struct Field;
|
||||
class MessageDefinition;
|
||||
class CTX;
|
||||
|
||||
struct Field {
|
||||
quint8 id;
|
||||
quint8 size;
|
||||
quint8 type;
|
||||
};
|
||||
bool readData(QFile *file, char *data, size_t size);
|
||||
template<class T> bool readValue(CTX &ctx, T &val);
|
||||
bool skipValue(CTX &ctx, size_t size);
|
||||
bool readField(CTX &ctx, Field *field, quint32 &val);
|
||||
|
||||
struct MessageDefinition {
|
||||
quint8 endian;
|
||||
quint16 globalId;
|
||||
quint8 numFields;
|
||||
Field *fields;
|
||||
quint8 numDevFields;
|
||||
Field *devFields;
|
||||
};
|
||||
bool parseHeader(CTX &ctx);
|
||||
bool parseRecord(CTX &ctx);
|
||||
bool parseDefinitionMessage(CTX &ctx, quint8 header);
|
||||
bool parseCompressedMessage(CTX &ctx, quint8 header);
|
||||
bool parseDataMessage(CTX &ctx, quint8 header);
|
||||
bool parseData(CTX &ctx, const MessageDefinition *def);
|
||||
|
||||
|
||||
void warning(const char *text) const;
|
||||
void clearDefinitions();
|
||||
|
||||
bool readData(char *data, size_t size);
|
||||
template<class T> bool readValue(T &val);
|
||||
bool skipValue(size_t size);
|
||||
|
||||
bool parseHeader();
|
||||
bool parseRecord(TrackData &track);
|
||||
bool parseDefinitionMessage(quint8 header);
|
||||
bool parseCompressedMessage(TrackData &track, quint8 header);
|
||||
bool parseDataMessage(TrackData &track, quint8 header);
|
||||
bool parseData(TrackData &track, MessageDefinition *def, quint8 offset);
|
||||
bool readField(Field *f, quint32 &val);
|
||||
bool addEntry(TrackData &track);
|
||||
|
||||
QIODevice *_device;
|
||||
QString _errorString;
|
||||
|
||||
quint32 _len;
|
||||
quint8 _endian;
|
||||
quint32 _timestamp, _last;
|
||||
MessageDefinition _defs[16];
|
||||
qreal _ratio;
|
||||
Trackpoint _trackpoint;
|
||||
};
|
||||
|
||||
#endif // FITPARSER_H
|
||||
|
||||
@ -67,7 +67,8 @@ inline QDebug operator<<(QDebug dbg, const Trackpoint &trackpoint)
|
||||
dbg.nospace() << "Trackpoint(" << trackpoint.coordinates() << ", "
|
||||
<< trackpoint.timestamp() << ", " << trackpoint.elevation() << ", "
|
||||
<< trackpoint.speed() << ", " << trackpoint.heartRate() << ", "
|
||||
<< trackpoint.temperature() << ")";
|
||||
<< trackpoint.temperature() << ", " << trackpoint.cadence() << ", "
|
||||
<< trackpoint.power() << ", " << trackpoint.ratio() << ")";
|
||||
return dbg.space();
|
||||
}
|
||||
#endif // QT_NO_DEBUG
|
||||
|
||||
Loading…
Reference in New Issue
Block a user