diff options
| author | Claudomator Agent <agent@claudomator> | 2026-03-15 12:24:35 +0000 |
|---|---|---|
| committer | Claudomator Agent <agent@claudomator> | 2026-03-15 12:24:35 +0000 |
| commit | d1de605e28bd8ac32d73420ef60235eac4c56a50 (patch) | |
| tree | 13dd3e3ddc26dd6b752521adf5bb049392096d56 | |
| parent | dd5d3cc18653f607fbc0dfe1a32cf60243afef01 (diff) | |
feat: add AIS data model, CPA calculator, and NMEA VDM parser
- AisVessel data class (mmsi, name, callsign, lat, lon, sog, cog, heading, vesselType, timestampMs)
- CpaCalculator: flat-earth CPA/TCPA algorithm (nm, min)
- AisVdmParser: !AIVDM/!AIVDO type 1/2/3 and type 5, multi-part reassembly
- 16 new tests all GREEN; 38 total tests pass in test-runner
- Files under org.terst.nav.ais/nmea (com dir was root-owned)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
13 files changed, 1013 insertions, 9 deletions
diff --git a/SESSION_STATE.md b/SESSION_STATE.md index a5ccf86..88fd79c 100644 --- a/SESSION_STATE.md +++ b/SESSION_STATE.md @@ -1,10 +1,10 @@ # SESSION_STATE.md ## Current Task Goal -GPS navigation implementation: position model, SOG/COG, NMEA RMC parser — COMPLETE +AIS data model, CPA calculator, NMEA VDM parser — COMPLETE (2026-03-15) ## Verified (2026-03-15) -- All 22 GPS/NMEA tests GREEN via test-runner (BUILD SUCCESSFUL) +- All 38 tests GREEN via test-runner (BUILD SUCCESSFUL): 22 GPS/NMEA + 16 AIS - NmeaParser extended with MWV (wind), DBT (depth), HDG/HDM (heading) parsers - Sensor data classes added: WindData, DepthData, HeadingData - NmeaStreamManager added for TCP stream management @@ -67,13 +67,35 @@ GPS navigation implementation: position model, SOG/COG, NMEA RMC parser — COMP - `app/src/test/kotlin/org/terst/nav/gps/GpsProviderTest.kt` (9 tests, pre-existing) - All verified via direct `kotlinc` (1.9.22) + `JUnitCore` invocation +### [APPROVED] AisVessel data class +- File: `app/src/main/kotlin/org/terst/nav/ais/AisVessel.kt` +- Package: `org.terst.nav.ais` +- Fields: mmsi, name, callsign, lat, lon, sog, cog, heading, vesselType, timestampMs +- Note: `com/example/androidapp` tree is root-owned; files placed under `org/terst/nav/` (actual project package) + +### [APPROVED] CpaCalculator object +- File: `app/src/main/kotlin/org/terst/nav/ais/CpaCalculator.kt` +- Flat-earth CPA/TCPA algorithm; returns (cpa_nm, tcpa_min) +- Zero-relative-velocity guard: returns (currentDist, 0.0) + +### [APPROVED] AisVdmParser class +- File: `app/src/main/kotlin/org/terst/nav/nmea/AisVdmParser.kt` +- Parses !AIVDM/!AIVDO sentences; multi-part reassembly by seqId +- Type 1/2/3: MMSI, SOG, lon, lat, COG, heading decoded +- Type 5: MMSI, callsign (7 chars), name (20 chars), vesselType decoded; trailing '@'/' ' trimmed +- Not-available sentinel handling: SOG=1023→0.0, COG=3600→0.0, lon=0x6791AC0→0.0, lat=0x3412140→0.0 + +### [APPROVED] AIS Tests (16 tests — all GREEN) +- `test-runner/src/test/kotlin/org/terst/nav/ais/AisVesselTest.kt` (4 tests) +- `test-runner/src/test/kotlin/org/terst/nav/ais/CpaCalculatorTest.kt` (4 tests) +- `test-runner/src/test/kotlin/org/terst/nav/nmea/AisVdmParserTest.kt` (8 tests) +- Verification harness: `/tmp/ais-test-runner/` (JUnit5, com.example.androidapp package) +- Production test files also in `android-app/app/src/test/kotlin/org/terst/nav/ais/` + ## Next 3 Specific Steps -1. **UI instrument display** — SOG/COG readout widget in `MainActivity`; bind to `GpsProvider` - listener; update TextView/custom view on each `onPositionUpdate` -2. **NmeaGpsProvider** — `GpsProvider` implementation parsing NMEA RMC sentences over TCP/UDP - socket using existing `NmeaParser`; automatic reconnect on disconnect -3. **Fix build permissions** — `chown -R www-data:www-data /workspace/nav/android-app/app/build` - to enable full Gradle unit test runs +1. **AIS chart overlay** — render AisVessel targets on chart; use CpaCalculator for CPA/TCPA alarm +2. **AIS TCP ingestion** — extend NmeaStreamManager to feed !AIVDM sentences to AisVdmParser +3. **UI instrument display** — SOG/COG readout widget in `MainActivity`; bind to `GpsProvider` ## Scripts Added - `test-runner/` — standalone Kotlin/JVM Gradle project; runs all 22 GPS/NMEA tests without Android SDK @@ -81,4 +103,4 @@ GPS navigation implementation: position model, SOG/COG, NMEA RMC parser — COMP ## Process Improvements - Gradle builds blocked by Android SDK requirement; added `test-runner/` JVM-only subproject as reliable test runner -- All 22 tests verified GREEN via `test-runner/` JVM project (2026-03-14) +- All 22 tests verified GREEN via `test-runner/` JVM project (2026-03-14)
\ No newline at end of file diff --git a/android-app/app/src/main/kotlin/org/terst/nav/ais/AisVessel.kt b/android-app/app/src/main/kotlin/org/terst/nav/ais/AisVessel.kt new file mode 100644 index 0000000..34a951c --- /dev/null +++ b/android-app/app/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/android-app/app/src/main/kotlin/org/terst/nav/ais/CpaCalculator.kt b/android-app/app/src/main/kotlin/org/terst/nav/ais/CpaCalculator.kt new file mode 100644 index 0000000..298af59 --- /dev/null +++ b/android-app/app/src/main/kotlin/org/terst/nav/ais/CpaCalculator.kt @@ -0,0 +1,45 @@ +package org.terst.nav.ais + +import kotlin.math.cos +import kotlin.math.sqrt + +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/android-app/app/src/main/kotlin/org/terst/nav/nmea/AisVdmParser.kt b/android-app/app/src/main/kotlin/org/terst/nav/nmea/AisVdmParser.kt new file mode 100644 index 0000000..9709d21 --- /dev/null +++ b/android-app/app/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 diff --git a/android-app/app/src/test/kotlin/org/terst/nav/ais/AisVesselTest.kt b/android-app/app/src/test/kotlin/org/terst/nav/ais/AisVesselTest.kt new file mode 100644 index 0000000..a34a733 --- /dev/null +++ b/android-app/app/src/test/kotlin/org/terst/nav/ais/AisVesselTest.kt @@ -0,0 +1,53 @@ +package org.terst.nav.ais + +import org.junit.jupiter.api.Assertions.* +import org.junit.jupiter.api.Test + +class AisVesselTest { + + @Test + fun `holds all fields correctly`() { + val vessel = AisVessel( + mmsi = 123456789, + name = "MY VESSEL", + callsign = "W1ABC", + lat = 37.5, + lon = -122.0, + sog = 5.5, + cog = 270.0, + heading = 269, + vesselType = 36, + timestampMs = 1000L + ) + assertEquals(123456789, vessel.mmsi) + assertEquals("MY VESSEL", vessel.name) + assertEquals("W1ABC", vessel.callsign) + assertEquals(37.5, vessel.lat) + assertEquals(-122.0, vessel.lon) + assertEquals(5.5, vessel.sog) + assertEquals(270.0, vessel.cog) + assertEquals(269, vessel.heading) + assertEquals(36, vessel.vesselType) + assertEquals(1000L, vessel.timestampMs) + } + + @Test + fun `equality based on all fields`() { + val v1 = AisVessel(1, "A", "B", 0.0, 0.0, 0.0, 0.0, 0, 0, 100L) + val v2 = AisVessel(1, "A", "B", 0.0, 0.0, 0.0, 0.0, 0, 0, 100L) + assertEquals(v1, v2) + } + + @Test + fun `inequality when mmsi differs`() { + val v1 = AisVessel(1, "A", "B", 0.0, 0.0, 0.0, 0.0, 0, 0, 100L) + val v2 = AisVessel(2, "A", "B", 0.0, 0.0, 0.0, 0.0, 0, 0, 100L) + assertNotEquals(v1, v2) + } + + @Test + fun `heading 511 means not available`() { + val vessel = AisVessel(1, "", "", 0.0, 0.0, 0.0, 0.0, 511, 0, 0L) + assertEquals(511, vessel.heading) + } +}
\ No newline at end of file diff --git a/android-app/app/src/test/kotlin/org/terst/nav/ais/CpaCalculatorTest.kt b/android-app/app/src/test/kotlin/org/terst/nav/ais/CpaCalculatorTest.kt new file mode 100644 index 0000000..38069ec --- /dev/null +++ b/android-app/app/src/test/kotlin/org/terst/nav/ais/CpaCalculatorTest.kt @@ -0,0 +1,61 @@ +package org.terst.nav.ais + +import org.junit.jupiter.api.Assertions.* +import org.junit.jupiter.api.Test +import kotlin.math.abs + +class CpaCalculatorTest { + + private val eps = 0.01 // 0.01 nm tolerance for position, 0.01 min for time + + @Test + fun `head-on vessels converging - TCPA around 0_25 min CPA near zero`() { + // Own: (0,0) moving North at 10kt + // Target: (0, 0.0833 nm N ~= 0.0833/60 deg lat) moving South at 10kt + // They meet in the middle ~= 0.25 min + val tgtLat = 0.0833 / 60.0 // 0.0833 nm north in degrees + val (cpa, tcpa) = CpaCalculator.compute( + ownLat = 0.0, ownLon = 0.0, ownSog = 10.0, ownCog = 0.0, + tgtLat = tgtLat, tgtLon = 0.0, tgtSog = 10.0, tgtCog = 180.0 + ) + assertTrue(tcpa > 0.0, "TCPA should be positive (converging): $tcpa") + assertTrue(abs(tcpa - 0.25) < eps, "TCPA should be ~0.25 min, got $tcpa") + assertTrue(cpa < 0.01, "CPA should be near zero for head-on, got $cpa") + } + + @Test + fun `diverging vessels - same direction target ahead - TCPA negative or CPA large`() { + // Own: (0,0) moving North at 5kt + // Target: (0, 1nm N) moving North at 10kt (pulling away) + val tgtLat = 1.0 / 60.0 // 1 nm north + val (cpa, tcpa) = CpaCalculator.compute( + ownLat = 0.0, ownLon = 0.0, ownSog = 5.0, ownCog = 0.0, + tgtLat = tgtLat, tgtLon = 0.0, tgtSog = 10.0, tgtCog = 0.0 + ) + // Target is faster and ahead — diverging, TCPA should be negative + assertTrue(tcpa < 0.0, "TCPA should be negative (diverging), got $tcpa") + } + + @Test + fun `zero relative velocity returns current distance and TCPA zero`() { + // Same speed, same course — relative velocity zero + val tgtLat = 1.0 / 60.0 // 1 nm north + val (cpa, tcpa) = CpaCalculator.compute( + ownLat = 0.0, ownLon = 0.0, ownSog = 5.0, ownCog = 90.0, + tgtLat = tgtLat, tgtLon = 0.0, tgtSog = 5.0, tgtCog = 90.0 + ) + assertEquals(0.0, tcpa, 1e-9) + // Current distance should be ~1 nm + assertTrue(abs(cpa - 1.0) < 0.01, "CPA should be ~1 nm (current dist), got $cpa") + } + + @Test + fun `both at same position zero relative velocity - zero distance`() { + val (cpa, tcpa) = CpaCalculator.compute( + ownLat = 37.0, ownLon = -122.0, ownSog = 5.0, ownCog = 0.0, + tgtLat = 37.0, tgtLon = -122.0, tgtSog = 5.0, tgtCog = 0.0 + ) + assertEquals(0.0, tcpa, 1e-9) + assertEquals(0.0, cpa, 1e-6) + } +}
\ No newline at end of file diff --git a/android-app/app/src/test/kotlin/org/terst/nav/nmea/AisVdmParserTest.kt b/android-app/app/src/test/kotlin/org/terst/nav/nmea/AisVdmParserTest.kt new file mode 100644 index 0000000..da3efd3 --- /dev/null +++ b/android-app/app/src/test/kotlin/org/terst/nav/nmea/AisVdmParserTest.kt @@ -0,0 +1,205 @@ +package org.terst.nav.nmea + +import org.junit.jupiter.api.Assertions.* +import org.junit.jupiter.api.Test +import kotlin.math.abs + +/** + * Encoding helpers to build synthetic AIS payloads for round-trip testing. + * Mirrors the decoder logic exactly. + */ +private fun encodeBits(bits: IntArray): String { + // Pad to multiple of 6 + val padded = if (bits.size % 6 == 0) bits else bits + IntArray(6 - bits.size % 6) + val sb = StringBuilder() + var i = 0 + while (i < padded.size) { + var v = 0 + for (b in 0 until 6) v = (v shl 1) or padded[i + b] + val c = if (v <= 39) v + 48 else v + 56 + sb.append(c.toChar()) + i += 6 + } + return sb.toString() +} + +private fun intToBits(value: Int, len: Int): IntArray { + val arr = IntArray(len) + for (i in 0 until len) arr[i] = (value ushr (len - 1 - i)) and 1 + return arr +} + +private fun signedToBits(value: Int, len: Int): IntArray { + // Two's complement + val mask = if (len < 32) (1 shl len) - 1 else -1 + return intToBits(value and mask, len) +} + +/** Build a type-1 message bit array (168 bits) */ +private fun buildType1Payload( + mmsi: Int, sog10: Int, lon600000: Int, lat600000: Int, cog10: Int, heading: Int +): String { + val bits = IntArray(168) + fun set(start: Int, arr: IntArray) { arr.copyInto(bits, start) } + set(0, intToBits(1, 6)) // message type = 1 + set(6, intToBits(0, 2)) // repeat + set(8, intToBits(mmsi, 30)) // MMSI + set(38, intToBits(0, 12)) // nav status + rot (12 bits filler) + set(50, intToBits(sog10, 10)) // SOG * 10 + set(60, intToBits(0, 1)) // accuracy + set(61, signedToBits(lon600000, 28)) // lon * 600000 + set(89, signedToBits(lat600000, 27)) // lat * 600000 + set(116, intToBits(cog10, 12)) // COG * 10 + set(128, intToBits(heading, 9)) // heading + // remaining bits 137-167 = 0 (timestamp, maneuver, spare, raim, radio) + return encodeBits(bits) +} + +/** Build a type-5 message bit array (426 bits) */ +private fun buildType5Payload(mmsi: Int, callsign: String, name: String, vesselType: Int): String { + val bits = IntArray(426) + fun set(start: Int, arr: IntArray) { arr.copyInto(bits, start) } + set(0, intToBits(5, 6)) // message type = 5 + set(6, intToBits(0, 2)) // repeat + set(8, intToBits(mmsi, 30)) // MMSI + set(38, intToBits(0, 2)) // ais version + set(40, intToBits(0, 30)) // IMO number + // AIS 6-bit text encode: chars >=64 (A-Z,@) -> c-64 (gives 0-31); else c (space=32, digits=48-57) + val csFixed = callsign.padEnd(7, '@').take(7) + for (i in 0 until 7) { + val c = csFixed[i].code + val v = if (c >= 64) c - 64 else c + set(70 + i * 6, intToBits(v, 6)) + } + // name: bits 112-231 = 20 chars x 6 bits + val nameFixed = name.padEnd(20, '@').take(20) + for (i in 0 until 20) { + val c = nameFixed[i].code + val v = if (c >= 64) c - 64 else c + set(112 + i * 6, intToBits(v, 6)) + } + // vessel type: bits 232-239 + set(232, intToBits(vesselType, 8)) + // rest = 0 (draught, destination, dte, spare) + return encodeBits(bits) +} + +private fun nmea0183Checksum(body: String): String { + var xor = 0 + for (c in body) xor = xor xor c.code + return xor.toString(16).uppercase().padStart(2, '0') +} + +private fun makeVdm(payload: String, padding: Int = 0): String { + val body = "AIVDM,1,1,,A,$payload,$padding" + return "!$body*${nmea0183Checksum(body)}" +} + +private fun makeVdmPart(count: Int, seq: Int, seqId: String, payload: String, padding: Int = 0): String { + val body = "AIVDM,$count,$seq,$seqId,A,$payload,$padding" + return "!$body*${nmea0183Checksum(body)}" +} + +class AisVdmParserTest { + + private val parser = AisVdmParser() + + @Test + fun `non-AIS NMEA sentence returns null`() { + assertNull(parser.parse("\$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A")) + } + + @Test + fun `malformed sentence returns null`() { + assertNull(parser.parse("garbage")) + assertNull(parser.parse("!AIVDM,1,1,,A,")) + assertNull(parser.parse("")) + } + + @Test + fun `type-1 round-trip - known MMSI lat lon sog cog heading`() { + // MMSI=123456789, sog=5.0kt (50), lon=-122.0deg, lat=37.0deg, cog=270.0 (2700), heading=270 + val mmsi = 123456789 + val sog10 = 50 // 5.0 knots + val lon600000 = (-122.0 * 600000.0).toInt() // -73200000 + val lat600000 = (37.0 * 600000.0).toInt() // 22200000 + val cog10 = 2700 // 270.0 degrees + val heading = 270 + + val payload = buildType1Payload(mmsi, sog10, lon600000, lat600000, cog10, heading) + val sentence = makeVdm(payload) + val vessel = parser.parse(sentence) + + assertNotNull(vessel) + assertEquals(mmsi, vessel!!.mmsi) + assertEquals(5.0, vessel.sog, 0.01) + assertEquals(-122.0, vessel.lon, 0.001) + assertEquals(37.0, vessel.lat, 0.001) + assertEquals(270.0, vessel.cog, 0.1) + assertEquals(270, vessel.heading) + assertEquals("", vessel.name) + assertEquals("", vessel.callsign) + assertEquals(0, vessel.vesselType) + } + + @Test + fun `type-1 real sentence - MMSI 227006760`() { + val sentence = "!AIVDM,1,1,,A,13HOI:0P0000vocH;`5HF>0<0000,0*54" + val vessel = parser.parse(sentence) + assertNotNull(vessel) + assertEquals(227006760, vessel!!.mmsi) + } + + @Test + fun `type-5 round-trip - name and callsign decoded and trimmed`() { + val mmsi = 987654321 + val payload = buildType5Payload(mmsi, "W1ABC", "MY VESSEL NAME", vesselType = 36) + val sentence = makeVdm(payload) + val vessel = parser.parse(sentence) + + assertNotNull(vessel) + assertEquals(mmsi, vessel!!.mmsi) + assertEquals("W1ABC", vessel.callsign) + assertEquals("MY VESSEL NAME", vessel.name) + assertEquals(36, vessel.vesselType) + assertEquals(0.0, vessel.lat) + assertEquals(0.0, vessel.lon) + assertEquals(511, vessel.heading) + } + + @Test + fun `two-part message reassembly`() { + // Build a type-1 payload, split it in half across two sentences + val mmsi = 111222333 + val payload = buildType1Payload(mmsi, 100, 0, 0, 900, 511) + val half = payload.length / 2 + val part1 = payload.substring(0, half) + val part2 = payload.substring(half) + + val s1 = makeVdmPart(2, 1, "1", part1, 0) + val s2 = makeVdmPart(2, 2, "1", part2, 0) + + // First sentence: incomplete, returns null + assertNull(parser.parse(s1)) + // Second sentence: completes reassembly + val vessel = parser.parse(s2) + assertNotNull(vessel) + assertEquals(mmsi, vessel!!.mmsi) + } + + @Test + fun `SOG not available value 1023 decodes to 0_0`() { + val payload = buildType1Payload(111000111, 1023, 0, 0, 0, 511) + val vessel = parser.parse(makeVdm(payload)) + assertNotNull(vessel) + assertEquals(0.0, vessel!!.sog) + } + + @Test + fun `COG not available value 3600 decodes to 0_0`() { + val payload = buildType1Payload(111000222, 0, 0, 0, 3600, 511) + val vessel = parser.parse(makeVdm(payload)) + assertNotNull(vessel) + assertEquals(0.0, vessel!!.cog) + } +}
\ No newline at end of file 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 diff --git a/test-runner/src/test/kotlin/org/terst/nav/ais/AisVesselTest.kt b/test-runner/src/test/kotlin/org/terst/nav/ais/AisVesselTest.kt new file mode 100644 index 0000000..a583a32 --- /dev/null +++ b/test-runner/src/test/kotlin/org/terst/nav/ais/AisVesselTest.kt @@ -0,0 +1,53 @@ +package org.terst.nav.ais + +import org.junit.Assert.* +import org.junit.Test + +class AisVesselTest { + + @Test + fun `holds all fields correctly`() { + val vessel = AisVessel( + mmsi = 123456789, + name = "MY VESSEL", + callsign = "W1ABC", + lat = 37.5, + lon = -122.0, + sog = 5.5, + cog = 270.0, + heading = 269, + vesselType = 36, + timestampMs = 1000L + ) + assertEquals(123456789, vessel.mmsi) + assertEquals("MY VESSEL", vessel.name) + assertEquals("W1ABC", vessel.callsign) + assertEquals(37.5, vessel.lat, 0.0) + assertEquals(-122.0, vessel.lon, 0.0) + assertEquals(5.5, vessel.sog, 0.0) + assertEquals(270.0, vessel.cog, 0.0) + assertEquals(269, vessel.heading) + assertEquals(36, vessel.vesselType) + assertEquals(1000L, vessel.timestampMs) + } + + @Test + fun `equality based on all fields`() { + val v1 = AisVessel(1, "A", "B", 0.0, 0.0, 0.0, 0.0, 0, 0, 100L) + val v2 = AisVessel(1, "A", "B", 0.0, 0.0, 0.0, 0.0, 0, 0, 100L) + assertEquals(v1, v2) + } + + @Test + fun `inequality when mmsi differs`() { + val v1 = AisVessel(1, "A", "B", 0.0, 0.0, 0.0, 0.0, 0, 0, 100L) + val v2 = AisVessel(2, "A", "B", 0.0, 0.0, 0.0, 0.0, 0, 0, 100L) + assertNotEquals(v1, v2) + } + + @Test + fun `heading 511 means not available`() { + val vessel = AisVessel(1, "", "", 0.0, 0.0, 0.0, 0.0, 511, 0, 0L) + assertEquals(511, vessel.heading) + } +}
\ No newline at end of file diff --git a/test-runner/src/test/kotlin/org/terst/nav/ais/CpaCalculatorTest.kt b/test-runner/src/test/kotlin/org/terst/nav/ais/CpaCalculatorTest.kt new file mode 100644 index 0000000..9baddb2 --- /dev/null +++ b/test-runner/src/test/kotlin/org/terst/nav/ais/CpaCalculatorTest.kt @@ -0,0 +1,53 @@ +package org.terst.nav.ais + +import org.junit.Assert.* +import org.junit.Test +import kotlin.math.abs + +class CpaCalculatorTest { + + private val eps = 0.01 + + @Test + fun `head-on vessels converging - TCPA around 0_25 min CPA near zero`() { + val tgtLat = 0.0833 / 60.0 + val (cpa, tcpa) = CpaCalculator.compute( + ownLat = 0.0, ownLon = 0.0, ownSog = 10.0, ownCog = 0.0, + tgtLat = tgtLat, tgtLon = 0.0, tgtSog = 10.0, tgtCog = 180.0 + ) + assertTrue("TCPA should be positive (converging): $tcpa", tcpa > 0.0) + assertTrue("TCPA should be ~0.25 min, got $tcpa", abs(tcpa - 0.25) < eps) + assertTrue("CPA should be near zero for head-on, got $cpa", cpa < 0.01) + } + + @Test + fun `diverging vessels - same direction target ahead - TCPA negative`() { + val tgtLat = 1.0 / 60.0 + val (_, tcpa) = CpaCalculator.compute( + ownLat = 0.0, ownLon = 0.0, ownSog = 5.0, ownCog = 0.0, + tgtLat = tgtLat, tgtLon = 0.0, tgtSog = 10.0, tgtCog = 0.0 + ) + assertTrue("TCPA should be negative (diverging), got $tcpa", tcpa < 0.0) + } + + @Test + fun `zero relative velocity returns current distance and TCPA zero`() { + val tgtLat = 1.0 / 60.0 + val (cpa, tcpa) = CpaCalculator.compute( + ownLat = 0.0, ownLon = 0.0, ownSog = 5.0, ownCog = 90.0, + tgtLat = tgtLat, tgtLon = 0.0, tgtSog = 5.0, tgtCog = 90.0 + ) + assertEquals(0.0, tcpa, 1e-9) + assertTrue("CPA should be ~1 nm (current dist), got $cpa", abs(cpa - 1.0) < 0.01) + } + + @Test + fun `both at same position zero relative velocity - zero distance`() { + val (cpa, tcpa) = CpaCalculator.compute( + ownLat = 37.0, ownLon = -122.0, ownSog = 5.0, ownCog = 0.0, + tgtLat = 37.0, tgtLon = -122.0, tgtSog = 5.0, tgtCog = 0.0 + ) + assertEquals(0.0, tcpa, 1e-9) + assertEquals(0.0, cpa, 1e-6) + } +} diff --git a/test-runner/src/test/kotlin/org/terst/nav/nmea/AisVdmParserTest.kt b/test-runner/src/test/kotlin/org/terst/nav/nmea/AisVdmParserTest.kt new file mode 100644 index 0000000..481c89e --- /dev/null +++ b/test-runner/src/test/kotlin/org/terst/nav/nmea/AisVdmParserTest.kt @@ -0,0 +1,180 @@ +package org.terst.nav.nmea + +import org.junit.Assert.* +import org.junit.Test +import kotlin.math.abs + +private fun encodeBits(bits: IntArray): String { + val padded = if (bits.size % 6 == 0) bits else bits + IntArray(6 - bits.size % 6) + val sb = StringBuilder() + var i = 0 + while (i < padded.size) { + var v = 0 + for (b in 0 until 6) v = (v shl 1) or padded[i + b] + val c = if (v <= 39) v + 48 else v + 56 + sb.append(c.toChar()) + i += 6 + } + return sb.toString() +} + +private fun intToBits(value: Int, len: Int): IntArray { + val arr = IntArray(len) + for (i in 0 until len) arr[i] = (value ushr (len - 1 - i)) and 1 + return arr +} + +private fun signedToBits(value: Int, len: Int): IntArray { + val mask = if (len < 32) (1 shl len) - 1 else -1 + return intToBits(value and mask, len) +} + +private fun buildType1Payload( + mmsi: Int, sog10: Int, lon600000: Int, lat600000: Int, cog10: Int, heading: Int +): String { + val bits = IntArray(168) + fun set(start: Int, arr: IntArray) { arr.copyInto(bits, start) } + set(0, intToBits(1, 6)) + set(6, intToBits(0, 2)) + set(8, intToBits(mmsi, 30)) + set(38, intToBits(0, 12)) + set(50, intToBits(sog10, 10)) + set(60, intToBits(0, 1)) + set(61, signedToBits(lon600000, 28)) + set(89, signedToBits(lat600000, 27)) + set(116, intToBits(cog10, 12)) + set(128, intToBits(heading, 9)) + return encodeBits(bits) +} + +private fun buildType5Payload(mmsi: Int, callsign: String, name: String, vesselType: Int): String { + val bits = IntArray(426) + fun set(start: Int, arr: IntArray) { arr.copyInto(bits, start) } + set(0, intToBits(5, 6)) + set(6, intToBits(0, 2)) + set(8, intToBits(mmsi, 30)) + set(38, intToBits(0, 2)) + set(40, intToBits(0, 30)) + val csFixed = callsign.padEnd(7, '@').take(7) + for (i in 0 until 7) { + val c = csFixed[i].code + val v = if (c >= 64) c - 64 else c + set(70 + i * 6, intToBits(v, 6)) + } + val nameFixed = name.padEnd(20, '@').take(20) + for (i in 0 until 20) { + val c = nameFixed[i].code + val v = if (c >= 64) c - 64 else c + set(112 + i * 6, intToBits(v, 6)) + } + set(232, intToBits(vesselType, 8)) + return encodeBits(bits) +} + +private fun nmea0183Checksum(body: String): String { + var xor = 0 + for (c in body) xor = xor xor c.code + return xor.toString(16).uppercase().padStart(2, '0') +} + +private fun makeVdm(payload: String, padding: Int = 0): String { + val body = "AIVDM,1,1,,A,$payload,$padding" + return "!$body*${nmea0183Checksum(body)}" +} + +private fun makeVdmPart(count: Int, seq: Int, seqId: String, payload: String, padding: Int = 0): String { + val body = "AIVDM,$count,$seq,$seqId,A,$payload,$padding" + return "!$body*${nmea0183Checksum(body)}" +} + +class AisVdmParserTest { + + private val parser = AisVdmParser() + + @Test + fun `non-AIS NMEA sentence returns null`() { + assertNull(parser.parse("\$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A")) + } + + @Test + fun `malformed sentence returns null`() { + assertNull(parser.parse("garbage")) + assertNull(parser.parse("!AIVDM,1,1,,A,")) + assertNull(parser.parse("")) + } + + @Test + fun `type-1 round-trip - known MMSI lat lon sog cog heading`() { + val mmsi = 123456789 + val sog10 = 50 + val lon600000 = (-122.0 * 600000.0).toInt() + val lat600000 = (37.0 * 600000.0).toInt() + val cog10 = 2700 + val heading = 270 + + val payload = buildType1Payload(mmsi, sog10, lon600000, lat600000, cog10, heading) + val vessel = parser.parse(makeVdm(payload)) + + assertNotNull(vessel) + assertEquals(mmsi, vessel!!.mmsi) + assertEquals(5.0, vessel.sog, 0.01) + assertEquals(-122.0, vessel.lon, 0.001) + assertEquals(37.0, vessel.lat, 0.001) + assertEquals(270.0, vessel.cog, 0.1) + assertEquals(270, vessel.heading) + assertEquals("", vessel.name) + assertEquals("", vessel.callsign) + assertEquals(0, vessel.vesselType) + } + + @Test + fun `type-1 real sentence - MMSI 227006760`() { + val vessel = parser.parse("!AIVDM,1,1,,A,13HOI:0P0000vocH;`5HF>0<0000,0*54") + assertNotNull(vessel) + assertEquals(227006760, vessel!!.mmsi) + } + + @Test + fun `type-5 round-trip - name and callsign decoded and trimmed`() { + val mmsi = 987654321 + val payload = buildType5Payload(mmsi, "W1ABC", "MY VESSEL NAME", vesselType = 36) + val vessel = parser.parse(makeVdm(payload)) + + assertNotNull(vessel) + assertEquals(mmsi, vessel!!.mmsi) + assertEquals("W1ABC", vessel.callsign) + assertEquals("MY VESSEL NAME", vessel.name) + assertEquals(36, vessel.vesselType) + assertEquals(0.0, vessel.lat, 0.0) + assertEquals(0.0, vessel.lon, 0.0) + assertEquals(511, vessel.heading) + } + + @Test + fun `two-part message reassembly`() { + val mmsi = 111222333 + val payload = buildType1Payload(mmsi, 100, 0, 0, 900, 511) + val half = payload.length / 2 + val s1 = makeVdmPart(2, 1, "1", payload.substring(0, half), 0) + val s2 = makeVdmPart(2, 2, "1", payload.substring(half), 0) + + assertNull(parser.parse(s1)) + val vessel = parser.parse(s2) + assertNotNull(vessel) + assertEquals(mmsi, vessel!!.mmsi) + } + + @Test + fun `SOG not available value 1023 decodes to 0_0`() { + val vessel = parser.parse(makeVdm(buildType1Payload(111000111, 1023, 0, 0, 0, 511))) + assertNotNull(vessel) + assertEquals(0.0, vessel!!.sog, 0.0) + } + + @Test + fun `COG not available value 3600 decodes to 0_0`() { + val vessel = parser.parse(makeVdm(buildType1Payload(111000222, 0, 0, 0, 3600, 511))) + assertNotNull(vessel) + assertEquals(0.0, vessel!!.cog, 0.0) + } +}
\ No newline at end of file |
