Skip to content

Commit

Permalink
fit: Added support for ANT+ FIT format (used by Garmin devices)
Browse files Browse the repository at this point in the history
  • Loading branch information
mlofjard committed Feb 10, 2024
1 parent 6722dd0 commit 46dbf5b
Show file tree
Hide file tree
Showing 9 changed files with 5,712 additions and 0 deletions.
2 changes: 2 additions & 0 deletions format/all/all.fqtest
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@ $ fq -n _registry.groups.probe
"bzip2",
"caff",
"elf",
"fit",
"flac",
"gif",
"gzip",
Expand Down Expand Up @@ -85,6 +86,7 @@ elf Executable and Linkable Format
ether8023_frame Ethernet 802.3 frame
exif Exchangeable Image File Format
fairplay_spc FairPlay Server Playback Context
fit ANT FIT
flac Free Lossless Audio Codec file
flac_frame FLAC frame
flac_metadatablock FLAC metadatablock
Expand Down
1 change: 1 addition & 0 deletions format/all/all.go
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@ import (
_ "github.com/wader/fq/format/dns"
_ "github.com/wader/fq/format/elf"
_ "github.com/wader/fq/format/fairplay"
_ "github.com/wader/fq/format/fit"
_ "github.com/wader/fq/format/flac"
_ "github.com/wader/fq/format/gif"
_ "github.com/wader/fq/format/gzip"
Expand Down
354 changes: 354 additions & 0 deletions format/fit/fit.go
Original file line number Diff line number Diff line change
@@ -0,0 +1,354 @@
package fit

import (
"embed"
"fmt"
"sort"

"github.com/wader/fq/format"
"github.com/wader/fq/format/fit/mappers"
"github.com/wader/fq/pkg/decode"
"github.com/wader/fq/pkg/interp"
"github.com/wader/fq/pkg/scalar"
"golang.org/x/text/encoding"
)

var fitFS embed.FS

func init() {
interp.RegisterFormat(
format.FIT,
&decode.Format{
Description: "ANT FIT",
Groups: []*decode.Group{format.Probe},
DecodeFn: decodeFIT,
})
interp.RegisterFS(fitFS)
}

var fitCRCTable = [16]uint16{
0x0000, 0xCC01, 0xD801, 0x1400, 0xF001, 0x3C00, 0x2800, 0xE401,
0xA001, 0x6C00, 0x7800, 0xB401, 0x5000, 0x9C01, 0x8801, 0x4400,
}

func calcCRC(bytes []byte) uint16 {
var crc uint16
crc = 0
for i := 0; i < len(bytes); i++ {
// compute checksum of lower four bits of byte
var byte = bytes[i]
var tmp = fitCRCTable[crc&0xF]
crc = (crc >> 4) & 0x0FFF
crc = crc ^ tmp ^ fitCRCTable[byte&0xF]
tmp = fitCRCTable[crc&0xF]
crc = (crc >> 4) & 0x0FFF
crc = crc ^ tmp ^ fitCRCTable[(byte>>4)&0xF]
}

return crc
}

type fitContext struct {
dataSize int
headerSize int
}

type dataRecordContext struct {
compressed bool
data bool
localMessageType int
hasDeveloperFields bool
}

type fileDescriptionContext struct {
devIdx uint64
fDefNo uint64
typ string
name string
unit string
nativeFieldNo uint64
nativeMsgNo uint64
}

type fieldDef struct {
name string
typ string
format string
unit string
scale float64
offset int64
size int
}

type devFieldDefMap map[uint64]map[uint64]fieldDef
type localFieldDefMap map[uint64]map[uint64]fieldDef
type localMsgIsDevDef map[uint64]bool

func fitDecodeFileHeader(d *decode.D, fc *fitContext) {
frameStart := d.Pos()

// d.FieldStruct("ident", func(d *decode.D) {
headerSize := d.FieldU8("headerSize")
d.FieldU8("protocolVersion")
d.FieldU16("profileVersion")
dataSize := d.FieldU32("dataSize")

d.FieldRawLen("dataType", 4*8, d.AssertBitBuf([]byte(".FIT")))
if headerSize == 14 {
headerCRC := calcCRC(d.BytesRange(frameStart, int(headerSize)-2))
d.FieldU16("crc", d.UintValidate(uint64(headerCRC)))
}
fc.headerSize = int(headerSize)
fc.dataSize = int(dataSize)
}

func fitDecodeDataRecordHeader(d *decode.D, drc *dataRecordContext) {
drc.compressed = d.FieldBool("normalHeader", scalar.BoolMapDescription{false: "Normal header",
true: "Compressed header"})
if drc.compressed {
localMessageType := d.FieldU2("localMessageType")
d.FieldU32("timeOffset")
drc.localMessageType = int(localMessageType)
drc.data = true
} else {
mTypeIsDef := d.FieldBool("messageType", scalar.BoolMap{true: {Sym: 1, Description: "Definition message"},
false: {Sym: 0, Description: "Data message"}})
hasDeveloperFields := d.FieldBool("hasDeveloperFields")
d.FieldBool("reserved")
localMessageType := d.FieldU4("localMessageType")

drc.hasDeveloperFields = hasDeveloperFields
drc.localMessageType = int(localMessageType)
drc.data = !mTypeIsDef
}
}

func fitDecodeDefinitionMessage(d *decode.D, drc *dataRecordContext, lmfd localFieldDefMap, dmfd devFieldDefMap, isDevMap localMsgIsDevDef) {
d.FieldU8("reserved")
endian := d.FieldU8("architecture")
switch endian {
case 0:
d.Endian = decode.LittleEndian
case 1:
d.Endian = decode.BigEndian
default:
d.Fatalf("Unknown endian %d", endian)
}
messageNo := d.FieldU16("globalMessageNumber", mappers.TypeDefMap["mesg_num"])
if messageNo == 206 { // developer field_description
isDevMap[uint64(drc.localMessageType)] = true
} else {
isDevMap[uint64(drc.localMessageType)] = false
}
numFields := d.FieldU8("fields")
lmfd[uint64(drc.localMessageType)] = make(map[uint64]fieldDef, numFields)
d.FieldArray("fieldDefinitions", func(d *decode.D) {
for i := 0; i < int(numFields); i++ {
d.FieldStruct("fieldDefinition", func(d *decode.D) {
fieldDefNo := d.FieldU8("fieldDefNo", mappers.FieldDefMap[messageNo])
size := d.FieldU8("size")
baseType := d.FieldU8("baseType", mappers.TypeDefMap["fit_base_type"])

var typ = mappers.TypeDefMap["fit_base_type"][baseType].Name
fDefLookup, isSet := mappers.FieldDefMap[messageNo][fieldDefNo]
if isSet {
var foundName = fDefLookup.Name
lmfd[uint64(drc.localMessageType)][uint64(i)] = fieldDef{name: foundName, typ: typ, size: int(size), format: fDefLookup.Type, unit: fDefLookup.Unit, scale: fDefLookup.Scale, offset: fDefLookup.Offset}
} else {
var foundName = fmt.Sprintf("UNKOWN_%d", fieldDefNo)
lmfd[uint64(drc.localMessageType)][uint64(i)] = fieldDef{name: foundName, typ: typ, size: int(size), format: "unknown"}
}
})
}
})
if drc.hasDeveloperFields {
numDevFields := d.FieldU8("devFields")

d.FieldArray("devFieldDefinitions", func(d *decode.D) {
for i := numFields; i < (numDevFields + numFields); i++ {
d.FieldStruct("devFieldDefinition", func(d *decode.D) {
fieldDefNo := d.FieldU8("fieldDefNo")
size := d.FieldU8("size")
devDataIdx := d.FieldU8("devDataIdx")

//baseType := d.FieldU8("baseType", mappers.TypeDefMap["fit_base_type"])

//var typ = mappers.TypeDefMap["fit_base_type"][baseType].Name
typ := dmfd[devDataIdx][fieldDefNo].typ
//fDefLookup, isSet := mappers.FieldDefMap[messageNo][fieldDefNo]
fDefLookup, isSet := dmfd[devDataIdx][fieldDefNo]
if isSet {
var foundName = fDefLookup.name
lmfd[uint64(drc.localMessageType)][uint64(i)] = fieldDef{name: foundName, typ: typ, size: int(size), unit: fDefLookup.unit, scale: fDefLookup.scale, offset: fDefLookup.offset}
} else {
var foundName = fmt.Sprintf("UNKOWN_%d", fieldDefNo)
lmfd[uint64(drc.localMessageType)][uint64(i)] = fieldDef{name: foundName, typ: typ, size: int(size), format: "unknown"}
}
})
}
})
}

}

func ensureDevFieldMap(dmfd devFieldDefMap, devIdx uint64, fieldDefNo uint64) {
_, devIsSet := dmfd[devIdx]
if !devIsSet {
dmfd[devIdx] = make(map[uint64]fieldDef)
}
}

func fieldUint(fieldFn func(string, ...scalar.UintMapper) uint64, d *decode.D, fdc *fileDescriptionContext, expectedSize int, fDef fieldDef, uintFormatter scalar.UintFn) {
var val uint64
if fDef.size != expectedSize {
d.FieldStr(fDef.name, fDef.size, encoding.Nop)
} else {
if uintFormatter != nil {
val = fieldFn(fDef.name, uintFormatter)
} else {
val = fieldFn(fDef.name)
}

switch fDef.name {
case "developer_data_index":
fdc.devIdx = val
case "field_definition_number":
fdc.fDefNo = val
case "fit_base_type_id":
fdc.typ = mappers.TypeDefMap["fit_base_type"][val].Name
case "native_field_num":
fdc.nativeFieldNo = val
case "native_mesg_num":
fdc.nativeMsgNo = val
}
}
}

func fieldSint(fieldFn func(string, ...scalar.SintMapper) int64, d *decode.D, fdc *fileDescriptionContext, expectedSize int, fDef fieldDef, sintFormatter scalar.SintFn) {
//var val uint64
if fDef.size != expectedSize {
d.FieldStr(fDef.name, fDef.size, encoding.Nop)
} else {
if sintFormatter != nil {
fieldFn(fDef.name, sintFormatter)
} else {
fieldFn(fDef.name)
}
//setDevFieldUint(isDevDep, dmfd, fDef.name, val);
}
}

func fieldFloat(fieldFn func(string, ...scalar.FltMapper) float64, d *decode.D, fdc *fileDescriptionContext, expectedSize int, fDef fieldDef) {
//var val uint64
if fDef.size != expectedSize {
d.FieldStr(fDef.name, fDef.size, encoding.Nop)
} else {
fieldFn(fDef.name)

//setDevFieldUint(isDevDep, dmfd, fDef.name, val);
}
}

func fieldString(d *decode.D, fdc *fileDescriptionContext, fDef fieldDef) {
val := d.FieldUTF8NullFixedLen(fDef.name, fDef.size)

switch fDef.name {
case "field_name":
fdc.name = val
case "units":
fdc.unit = val
}
}

func fitDecodeDataMessage(d *decode.D, drc *dataRecordContext, lmfd localFieldDefMap, dmfd devFieldDefMap, isDevMap localMsgIsDevDef) {
var fdc fileDescriptionContext
keys := make([]int, len(lmfd[uint64(drc.localMessageType)]))
i := 0
for k := range lmfd[uint64(drc.localMessageType)] {
keys[i] = int(k)
i++
}
sort.Ints(keys)

isDevDep := isDevMap[uint64(drc.localMessageType)]

for _, k := range keys {
fDef := lmfd[uint64(drc.localMessageType)][uint64(k)]

var uintFormatter = mappers.GetUintFormatter(fDef.format, fDef.unit, fDef.scale, fDef.offset)
var sintFormatter = mappers.GetSintFormatter(fDef.format, fDef.unit, fDef.scale, fDef.offset)

switch fDef.typ {
// case "byte":
// d.FieldStr(fDef.name, fDef.size, encoding.Nop)
case "enum", "uint8", "uint8z", "byte":
fieldUint(d.FieldU8, d, &fdc, 1, fDef, uintFormatter)
case "sint8":
fieldSint(d.FieldS8, d, &fdc, 1, fDef, sintFormatter)
case "sint16":
fieldSint(d.FieldS16, d, &fdc, 2, fDef, sintFormatter)
case "uint16", "uint16z":
fieldUint(d.FieldU16, d, &fdc, 2, fDef, uintFormatter)
case "sint32":
fieldSint(d.FieldS32, d, &fdc, 4, fDef, sintFormatter)
case "uint32", "uint32z":
fieldUint(d.FieldU32, d, &fdc, 4, fDef, uintFormatter)
case "float32":
fieldFloat(d.FieldF32, d, &fdc, 4, fDef)
case "float64":
fieldFloat(d.FieldF64, d, &fdc, 8, fDef)
case "sint64":
fieldSint(d.FieldS64, d, &fdc, 4, fDef, sintFormatter)
case "uint64", "uint64z":
fieldUint(d.FieldU64, d, &fdc, 8, fDef, uintFormatter)
case "string":
fieldString(d, &fdc, fDef)
default:
d.Fatalf("Unknown type %s", fDef.typ)
}
}

if isDevDep {
ensureDevFieldMap(dmfd, fdc.devIdx, fdc.fDefNo)
dmfd[fdc.devIdx][fdc.fDefNo] = fieldDef{name: fdc.name, typ: fdc.typ, unit: fdc.unit, scale: 0, offset: 0}
}
}

func decodeFIT(d *decode.D) any {
var fc fitContext
d.Endian = decode.LittleEndian

var lmfd localFieldDefMap = make(localFieldDefMap)
var dmfd devFieldDefMap = make(devFieldDefMap)
var isDevMap localMsgIsDevDef = make(localMsgIsDevDef)

//decodeBSONDocument(d)
d.FieldStruct("header", func(d *decode.D) { fitDecodeFileHeader(d, &fc) })

d.FieldArray("dataRecords", func(d *decode.D) {
// // headerPos := d.Pos()

for d.Pos() < int64((fc.headerSize+fc.dataSize)*8) {
d.FieldStruct("dataRecord", func(d *decode.D) {
var drc dataRecordContext
d.FieldStruct("dataRecordHeader", func(d *decode.D) { fitDecodeDataRecordHeader(d, &drc) })
switch drc.data {
case true:
d.FieldStruct("dataMessage", func(d *decode.D) { fitDecodeDataMessage(d, &drc, lmfd, dmfd, isDevMap) })
case false:
d.FieldStruct("definitionMessage", func(d *decode.D) { fitDecodeDefinitionMessage(d, &drc, lmfd, dmfd, isDevMap) })
}
})
}
})

var fileCRC uint16
if fc.headerSize == 12 {
fileCRC = calcCRC(d.BytesRange(0, fc.dataSize+fc.headerSize)) // 12 byte header - CRC whole file except the CRC itself
} else {
fileCRC = calcCRC(d.BytesRange(14*8, fc.dataSize)) // 14 byte header - CRC everything below header except the CRC itself
}
d.FieldU16("crc", d.UintValidate(uint64(fileCRC)))

return nil
}
Loading

0 comments on commit 46dbf5b

Please sign in to comment.