protocol/aprs: refactored

protocol/aprs/mice.go

@@ -0,0 +1,428 @@
+package aprs
+
+import (
+	"bytes"
+	"errors"
+	"fmt"
+)
+
+type MessageType int
+
+const (
+	MsgEmergency MessageType = iota
+	MsgPriority
+	MsgSpecial
+	MsgCommitted
+	MsgReturning
+	MsgInService
+	MsgEnRoute
+	MsgOffDuty
+	MsgCustom0
+	MsgCustom1
+	MsgCustom2
+	MsgCustom3
+	MsgCustom4
+	MsgCustom5
+	MsgCustom6
+)
+
+type MicE struct {
+	// Core
+	Latitude     float64
+	Longitude    float64
+	Ambiguity    int
+	Velocity     *Velocity
+	Symbol       string
+	Type         MessageType
+	HasMessaging bool
+
+	// Extensions
+	Altitude  float64 // in meters
+	DAO       *DAO
+	Telemetry *Telemetry
+	Comment   string
+
+	// Raw
+	RawDest string
+	RawInfo []byte
+}
+
+func (report *MicE) String() string {
+	return report.Comment
+}
+
+type DAO struct {
+	LatOffset float64
+	LonOffset float64
+}
+
+type micEDecoder struct{}
+
+func (d micEDecoder) CanDecode(frame *Frame) bool {
+	if len(frame.Destination.Call) == 6 && len(frame.Raw) >= 8 {
+		for i := range 6 {
+			if _, _, _, _, ok := decodeDestChar(frame.Destination.Call[i]); !ok {
+				return false
+			}
+		}
+		return true
+	}
+	return false
+}
+
+func (d micEDecoder) Decode(frame *Frame) (data Data, err error) {
+	if len(frame.Destination.Call) < 6 {
+		return nil, errors.New("destination too short for Mic-E")
+	}
+	if len(frame.Raw) < 9 {
+		return nil, errors.New("info field too short for Mic-E")
+	}
+
+	var (
+		r           = new(MicE)
+		north, west bool
+		dest        = frame.Destination.Call
+		info        = []byte(frame.Raw[1:])
+	)
+	if r.Latitude, r.Type, r.Ambiguity, north, err = r.decodeLatitude(dest); err != nil {
+		return
+	}
+	if r.Longitude, west, r.Velocity, r.Symbol, err = r.decodeMicELongitudeAndMotion(dest, info); err != nil {
+		return
+	}
+
+	if !north {
+		r.Latitude = -r.Latitude
+	}
+	if west {
+		r.Longitude = -r.Longitude
+	}
+
+	r.parseExtensions(info[8:])
+
+	return r, nil
+}
+
+func (r *MicE) decodeLatitude(dest string) (
+	lat float64,
+	msg MessageType,
+	ambiguity int,
+	north bool,
+	err error,
+) {
+	if len(dest) != 6 {
+		return 0, 0, 0, false, errors.New("aprs: MicE destination must be 6 chars")
+	}
+
+	var digits [6]int
+	var msgBits int
+
+	for i := range 6 {
+		c := dest[i]
+
+		d, amb, nBit, mBit, ok := decodeDestChar(c)
+		if !ok {
+			return 0, 0, 0, false, fmt.Errorf("invalid dest char %q", c)
+		}
+
+		digits[i] = d
+
+		if amb {
+			ambiguity++
+		}
+
+		// Only first 3 chars contain message bits
+		if i < 3 && mBit {
+			msgBits |= 1 << (2 - i)
+		}
+
+		// North bit defined by char 3
+		if i == 3 {
+			north = nBit
+		}
+	}
+
+	// Apply ambiguity masking per spec
+	maskAmbiguity(digits[:], ambiguity)
+
+	deg := digits[0]*10 + digits[1]
+	min := digits[2]*10 + digits[3]
+	hun := digits[4]*10 + digits[5]
+
+	if deg > 89 || min > 59 || hun > 99 {
+		return 0, 0, 0, false, errors.New("invalid latitude range")
+	}
+
+	lat = float64(deg) +
+		float64(min)/60.0 +
+		float64(hun)/6000.0
+
+	if !north {
+		lat = -lat
+	}
+
+	return lat, r.interpretMessage(msgBits), ambiguity, north, nil
+}
+
+func decodeDestChar(c byte) (
+	digit int,
+	ambiguity bool,
+	northBit bool,
+	msgBit bool,
+	ok bool,
+) {
+	switch {
+	case c >= '0' && c <= '9':
+		return int(c - '0'), false, true, false, true
+
+	case c >= 'A' && c <= 'J':
+		return int(c - 'A'), false, true, true, true
+
+	case c >= 'P' && c <= 'Y':
+		return int(c - 'P'), false, false, true, true
+
+	case c == 'K' || c == 'L' || c == 'Z':
+		return 0, true, true, false, true
+
+	default:
+		return 0, false, false, false, false
+	}
+}
+
+func maskAmbiguity(digits []int, ambiguity int) {
+	for i := 0; i < ambiguity && i < len(digits); i++ {
+		idx := len(digits) - 1 - i
+		digits[idx] = 0
+	}
+}
+
+func (r *MicE) decodeMicELongitudeAndMotion(dest string, info []byte) (lon float64, west bool, velocity *Velocity, symbol string, err error) {
+	if len(info) < 3 {
+		err = errors.New("info too short for longitude")
+		return
+	}
+
+	d := int(info[0]) - 28
+	m := int(info[1]) - 28
+	h := int(info[2]) - 28
+	if d < 0 || m < 0 || h < 0 {
+		err = errors.New("invalid longitude encoding")
+		return
+	}
+
+	// 100° offset bit from dest[4]
+	if dest[4] >= 'P' {
+		d += 100
+	}
+
+	// Wrap correction
+	if d >= 180 {
+		d -= 80
+	}
+
+	if m >= 60 {
+		m -= 60
+		d++
+	}
+
+	lon = float64(d) +
+		float64(m)/60.0 +
+		float64(h)/6000.0
+
+	// East/West from dest[5]
+	west = dest[5] >= 'P'
+	if west {
+		lon = -lon
+	}
+
+	// Speed/course
+	if len(info) >= 6 {
+		s1 := int(info[3]) - 28
+		s2 := int(info[4]) - 28
+		s3 := int(info[5]) - 28
+
+		if !(s1 < 0 || s2 < 0 || s3 < 0) {
+			speed := s1*10 + s2/10
+			course := (s2%10)*100 + s3
+
+			if speed >= 800 {
+				speed -= 800
+			}
+			if course >= 400 {
+				course -= 400
+			}
+
+			if course >= 360 {
+				course %= 360
+			}
+
+			velocity = &Velocity{
+				Speed:  knotsToMetersPerSecond(float64(speed)),
+				Course: course,
+			}
+		}
+	}
+
+	// Symbol
+	if len(info) >= 8 {
+		symbol = string([]byte{info[7], info[6]})
+	}
+
+	return
+}
+
+func (report *MicE) interpretMessage(bits int) MessageType {
+	if bits == 0 {
+		return MsgEmergency
+	}
+	if bits <= 7 {
+		return MessageType(bits)
+	}
+	return MsgCustom0
+}
+
+func (report *MicE) parseExtensions(info []byte) {
+	info = report.parseOldTelemetry(info)
+	info = report.parseNewTelemetry(info)
+	info = report.parseAltitude(info)
+	info = report.parseDAO(info)
+	info = report.parseRadioPrefix(info)
+
+	// Remainder is comment
+	if len(info) > 0 {
+		report.Comment = string(info)
+	}
+}
+
+func (report *MicE) parseRadioPrefix(info []byte) []byte {
+	if len(info) == 0 {
+		return info
+	}
+
+	switch info[0] {
+	case '>', ']': // Kenwood
+		return info[1:]
+	case '`': // Yaesu / Byonics / etc.
+		report.HasMessaging = true
+		return info[1:]
+	case '\'': // Yaesu / Byonics / etc.
+		return info[1:]
+	default:
+		return info
+	}
+}
+
+func (report *MicE) parseOldTelemetry(info []byte) []byte {
+	if len(info) < 1 {
+		return info
+	}
+
+	b := info[0]
+	if b != '`' && b != '\'' {
+		return info
+	}
+
+	// Need 6 bytes total
+	if 6 > len(info) {
+		return info // not fatal, ignore malformed
+	}
+
+	data := info[1:6]
+	values := make([]int, 5)
+	for i := 0; i < 5; i++ {
+		values[i] = int(data[i] - 33)
+	}
+
+	report.Telemetry = &Telemetry{
+		Analog: values,
+	}
+
+	return info[6:]
+}
+
+func (report *MicE) parseNewTelemetry(info []byte) []byte {
+	i := bytes.IndexByte(info, '|')
+	if i == -1 {
+		return info
+	}
+
+	prefix, data := info[:i], info[i+1:]
+	if i = bytes.IndexByte(data, '|'); i == -1 {
+		return info
+	}
+	suffix, data := data[i+1:], data[:i]
+	if len(data) < 2 || len(data)%2 != 0 {
+		return info
+	}
+
+	report.Telemetry = new(Telemetry)
+	var err error
+	if report.Telemetry.ID, err = base91Decode(string(data[:2])); err != nil {
+		return info
+	}
+	data = data[2:]
+
+	for range 5 {
+		if len(data) == 0 {
+			break
+		}
+		var value int
+		if value, err = base91Decode(string(data[:2])); err != nil {
+			return info
+		}
+		report.Telemetry.Analog = append(report.Telemetry.Analog, value)
+		data = data[2:]
+	}
+
+	if len(data) > 0 {
+		var digital int
+		if digital, err = base91Decode(string(data[:2])); err != nil {
+			return info
+		}
+		for range 8 {
+			report.Telemetry.Digital = append(report.Telemetry.Digital, (digital&1) == 1)
+			digital >>= 1
+		}
+	}
+
+	return append(prefix, suffix...)
+}
+
+func (report *MicE) parseAltitude(info []byte) []byte {
+	if 4 > len(info) {
+		return info
+	}
+
+	if info[3] != '}' {
+		return info
+	}
+
+	alt, err := base91Decode(string(info[:3]))
+	if err != nil {
+		return info
+	}
+	value := alt - 10000
+	report.Altitude = feetToMeters(float64(value))
+
+	return info[4:]
+}
+
+func (report *MicE) parseDAO(info []byte) []byte {
+	if 6 > len(info) {
+		return info
+	}
+
+	if info[0] != '!' || info[1] != 'W' {
+		return info
+	}
+
+	latOff := float64(info[2]-33) / 10000.0
+	lonOff := float64(info[3]-33) / 10000.0
+
+	report.DAO = &DAO{
+		LatOffset: latOff,
+		LonOffset: lonOff,
+	}
+
+	return info[6:]
+}