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
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
|
package com.example.androidapp.gps
import com.example.androidapp.data.model.SensorData
import com.example.androidapp.wind.TrueWindCalculator
import com.example.androidapp.wind.ApparentWind
import com.example.androidapp.wind.TrueWindData
import kotlinx.coroutines.flow.MutableStateFlow
import kotlinx.coroutines.flow.StateFlow
import kotlinx.coroutines.flow.asStateFlow
/** Source of the currently active GPS fix. */
enum class GpsSource { NONE, NMEA, ANDROID }
/**
* Aggregates real-time location and environmental sensor data for use throughout
* the safety subsystem (Section 4.6 of COMPONENT_DESIGN.md).
*
* ## GPS sensor fusion
* The service accepts fixes from two independent sources:
* - **NMEA GPS** — dedicated marine GPS received via [updateNmeaGps] (higher priority)
* - **Android GPS** — device built-in location via [updateAndroidGps] (fallback)
*
* Selection policy (evaluated on every new fix):
* 1. Prefer NMEA when its most recent fix is no older than [nmeaStalenessThresholdMs].
* 2. When NMEA is marginally stale (older than [nmeaStalenessThresholdMs] but within
* [nmeaExtendedThresholdMs]) **and** Android GPS is also available, compare
* [GpsPosition.accuracyMeters]: keep NMEA if its reported accuracy is strictly better
* (lower metres). Fall back to Android when accuracy is unavailable or Android wins.
* 3. Fall back to Android GPS when NMEA is very stale (beyond [nmeaExtendedThresholdMs]).
* 4. Use stale NMEA only when Android GPS has never provided a fix.
* 5. [bestPosition] is null until at least one source has reported.
*
* Call [updateSensorData] whenever new NMEA or Signal K sensor data arrives and
* [updateCurrentConditions] when a fresh marine-forecast response is received.
* Use [snapshot] to capture a point-in-time reading at safety-critical moments
* such as MOB activation.
*
* @param nmeaStalenessThresholdMs Maximum age (ms) of an NMEA fix before it enters the
* quality-comparison zone. Default: 5 000 ms.
* @param nmeaExtendedThresholdMs Maximum age (ms) up to which a marginally-stale NMEA fix
* can still win over Android if its [GpsPosition.accuracyMeters] is strictly better.
* Must be ≥ [nmeaStalenessThresholdMs]. Default: 10 000 ms.
* @param clockMs Injectable clock for unit-testable staleness checks.
*/
class LocationService(
private val windCalculator: TrueWindCalculator = TrueWindCalculator(),
private val nmeaStalenessThresholdMs: Long = 5_000L,
private val nmeaExtendedThresholdMs: Long = 10_000L,
private val clockMs: () -> Long = System::currentTimeMillis
) {
private val _latestSensor = MutableStateFlow<SensorData?>(null)
/** The most recently received unified sensor reading. */
val latestSensor: StateFlow<SensorData?> = _latestSensor.asStateFlow()
private val _latestTrueWind = MutableStateFlow<TrueWindData?>(null)
/** Most recent resolved true-wind vector, updated whenever a full sensor reading arrives. */
val latestTrueWind: StateFlow<TrueWindData?> = _latestTrueWind.asStateFlow()
private val _currentSpeedKt = MutableStateFlow<Double?>(null)
private val _currentDirectionDeg = MutableStateFlow<Double?>(null)
// ── GPS sensor fusion state ───────────────────────────────────────────────
private var lastNmeaPosition: GpsPosition? = null
private var lastAndroidPosition: GpsPosition? = null
private val _bestPosition = MutableStateFlow<GpsPosition?>(null)
/**
* The best available GPS fix, selected from NMEA and Android sources according
* to the fusion policy described in the class KDoc. Null until at least one
* source reports a fix.
*/
val bestPosition: StateFlow<GpsPosition?> = _bestPosition.asStateFlow()
private val _activeGpsSource = MutableStateFlow(GpsSource.NONE)
/** The source that produced [bestPosition]. [GpsSource.NONE] before any fix arrives. */
val activeGpsSource: StateFlow<GpsSource> = _activeGpsSource.asStateFlow()
/**
* Ingest a new sensor reading. If the reading carries apparent wind, boat speed,
* and heading, true wind is resolved immediately via [TrueWindCalculator] and
* stored in [latestTrueWind].
*/
fun updateSensorData(data: SensorData) {
_latestSensor.value = data
val aws = data.apparentWindSpeedKt
val awa = data.apparentWindAngleDeg
val bsp = data.speedOverGroundKt // use SOG as proxy when BSP is absent
val hdg = data.headingTrueDeg
if (aws != null && awa != null && bsp != null && hdg != null) {
_latestTrueWind.value = windCalculator.update(
apparent = ApparentWind(speedKt = aws, angleDeg = awa),
bsp = bsp,
hdgDeg = hdg
)
}
}
// ── GPS source ingestion ──────────────────────────────────────────────────
/**
* Ingest a new GPS fix from the NMEA source (e.g. a marine chartplotter or
* NMEA multiplexer). Triggers a fusion re-evaluation.
*/
fun updateNmeaGps(position: GpsPosition) {
lastNmeaPosition = position
recomputeBestPosition()
}
/**
* Ingest a new GPS fix from the Android system location provider.
* Triggers a fusion re-evaluation.
*/
fun updateAndroidGps(position: GpsPosition) {
lastAndroidPosition = position
recomputeBestPosition()
}
/**
* Selects the best GPS fix and updates [bestPosition] / [activeGpsSource].
*
* Priority tiers (in order):
* 1. Fresh NMEA (age ≤ [nmeaStalenessThresholdMs]) — always preferred.
* 2. Marginally-stale NMEA (age in (primary, extended] threshold) when Android is
* also available — keep NMEA only if its [GpsPosition.accuracyMeters] is strictly
* better than Android's; otherwise use Android.
* 3. Android GPS (any age) once NMEA is beyond the extended threshold.
* 4. Stale NMEA — used as last resort when Android has never reported.
*/
private fun recomputeBestPosition() {
val now = clockMs()
val nmea = lastNmeaPosition
val android = lastAndroidPosition
val nmeaAge = nmea?.let { now - it.timestampMs }
val nmeaFresh = nmeaAge != null && nmeaAge <= nmeaStalenessThresholdMs
val nmeaMarginallyStale = nmeaAge != null &&
nmeaAge > nmeaStalenessThresholdMs &&
nmeaAge <= nmeaExtendedThresholdMs
val (best, source) = when {
nmeaFresh -> nmea!! to GpsSource.NMEA
nmeaMarginallyStale && android != null ->
// Quality tie-break: NMEA wins only when it has a strictly better accuracy.
if (nmea!!.hasStrictlyBetterAccuracyThan(android)) nmea to GpsSource.NMEA
else android to GpsSource.ANDROID
android != null -> android to GpsSource.ANDROID
nmea != null -> nmea to GpsSource.NMEA // only source, however stale
else -> null to GpsSource.NONE
}
_bestPosition.value = best
_activeGpsSource.value = source
}
// ── private helpers ───────────────────────────────────────────────────────
/**
* Returns true when this fix carries an accuracy estimate that is numerically
* smaller (i.e. better) than [other]'s. Returns false when either estimate is
* absent — conservatively preferring the other source when quality is unknown.
*/
private fun GpsPosition.hasStrictlyBetterAccuracyThan(other: GpsPosition): Boolean {
val thisAccuracy = accuracyMeters ?: return false
val otherAccuracy = other.accuracyMeters ?: return true
return thisAccuracy < otherAccuracy
}
/**
* Update the ocean current conditions from the latest marine-forecast response.
*
* @param speedKt Current speed in knots (null to clear)
* @param directionDeg Direction the current flows TOWARD, in degrees (null to clear)
*/
fun updateCurrentConditions(speedKt: Double?, directionDeg: Double?) {
_currentSpeedKt.value = speedKt
_currentDirectionDeg.value = directionDeg
}
/**
* Captures a snapshot of wind and current conditions at the current moment.
*
* All fields are nullable — only data that was available at snapshot time is
* populated. This snapshot is intended to be logged alongside a [MobEvent]
* at the instant of MOB activation.
*/
fun snapshot(): EnvironmentalSnapshot {
val trueWind = _latestTrueWind.value
return EnvironmentalSnapshot(
windSpeedKt = trueWind?.speedKt,
windDirectionDeg = trueWind?.directionDeg,
currentSpeedKt = _currentSpeedKt.value,
currentDirectionDeg = _currentDirectionDeg.value
)
}
}
/**
* Point-in-time snapshot of wind and current conditions.
*
* @param windSpeedKt True Wind Speed in knots; null if sensors were unavailable.
* @param windDirectionDeg True Wind Direction (degrees true, wind comes FROM); null if unavailable.
* @param currentSpeedKt Ocean current speed in knots; null if forecast was unavailable.
* @param currentDirectionDeg Ocean current direction (degrees, flows TOWARD); null if unavailable.
*/
data class EnvironmentalSnapshot(
val windSpeedKt: Double?,
val windDirectionDeg: Double?,
val currentSpeedKt: Double?,
val currentDirectionDeg: Double?
)
|
