1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
|
package org.terst.nav.nmea
import org.terst.nav.ais.AisVessel
class AisVdmParser {
// Keyed by seqId -> list of (seq, payload)
private val fragments = mutableMapOf<String, MutableList<Pair<Int, String>>>()
fun parse(sentence: String): AisVessel? {
if (!sentence.startsWith("!AIVDM") && !sentence.startsWith("!AIVDO")) return null
val withoutBang = sentence.drop(1)
val starIdx = withoutBang.indexOf('*')
val body = if (starIdx >= 0) withoutBang.substring(0, starIdx) else withoutBang
val fields = body.split(",")
if (fields.size < 7) return null
val count = fields[1].toIntOrNull() ?: return null
val seq = fields[2].toIntOrNull() ?: return null
val seqId = fields[3]
val payload = fields[5]
val padding = fields[6].toIntOrNull() ?: 0
val combinedPayload: String
if (count <= 1) {
combinedPayload = payload
} else {
val list = fragments.getOrPut(seqId) { mutableListOf() }
list.add(seq to payload)
if (list.size < count) return null
fragments.remove(seqId)
combinedPayload = list.sortedBy { it.first }.joinToString("") { it.second }
}
val bits = decodeToBits(combinedPayload, padding)
if (bits.size < 6) return null
return when (extractUInt(bits, 0, 6)) {
1, 2, 3 -> parseType123(bits)
5 -> parseType5(bits)
else -> null
}
}
private fun decodeToBits(payload: String, padding: Int): IntArray {
val allBits = ArrayList<Int>(payload.length * 6)
for (c in payload) {
var v = c.code - 48
if (v > 39) v -= 8
for (b in 5 downTo 0) allBits.add((v ushr b) and 1)
}
return if (padding > 0 && padding < allBits.size)
allBits.dropLast(padding).toIntArray()
else
allBits.toIntArray()
}
private fun extractUInt(bits: IntArray, start: Int, len: Int): Int {
var v = 0
for (i in 0 until len) {
val bit = if (start + i < bits.size) bits[start + i] else 0
v = (v shl 1) or bit
}
return v
}
private fun extractInt(bits: IntArray, start: Int, len: Int): Int {
val unsigned = extractUInt(bits, start, len)
return if (len > 0 && (unsigned and (1 shl (len - 1))) != 0)
unsigned - (1 shl len)
else
unsigned
}
private fun parseType123(bits: IntArray): AisVessel? {
if (bits.size < 137) return null
val mmsi = extractUInt(bits, 8, 30)
val sogRaw = extractUInt(bits, 50, 10)
val sog = if (sogRaw == 1023) 0.0 else sogRaw / 10.0
val lonRaw = extractInt(bits, 61, 28)
val lon = if (lonRaw == 0x6791AC0) 0.0 else lonRaw / 600000.0
val latRaw = extractInt(bits, 89, 27)
val lat = if (latRaw == 0x3412140) 0.0 else latRaw / 600000.0
val cogRaw = extractUInt(bits, 116, 12)
val cog = if (cogRaw == 3600) 0.0 else cogRaw / 10.0
val heading = extractUInt(bits, 128, 9)
return AisVessel(
mmsi = mmsi,
name = "",
callsign = "",
lat = lat,
lon = lon,
sog = sog,
cog = cog,
heading = heading,
vesselType = 0,
timestampMs = System.currentTimeMillis()
)
}
private fun parseType5(bits: IntArray): AisVessel? {
if (bits.size < 240) return null
val mmsi = extractUInt(bits, 8, 30)
val callsign = decodeText(bits, 70, 7).trimEnd('@', ' ')
val name = decodeText(bits, 112, 20).trimEnd('@', ' ')
val vesselType = extractUInt(bits, 232, 8)
return AisVessel(
mmsi = mmsi,
name = name,
callsign = callsign,
lat = 0.0, lon = 0.0, sog = 0.0, cog = 0.0,
heading = 511,
vesselType = vesselType,
timestampMs = System.currentTimeMillis()
)
}
// AIS 6-bit text: bits<32 -> bits+64 (maps to @,A-Z,...), else bits (maps to space,digits,symbols)
private fun decodeText(bits: IntArray, start: Int, nChars: Int): String {
val sb = StringBuilder(nChars)
for (i in 0 until nChars) {
val v = extractUInt(bits, start + i * 6, 6)
val c = if (v < 32) v + 64 else v
sb.append(c.toChar())
}
return sb.toString()
}
}
|
