basic encoding and decoding

2026-08-20 14:19:00 +00:00 · 2024-12-24 15:47:54 -06:00
parent 46c2dfc512
commit 324f602d97
3 changed files with 228 additions and 0 deletions
@@ -0,0 +1,139 @@
+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
+}
@@ -0,0 +1,86 @@
+package freeD
+
+import (
+	"fmt"
+	"reflect"
+	"testing"
+)
+
+func TestMessageDecoding(t *testing.T) {
+	testCases := []struct {
+		description string
+		bytes       []byte
+		expected    FreeDPosition
+	}{
+		{
+			description: "simple position message",
+			bytes: []byte{0xd1, 0x01, 0x5a, 0x00, 0x00, 0x2d, 0x00, 0x00, 0xa6, 0x00, 0x00, 0x7f, 0xff, 0x40, 0x7f, 0xff, 0x80, 0x7f, 0xff,
+				0xc0, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x00, 0x00, 50,
+			},
+			expected: FreeDPosition{
+				ID:    1,
+				Pan:   180,
+				Tilt:  90,
+				Roll:  -180,
+				PosX:  131069,
+				PosY:  131070,
+				PosZ:  131071,
+				Zoom:  66051,
+				Focus: 263430,
+			},
+		},
+	}
+
+	for _, testCase := range testCases {
+
+		actual, err := Decode(testCase.bytes)
+
+		if err != nil {
+			t.Errorf("Test '%s' failed to decode chunk properly", testCase.description)
+			fmt.Println(err)
+		}
+
+		if !reflect.DeepEqual(actual, testCase.expected) {
+			t.Errorf("Test '%s' failed to decode chunk properly", testCase.description)
+			fmt.Printf("expected: %+v\n", testCase.expected)
+			fmt.Printf("actual: %+v\n", actual)
+		}
+	}
+}
+
+func TestMessageEncoding(t *testing.T) {
+	testCases := []struct {
+		description string
+		expected    []byte
+		message     FreeDPosition
+	}{
+		{
+			description: "simple position message",
+			expected: []byte{0xd1, 0x01, 0x5a, 0x00, 0x00, 0x2d, 0x00, 0x00, 0xa6, 0x00, 0x00, 0x7f, 0xff, 0x40, 0x7f, 0xff, 0x80, 0x7f, 0xff,
+				0xc0, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x00, 0x00, 50,
+			},
+			message: FreeDPosition{
+				ID:    1,
+				Pan:   180,
+				Tilt:  90,
+				Roll:  -180,
+				PosX:  131069,
+				PosY:  131070,
+				PosZ:  131071,
+				Zoom:  66051,
+				Focus: 263430,
+			},
+		},
+	}
+
+	for _, testCase := range testCases {
+
+		actual := Encode(testCase.message)
+
+		if !reflect.DeepEqual(actual, testCase.expected) {
+			t.Errorf("Test '%s' failed to decode chunk properly", testCase.description)
+			fmt.Printf("expected: %+v\n", testCase.expected)
+			fmt.Printf("actual: %+v\n", actual)
+		}
+	}
+}