fix pbh

fsd/handler.go

@@ -151,13 +151,17 @@ func (s *Server) handlePilotPosition(client *Client, packet []byte) {
 
 	// Update state
 	client.transponder.Store(string(getField(packet, 2)))
+
 	groundspeed, _ := strconv.Atoi(string(getField(packet, 7)))
 	client.groundspeed.Store(int32(groundspeed))
+
 	altitude, _ := strconv.Atoi(string(getField(packet, 6)))
 	client.altitude.Store(int32(altitude))
+
 	pbhUint, _ := strconv.ParseUint(string(getField(packet, 8)), 10, 32)
-	_, _, heading := pitchBankHeading(pbhUint).vals()
+	_, _, heading := pitchBankHeading(uint32(pbhUint))
 	client.heading.Store(int32(heading))
+
 	client.lastUpdated.Store(time.Now())
 }
 

fsd/util.go

@@ -4,7 +4,6 @@ import (
 	"bytes"
 	"encoding/base64"
 	"encoding/json"
-	"errors"
 	"slices"
 	"strconv"
 	"strings"
@@ -377,27 +376,14 @@ func buildBeaconCodePacket(source, recipient, targetCallsign, beaconCode string)
 	return builder.String()
 }
 
-type pitchBankHeading uint32
+func pitchBankHeading(packed uint32) (pitch float64, bank float64, heading float64) {
+	// Map 11 bits of resolution to degrees [0..359]
+	const conversionRatio float64 = 359.0 / 1023.0
+	const mask uint32 = 1023 // 0b1111111111
 
-const maxPbhValue = 0b1111111111
-
-func newPitchBankHeading(pitch uint32, bank uint32, heading uint32) (pbh pitchBankHeading, err error) {
-	if pitch > maxPbhValue || bank > maxPbhValue || heading > maxPbhValue {
-		err = errors.New("out of range")
-		return
-	}
-
-	pbh = (pbh | pitchBankHeading(pitch)) << 10
-	pbh = (pbh | pitchBankHeading(bank)) << 10
-	pbh = (pbh | pitchBankHeading(heading)) << 2
-
-	return
-}
-
-func (pbh pitchBankHeading) vals() (pitch uint32, bank uint32, heading uint32) {
-	pitch = uint32(pbh>>22) & maxPbhValue
-	bank = uint32(pbh>>12) & maxPbhValue
-	heading = uint32(pbh>>2) & maxPbhValue
+	pitch = float64(packed>>22&mask) * conversionRatio
+	bank = float64(packed>>12&mask) * conversionRatio
+	heading = float64(packed>>2&mask) * conversionRatio
 
 	return
 }

fsd/util_test.go

@@ -1,6 +1,7 @@
 package fsd
 
 import (
+	"fmt"
 	"testing"
 )
 
@@ -52,3 +53,10 @@ func TestGetField(t *testing.T) {
 		}
 	}
 }
+
+func TestPitchBankHeading(t *testing.T) {
+	pitch, bank, heading := pitchBankHeading(4261294148)
+	fmt.Println(pitch)
+	fmt.Println(bank)
+	fmt.Println(heading)
+}