Skip to content

Commit 8b48c69

Browse files
mlofjardmlofjard
committed
fit: Added support for ANT+ FIT format (used by Garmin devices)
1 parent e23409b commit 8b48c69

File tree

9 files changed

+5712
-0
lines changed

9 files changed

+5712
-0
lines changed

‎format/all/all.fqtest‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@ $ fq -n _registry.groups.probe
1010
"bzip2",
1111
"caff",
1212
"elf",
13+
"fit",
1314
"flac",
1415
"gif",
1516
"gzip",
@@ -85,6 +86,7 @@ elf Executable and Linkable Format
8586
ether8023_frame Ethernet 802.3 frame
8687
exif Exchangeable Image File Format
8788
fairplay_spc FairPlay Server Playback Context
89+
fit ANT FIT
8890
flac Free Lossless Audio Codec file
8991
flac_frame FLAC frame
9092
flac_metadatablock FLAC metadatablock

‎format/all/all.go‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@ import (
2222
_ "github.com/wader/fq/format/dns"
2323
_ "github.com/wader/fq/format/elf"
2424
_ "github.com/wader/fq/format/fairplay"
25+
_ "github.com/wader/fq/format/fit"
2526
_ "github.com/wader/fq/format/flac"
2627
_ "github.com/wader/fq/format/gif"
2728
_ "github.com/wader/fq/format/gzip"

‎format/fit/fit.go‎

Lines changed: 354 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,354 @@
1+
package fit
2+
3+
import (
4+
"embed"
5+
"fmt"
6+
"sort"
7+
8+
"github.com/wader/fq/format"
9+
"github.com/wader/fq/format/fit/mappers"
10+
"github.com/wader/fq/pkg/decode"
11+
"github.com/wader/fq/pkg/interp"
12+
"github.com/wader/fq/pkg/scalar"
13+
"golang.org/x/text/encoding"
14+
)
15+
16+
var fitFS embed.FS
17+
18+
func init() {
19+
interp.RegisterFormat(
20+
format.FIT,
21+
&decode.Format{
22+
Description: "ANT FIT",
23+
Groups: []*decode.Group{format.Probe},
24+
DecodeFn: decodeFIT,
25+
})
26+
interp.RegisterFS(fitFS)
27+
}
28+
29+
var fitCRCTable = [16]uint16{
30+
0x0000, 0xCC01, 0xD801, 0x1400, 0xF001, 0x3C00, 0x2800, 0xE401,
31+
0xA001, 0x6C00, 0x7800, 0xB401, 0x5000, 0x9C01, 0x8801, 0x4400,
32+
}
33+
34+
func calcCRC(bytes []byte) uint16 {
35+
var crc uint16
36+
crc = 0
37+
for i := 0; i < len(bytes); i++ {
38+
// compute checksum of lower four bits of byte
39+
var byte = bytes[i]
40+
var tmp = fitCRCTable[crc&0xF]
41+
crc = (crc >> 4) & 0x0FFF
42+
crc = crc ^ tmp ^ fitCRCTable[byte&0xF]
43+
tmp = fitCRCTable[crc&0xF]
44+
crc = (crc >> 4) & 0x0FFF
45+
crc = crc ^ tmp ^ fitCRCTable[(byte>>4)&0xF]
46+
}
47+
48+
return crc
49+
}
50+
51+
type fitContext struct {
52+
dataSize int
53+
headerSize int
54+
}
55+
56+
type dataRecordContext struct {
57+
compressed bool
58+
data bool
59+
localMessageType int
60+
hasDeveloperFields bool
61+
}
62+
63+
type fileDescriptionContext struct {
64+
devIdx uint64
65+
fDefNo uint64
66+
typ string
67+
name string
68+
unit string
69+
nativeFieldNo uint64
70+
nativeMsgNo uint64
71+
}
72+
73+
type fieldDef struct {
74+
name string
75+
typ string
76+
format string
77+
unit string
78+
scale float64
79+
offset int64
80+
size int
81+
}
82+
83+
type devFieldDefMap map[uint64]map[uint64]fieldDef
84+
type localFieldDefMap map[uint64]map[uint64]fieldDef
85+
type localMsgIsDevDef map[uint64]bool
86+
87+
func fitDecodeFileHeader(d *decode.D, fc *fitContext) {
88+
frameStart := d.Pos()
89+
90+
// d.FieldStruct("ident", func(d *decode.D) {
91+
headerSize := d.FieldU8("headerSize")
92+
d.FieldU8("protocolVersion")
93+
d.FieldU16("profileVersion")
94+
dataSize := d.FieldU32("dataSize")
95+
96+
d.FieldRawLen("dataType", 4*8, d.AssertBitBuf([]byte(".FIT")))
97+
if headerSize == 14 {
98+
headerCRC := calcCRC(d.BytesRange(frameStart, int(headerSize)-2))
99+
d.FieldU16("crc", d.UintValidate(uint64(headerCRC)))
100+
}
101+
fc.headerSize = int(headerSize)
102+
fc.dataSize = int(dataSize)
103+
}
104+
105+
func fitDecodeDataRecordHeader(d *decode.D, drc *dataRecordContext) {
106+
drc.compressed = d.FieldBool("normalHeader", scalar.BoolMapDescription{false: "Normal header",
107+
true: "Compressed header"})
108+
if drc.compressed {
109+
localMessageType := d.FieldU2("localMessageType")
110+
d.FieldU32("timeOffset")
111+
drc.localMessageType = int(localMessageType)
112+
drc.data = true
113+
} else {
114+
mTypeIsDef := d.FieldBool("messageType", scalar.BoolMap{true: {Sym: 1, Description: "Definition message"},
115+
false: {Sym: 0, Description: "Data message"}})
116+
hasDeveloperFields := d.FieldBool("hasDeveloperFields")
117+
d.FieldBool("reserved")
118+
localMessageType := d.FieldU4("localMessageType")
119+
120+
drc.hasDeveloperFields = hasDeveloperFields
121+
drc.localMessageType = int(localMessageType)
122+
drc.data = !mTypeIsDef
123+
}
124+
}
125+
126+
func fitDecodeDefinitionMessage(d *decode.D, drc *dataRecordContext, lmfd localFieldDefMap, dmfd devFieldDefMap, isDevMap localMsgIsDevDef) {
127+
d.FieldU8("reserved")
128+
endian := d.FieldU8("architecture")
129+
switch endian {
130+
case 0:
131+
d.Endian = decode.LittleEndian
132+
case 1:
133+
d.Endian = decode.BigEndian
134+
default:
135+
d.Fatalf("Unknown endian %d", endian)
136+
}
137+
messageNo := d.FieldU16("globalMessageNumber", mappers.TypeDefMap["mesg_num"])
138+
if messageNo == 206 { // developer field_description
139+
isDevMap[uint64(drc.localMessageType)] = true
140+
} else {
141+
isDevMap[uint64(drc.localMessageType)] = false
142+
}
143+
numFields := d.FieldU8("fields")
144+
lmfd[uint64(drc.localMessageType)] = make(map[uint64]fieldDef, numFields)
145+
d.FieldArray("fieldDefinitions", func(d *decode.D) {
146+
for i := 0; i < int(numFields); i++ {
147+
d.FieldStruct("fieldDefinition", func(d *decode.D) {
148+
fieldDefNo := d.FieldU8("fieldDefNo", mappers.FieldDefMap[messageNo])
149+
size := d.FieldU8("size")
150+
baseType := d.FieldU8("baseType", mappers.TypeDefMap["fit_base_type"])
151+
152+
var typ = mappers.TypeDefMap["fit_base_type"][baseType].Name
153+
fDefLookup, isSet := mappers.FieldDefMap[messageNo][fieldDefNo]
154+
if isSet {
155+
var foundName = fDefLookup.Name
156+
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}
157+
} else {
158+
var foundName = fmt.Sprintf("UNKOWN_%d", fieldDefNo)
159+
lmfd[uint64(drc.localMessageType)][uint64(i)] = fieldDef{name: foundName, typ: typ, size: int(size), format: "unknown"}
160+
}
161+
})
162+
}
163+
})
164+
if drc.hasDeveloperFields {
165+
numDevFields := d.FieldU8("devFields")
166+
167+
d.FieldArray("devFieldDefinitions", func(d *decode.D) {
168+
for i := numFields; i < (numDevFields + numFields); i++ {
169+
d.FieldStruct("devFieldDefinition", func(d *decode.D) {
170+
fieldDefNo := d.FieldU8("fieldDefNo")
171+
size := d.FieldU8("size")
172+
devDataIdx := d.FieldU8("devDataIdx")
173+
174+
//baseType := d.FieldU8("baseType", mappers.TypeDefMap["fit_base_type"])
175+
176+
//var typ = mappers.TypeDefMap["fit_base_type"][baseType].Name
177+
typ := dmfd[devDataIdx][fieldDefNo].typ
178+
//fDefLookup, isSet := mappers.FieldDefMap[messageNo][fieldDefNo]
179+
fDefLookup, isSet := dmfd[devDataIdx][fieldDefNo]
180+
if isSet {
181+
var foundName = fDefLookup.name
182+
lmfd[uint64(drc.localMessageType)][uint64(i)] = fieldDef{name: foundName, typ: typ, size: int(size), unit: fDefLookup.unit, scale: fDefLookup.scale, offset: fDefLookup.offset}
183+
} else {
184+
var foundName = fmt.Sprintf("UNKOWN_%d", fieldDefNo)
185+
lmfd[uint64(drc.localMessageType)][uint64(i)] = fieldDef{name: foundName, typ: typ, size: int(size), format: "unknown"}
186+
}
187+
})
188+
}
189+
})
190+
}
191+
192+
}
193+
194+
func ensureDevFieldMap(dmfd devFieldDefMap, devIdx uint64, fieldDefNo uint64) {
195+
_, devIsSet := dmfd[devIdx]
196+
if !devIsSet {
197+
dmfd[devIdx] = make(map[uint64]fieldDef)
198+
}
199+
}
200+
201+
func fieldUint(fieldFn func(string, ...scalar.UintMapper) uint64, d *decode.D, fdc *fileDescriptionContext, expectedSize int, fDef fieldDef, uintFormatter scalar.UintFn) {
202+
var val uint64
203+
if fDef.size != expectedSize {
204+
d.FieldStr(fDef.name, fDef.size, encoding.Nop)
205+
} else {
206+
if uintFormatter != nil {
207+
val = fieldFn(fDef.name, uintFormatter)
208+
} else {
209+
val = fieldFn(fDef.name)
210+
}
211+
212+
switch fDef.name {
213+
case "developer_data_index":
214+
fdc.devIdx = val
215+
case "field_definition_number":
216+
fdc.fDefNo = val
217+
case "fit_base_type_id":
218+
fdc.typ = mappers.TypeDefMap["fit_base_type"][val].Name
219+
case "native_field_num":
220+
fdc.nativeFieldNo = val
221+
case "native_mesg_num":
222+
fdc.nativeMsgNo = val
223+
}
224+
}
225+
}
226+
227+
func fieldSint(fieldFn func(string, ...scalar.SintMapper) int64, d *decode.D, fdc *fileDescriptionContext, expectedSize int, fDef fieldDef, sintFormatter scalar.SintFn) {
228+
//var val uint64
229+
if fDef.size != expectedSize {
230+
d.FieldStr(fDef.name, fDef.size, encoding.Nop)
231+
} else {
232+
if sintFormatter != nil {
233+
fieldFn(fDef.name, sintFormatter)
234+
} else {
235+
fieldFn(fDef.name)
236+
}
237+
//setDevFieldUint(isDevDep, dmfd, fDef.name, val);
238+
}
239+
}
240+
241+
func fieldFloat(fieldFn func(string, ...scalar.FltMapper) float64, d *decode.D, fdc *fileDescriptionContext, expectedSize int, fDef fieldDef) {
242+
//var val uint64
243+
if fDef.size != expectedSize {
244+
d.FieldStr(fDef.name, fDef.size, encoding.Nop)
245+
} else {
246+
fieldFn(fDef.name)
247+
248+
//setDevFieldUint(isDevDep, dmfd, fDef.name, val);
249+
}
250+
}
251+
252+
func fieldString(d *decode.D, fdc *fileDescriptionContext, fDef fieldDef) {
253+
val := d.FieldUTF8NullFixedLen(fDef.name, fDef.size)
254+
255+
switch fDef.name {
256+
case "field_name":
257+
fdc.name = val
258+
case "units":
259+
fdc.unit = val
260+
}
261+
}
262+
263+
func fitDecodeDataMessage(d *decode.D, drc *dataRecordContext, lmfd localFieldDefMap, dmfd devFieldDefMap, isDevMap localMsgIsDevDef) {
264+
var fdc fileDescriptionContext
265+
keys := make([]int, len(lmfd[uint64(drc.localMessageType)]))
266+
i := 0
267+
for k := range lmfd[uint64(drc.localMessageType)] {
268+
keys[i] = int(k)
269+
i++
270+
}
271+
sort.Ints(keys)
272+
273+
isDevDep := isDevMap[uint64(drc.localMessageType)]
274+
275+
for _, k := range keys {
276+
fDef := lmfd[uint64(drc.localMessageType)][uint64(k)]
277+
278+
var uintFormatter = mappers.GetUintFormatter(fDef.format, fDef.unit, fDef.scale, fDef.offset)
279+
var sintFormatter = mappers.GetSintFormatter(fDef.format, fDef.unit, fDef.scale, fDef.offset)
280+
281+
switch fDef.typ {
282+
// case "byte":
283+
// d.FieldStr(fDef.name, fDef.size, encoding.Nop)
284+
case "enum", "uint8", "uint8z", "byte":
285+
fieldUint(d.FieldU8, d, &fdc, 1, fDef, uintFormatter)
286+
case "sint8":
287+
fieldSint(d.FieldS8, d, &fdc, 1, fDef, sintFormatter)
288+
case "sint16":
289+
fieldSint(d.FieldS16, d, &fdc, 2, fDef, sintFormatter)
290+
case "uint16", "uint16z":
291+
fieldUint(d.FieldU16, d, &fdc, 2, fDef, uintFormatter)
292+
case "sint32":
293+
fieldSint(d.FieldS32, d, &fdc, 4, fDef, sintFormatter)
294+
case "uint32", "uint32z":
295+
fieldUint(d.FieldU32, d, &fdc, 4, fDef, uintFormatter)
296+
case "float32":
297+
fieldFloat(d.FieldF32, d, &fdc, 4, fDef)
298+
case "float64":
299+
fieldFloat(d.FieldF64, d, &fdc, 8, fDef)
300+
case "sint64":
301+
fieldSint(d.FieldS64, d, &fdc, 4, fDef, sintFormatter)
302+
case "uint64", "uint64z":
303+
fieldUint(d.FieldU64, d, &fdc, 8, fDef, uintFormatter)
304+
case "string":
305+
fieldString(d, &fdc, fDef)
306+
default:
307+
d.Fatalf("Unknown type %s", fDef.typ)
308+
}
309+
}
310+
311+
if isDevDep {
312+
ensureDevFieldMap(dmfd, fdc.devIdx, fdc.fDefNo)
313+
dmfd[fdc.devIdx][fdc.fDefNo] = fieldDef{name: fdc.name, typ: fdc.typ, unit: fdc.unit, scale: 0, offset: 0}
314+
}
315+
}
316+
317+
func decodeFIT(d *decode.D) any {
318+
var fc fitContext
319+
d.Endian = decode.LittleEndian
320+
321+
var lmfd localFieldDefMap = make(localFieldDefMap)
322+
var dmfd devFieldDefMap = make(devFieldDefMap)
323+
var isDevMap localMsgIsDevDef = make(localMsgIsDevDef)
324+
325+
//decodeBSONDocument(d)
326+
d.FieldStruct("header", func(d *decode.D) { fitDecodeFileHeader(d, &fc) })
327+
328+
d.FieldArray("dataRecords", func(d *decode.D) {
329+
// // headerPos := d.Pos()
330+
331+
for d.Pos() < int64((fc.headerSize+fc.dataSize)*8) {
332+
d.FieldStruct("dataRecord", func(d *decode.D) {
333+
var drc dataRecordContext
334+
d.FieldStruct("dataRecordHeader", func(d *decode.D) { fitDecodeDataRecordHeader(d, &drc) })
335+
switch drc.data {
336+
case true:
337+
d.FieldStruct("dataMessage", func(d *decode.D) { fitDecodeDataMessage(d, &drc, lmfd, dmfd, isDevMap) })
338+
case false:
339+
d.FieldStruct("definitionMessage", func(d *decode.D) { fitDecodeDefinitionMessage(d, &drc, lmfd, dmfd, isDevMap) })
340+
}
341+
})
342+
}
343+
})
344+
345+
var fileCRC uint16
346+
if fc.headerSize == 12 {
347+
fileCRC = calcCRC(d.BytesRange(0, fc.dataSize+fc.headerSize)) // 12 byte header - CRC whole file except the CRC itself
348+
} else {
349+
fileCRC = calcCRC(d.BytesRange(14*8, fc.dataSize)) // 14 byte header - CRC everything below header except the CRC itself
350+
}
351+
d.FieldU16("crc", d.UintValidate(uint64(fileCRC)))
352+
353+
return nil
354+
}

0 commit comments

Comments
 (0)