diff options
Diffstat (limited to 'test-runner/src/main/kotlin/org/terst')
3 files changed, 189 insertions, 0 deletions
diff --git a/test-runner/src/main/kotlin/org/terst/nav/ais/AisVessel.kt b/test-runner/src/main/kotlin/org/terst/nav/ais/AisVessel.kt new file mode 100644 index 0000000..34a951c --- /dev/null +++ b/test-runner/src/main/kotlin/org/terst/nav/ais/AisVessel.kt @@ -0,0 +1,14 @@ +package org.terst.nav.ais + +data class AisVessel( + val mmsi: Int, + val name: String, + val callsign: String, + val lat: Double, + val lon: Double, + val sog: Double, + val cog: Double, + val heading: Int, + val vesselType: Int, + val timestampMs: Long +)
\ No newline at end of file diff --git a/test-runner/src/main/kotlin/org/terst/nav/ais/CpaCalculator.kt b/test-runner/src/main/kotlin/org/terst/nav/ais/CpaCalculator.kt new file mode 100644 index 0000000..7e98451 --- /dev/null +++ b/test-runner/src/main/kotlin/org/terst/nav/ais/CpaCalculator.kt @@ -0,0 +1,46 @@ +package org.terst.nav.ais + +import kotlin.math.cos +import kotlin.math.sqrt +import kotlin.math.PI + +object CpaCalculator { + fun compute( + ownLat: Double, ownLon: Double, ownSog: Double, ownCog: Double, + tgtLat: Double, tgtLon: Double, tgtSog: Double, tgtCog: Double + ): Pair<Double, Double> { + val refLat = Math.toRadians((ownLat + tgtLat) / 2.0) + + // Flat-earth positions in nautical miles + val ownX = ownLon * 60.0 * cos(refLat) + val ownY = ownLat * 60.0 + val tgtX = tgtLon * 60.0 * cos(refLat) + val tgtY = tgtLat * 60.0 + + // Velocities in nm/min + val ownCogRad = Math.toRadians(ownCog) + val tgtCogRad = Math.toRadians(tgtCog) + val ownVx = ownSog * Math.sin(ownCogRad) / 60.0 + val ownVy = ownSog * Math.cos(ownCogRad) / 60.0 + val tgtVx = tgtSog * Math.sin(tgtCogRad) / 60.0 + val tgtVy = tgtSog * Math.cos(tgtCogRad) / 60.0 + + val dx = tgtX - ownX + val dy = tgtY - ownY + val dvx = tgtVx - ownVx + val dvy = tgtVy - ownVy + + val dv2 = dvx * dvx + dvy * dvy + if (dv2 < 1e-9) { + val currentDist = sqrt(dx * dx + dy * dy) + return Pair(currentDist, 0.0) + } + + val tcpa = -(dx * dvx + dy * dvy) / dv2 + val cpaX = dx + dvx * tcpa + val cpaY = dy + dvy * tcpa + val cpa = sqrt(cpaX * cpaX + cpaY * cpaY) + + return Pair(cpa, tcpa) + } +} diff --git a/test-runner/src/main/kotlin/org/terst/nav/nmea/AisVdmParser.kt b/test-runner/src/main/kotlin/org/terst/nav/nmea/AisVdmParser.kt new file mode 100644 index 0000000..9709d21 --- /dev/null +++ b/test-runner/src/main/kotlin/org/terst/nav/nmea/AisVdmParser.kt @@ -0,0 +1,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() + } +}
\ No newline at end of file |