429 lines
7.4 KiB
Go
429 lines
7.4 KiB
Go
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:]
|
|
}
|