mirror of
https://github.com/jwetzell/free-d-go.git
synced 2026-08-17 21:13:55 +00:00
Joel Wetzell
129 lines
3.0 KiB
Go
129 lines
3.0 KiB
Go
package freeD
|
|
|
|
import (
|
|
"encoding/binary"
|
|
"errors"
|
|
"math"
|
|
)
|
|
|
|
type FreeDPosition struct {
|
|
ID uint8
|
|
Pan float32
|
|
Tilt float32
|
|
Roll float32
|
|
PosX float32
|
|
PosY float32
|
|
PosZ float32
|
|
Zoom int32
|
|
Focus int32
|
|
}
|
|
|
|
func Encode(message FreeDPosition) [29]byte {
|
|
var bytes [29]byte
|
|
|
|
bytes[0] = 0xd1
|
|
bytes[1] = message.ID
|
|
|
|
copy(bytes[2:5], rotationToFreeDUnits(message.Pan))
|
|
copy(bytes[5:8], rotationToFreeDUnits(message.Tilt))
|
|
copy(bytes[8:11], rotationToFreeDUnits(message.Roll))
|
|
|
|
copy(bytes[11:14], positionToFreeDUnits(message.PosX))
|
|
copy(bytes[14:17], positionToFreeDUnits(message.PosY))
|
|
copy(bytes[17:20], positionToFreeDUnits(message.PosZ))
|
|
|
|
bytes[20] = uint8(math.Trunc(float64(message.Zoom / 65536)))
|
|
bytes[21] = uint8(int64(message.Zoom/256) % 256)
|
|
bytes[22] = uint8(int32(message.Zoom) % 256)
|
|
|
|
bytes[23] = uint8(math.Trunc(float64(message.Focus / 65536)))
|
|
bytes[24] = uint8(int64(message.Focus/256) % 256)
|
|
bytes[25] = uint8(int32(message.Focus) % 256)
|
|
|
|
// spare area?
|
|
bytes[26] = 0
|
|
bytes[27] = 0
|
|
|
|
bytes[28] = checksum(bytes[0:28])
|
|
return bytes
|
|
}
|
|
|
|
func Decode(bytes [29]byte) (FreeDPosition, error) {
|
|
if bytes[0] != 0xd1 {
|
|
return FreeDPosition{}, errors.New("only FreeD position messages are currently supported")
|
|
}
|
|
|
|
checksum := checksum(bytes[0 : len(bytes)-1])
|
|
|
|
if checksum != bytes[len(bytes)-1] {
|
|
return FreeDPosition{}, errors.New("FreeD packet checksum failed")
|
|
}
|
|
|
|
id := bytes[1]
|
|
pan := freeDUnitsToRotation(bytes[2], bytes[3], bytes[4])
|
|
tilt := freeDUnitsToRotation(bytes[5], bytes[6], bytes[7])
|
|
roll := freeDUnitsToRotation(bytes[8], bytes[9], bytes[10])
|
|
|
|
posX := freeDUnitsToPosition(bytes[11], bytes[12], bytes[13])
|
|
posY := freeDUnitsToPosition(bytes[14], bytes[15], bytes[16])
|
|
posZ := freeDUnitsToPosition(bytes[17], bytes[18], bytes[19])
|
|
|
|
zoom := int32(bytes[20])*65536 + int32(bytes[21])*256 + int32(bytes[22])
|
|
focus := int32(bytes[23])*65536 + int32(bytes[24])*256 + int32(bytes[25])
|
|
|
|
return FreeDPosition{
|
|
ID: id,
|
|
Pan: pan,
|
|
Tilt: tilt,
|
|
Roll: roll,
|
|
PosX: posX,
|
|
PosY: posY,
|
|
PosZ: posZ,
|
|
Zoom: zoom,
|
|
Focus: focus,
|
|
}, nil
|
|
}
|
|
|
|
func checksum(bytes []byte) uint8 {
|
|
var checksum uint8 = 0x40
|
|
|
|
for _, value := range bytes {
|
|
checksum -= value
|
|
}
|
|
return checksum
|
|
}
|
|
|
|
func rotationToFreeDUnits(rotation float32) []byte {
|
|
units := int32(rotation * 32768 * 256)
|
|
|
|
buf := make([]byte, 4)
|
|
binary.BigEndian.PutUint32(buf, uint32(units))
|
|
return buf[0:3]
|
|
}
|
|
|
|
func positionToFreeDUnits(position float32) []byte {
|
|
units := int32(position * 64 * 256)
|
|
|
|
buf := make([]byte, 4)
|
|
binary.BigEndian.PutUint32(buf, uint32(units))
|
|
return buf[0:3]
|
|
}
|
|
|
|
func freeDUnitsToRotation(upper uint8, middle uint8, lower uint8) float32 {
|
|
var units int32
|
|
units |= int32(lower) << 8
|
|
units |= int32(middle) << 16
|
|
units |= int32(upper) << 24
|
|
|
|
return float32(units) / 256 / 32768
|
|
}
|
|
|
|
func freeDUnitsToPosition(upper uint8, middle uint8, lower uint8) float32 {
|
|
var units int32
|
|
units |= int32(lower) << 8
|
|
units |= int32(middle) << 16
|
|
units |= int32(upper) << 24
|
|
|
|
return float32(units) / 256 / 64
|
|
}
|