diff --git a/fsd/handler.go b/fsd/handler.go index 7c24ed8..e8c1b62 100644 --- a/fsd/handler.go +++ b/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()) } diff --git a/fsd/util.go b/fsd/util.go index 1069822..35dc8b4 100644 --- a/fsd/util.go +++ b/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 } diff --git a/fsd/util_test.go b/fsd/util_test.go index 4852f26..8983ddb 100644 --- a/fsd/util_test.go +++ b/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) +}