mirror of
https://github.com/jwetzell/free-d-go.git
synced 2026-08-19 13:53:58 +00:00
139 lines
3.3 KiB
Go
139 lines
3.3 KiB
Go
package freeD
|
|
|
|
import (
|
|
"bytes"
|
|
"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) []byte {
|
|
bytes := []byte{}
|
|
|
|
bytes = append(bytes, 0xd1)
|
|
bytes = append(bytes, message.ID)
|
|
|
|
bytes = append(bytes, rotationToFreeDUnits(message.Pan)...)
|
|
bytes = append(bytes, rotationToFreeDUnits(message.Tilt)...)
|
|
bytes = append(bytes, rotationToFreeDUnits(message.Roll)...)
|
|
|
|
bytes = append(bytes, positionToFreeDUnits(message.PosX)...)
|
|
bytes = append(bytes, positionToFreeDUnits(message.PosY)...)
|
|
bytes = append(bytes, positionToFreeDUnits(message.PosZ)...)
|
|
|
|
bytes = append(bytes, uint8(math.Trunc(float64(message.Zoom/65536))))
|
|
bytes = append(bytes, uint8(int64(message.Zoom/256)%256))
|
|
bytes = append(bytes, uint8(int32(message.Zoom)%256))
|
|
|
|
bytes = append(bytes, uint8(math.Trunc(float64(message.Focus/65536))))
|
|
bytes = append(bytes, uint8(int64(message.Focus/256)%256))
|
|
bytes = append(bytes, uint8(int32(message.Focus)%256))
|
|
|
|
// spare area?
|
|
bytes = append(bytes, 0, 0)
|
|
|
|
bytes = append(bytes, checksum(bytes[0:28]))
|
|
return bytes
|
|
}
|
|
|
|
func Decode(bytes []byte) (FreeDPosition, error) {
|
|
if len(bytes) != 29 {
|
|
return FreeDPosition{}, errors.New("FreeD packet must be 29 bytes long")
|
|
}
|
|
|
|
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)
|
|
|
|
var buf bytes.Buffer
|
|
err := binary.Write(&buf, binary.BigEndian, units)
|
|
if err != nil {
|
|
panic(err)
|
|
}
|
|
return buf.Bytes()[0:3]
|
|
}
|
|
|
|
func positionToFreeDUnits(position float32) []byte {
|
|
units := int32(position * 64 * 256)
|
|
|
|
var buf bytes.Buffer
|
|
err := binary.Write(&buf, binary.BigEndian, units)
|
|
if err != nil {
|
|
panic(err)
|
|
}
|
|
return buf.Bytes()[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
|
|
}
|