forked from JuanIrache/gopro-utils
-
Notifications
You must be signed in to change notification settings - Fork 0
/
accl.go
36 lines (29 loc) · 1018 Bytes
/
accl.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
package telemetry
import (
"math"
"encoding/binary"
"errors"
)
// Accelerometer in m/s for XYZ
type ACCL struct {
X float64
Y float64
Z float64
}
func (accl *ACCL) Parse(bytes []byte, scale *SCAL) error {
// hero 5/6 have 2 bytes per axis
if 6 == len(bytes) {
accl.X = float64(int16(binary.BigEndian.Uint16(bytes[0:2]))) / float64(scale.Values[0])
accl.Y = float64(int16(binary.BigEndian.Uint16(bytes[2:4]))) / float64(scale.Values[0])
accl.Z = float64(int16(binary.BigEndian.Uint16(bytes[4:6]))) / float64(scale.Values[0])
return nil
}
// fusion cameras have 4 bytes per axis (float32)
if 12 == len(bytes) {
accl.X = float64(math.Float32frombits(binary.BigEndian.Uint32(bytes[0:4]))) / float64(scale.Values[0])
accl.Y = float64(math.Float32frombits(binary.BigEndian.Uint32(bytes[4:8]))) / float64(scale.Values[0])
accl.Z = float64(math.Float32frombits(binary.BigEndian.Uint32(bytes[8:12]))) / float64(scale.Values[0])
return nil
}
return errors.New("Invalid length ACCL packet")
}