128 lines
4.3 KiB
Dart
128 lines
4.3 KiB
Dart
import 'dart:async';
|
|
|
|
import '../flight_controller_link.dart';
|
|
import 'msp_client.dart';
|
|
import 'msp_commands.dart';
|
|
import 'msp_telemetry_codec.dart';
|
|
|
|
/// Eigener Poll-Takt ueber [MspClient] (Doku Kommunikationsschicht Abschnitt
|
|
/// 4, "Polling"): MSP liefert nichts von selbst. Rundenbasiert statt
|
|
/// paralleler Timer pro Nachrichtengruppe, weil [MspClient] Anfragen
|
|
/// ohnehin serialisiert (immer nur eine offene Anfrage) - ein
|
|
/// Round-Robin-Zyklus bildet die in der Doku genannten Raten ab, ohne dass
|
|
/// sich Anfragen ueberlappen koennten:
|
|
///
|
|
/// - Position/Lage/Hoehe (5-10 Hz): jeden Zyklus (`MSP_RAW_GPS` +
|
|
/// `MSP_ALTITUDE`).
|
|
/// - Status/Flugmodus/Akku/Link (2 Hz): jeden [_statusEveryNCycles]-ten
|
|
/// Zyklus (`MSP2_INAV_STATUS`, `MSP_NAV_STATUS`, `MSP2_INAV_ANALOG`,
|
|
/// `MSP2_INAV_GET_LINK_STATS`) - Flugmodus wird aus derselben
|
|
/// `MSP_NAV_STATUS`-Antwort gelesen wie der aktive Wegpunkt, keine
|
|
/// zusaetzliche Anfrage noetig.
|
|
///
|
|
/// Der Strom wird NICHT auf einen gueltigen GPS-Fix gegated - jeder Zyklus
|
|
/// emittiert ein [TelemetryFrame], auch ohne Fix (dann mit `hasFix: false`
|
|
/// und der vom FC gemeldeten, ggf. bedeutungslosen Position). Konsumenten
|
|
/// wie FlyScreen entscheiden selbst anhand von [TelemetryFrame.hasFix], ob
|
|
/// sie die Position anzeigen.
|
|
class MspTelemetryPoller {
|
|
MspTelemetryPoller(this._client);
|
|
|
|
final MspClient _client;
|
|
final _framesController = StreamController<TelemetryFrame>.broadcast();
|
|
|
|
static const _cycleInterval = Duration(milliseconds: 150); // ~6-7 Hz
|
|
static const _statusEveryNCycles = 3; // ~2 Hz bei 150-ms-Takt
|
|
|
|
bool _running = false;
|
|
int _cycle = 0;
|
|
bool _armed = false;
|
|
int? _activeWaypointIndex;
|
|
int _batteryPercent = 0;
|
|
double _batteryVoltage = 0;
|
|
double _currentA = 0;
|
|
int _linkQuality = 0;
|
|
int _snrDb = 0;
|
|
int _navMode = 0;
|
|
String _flightMode = 'Idle';
|
|
|
|
Stream<TelemetryFrame> get frames => _framesController.stream;
|
|
|
|
void start() {
|
|
if (_running) return;
|
|
_running = true;
|
|
unawaited(_loop());
|
|
}
|
|
|
|
void stop() {
|
|
_running = false;
|
|
}
|
|
|
|
Future<void> _loop() async {
|
|
while (_running) {
|
|
try {
|
|
await _runCycle();
|
|
} catch (_) {
|
|
// Eine einzelne fehlgeschlagene Anfrage (Timeout/CRC/Transport
|
|
// getrennt) darf den Takt nicht stoppen - der naechste Zyklus
|
|
// startet nach der Wartezeit regulaer weiter (Doku 4: Zeitgrenze +
|
|
// Wiederholung; Wiederverbinden passiert bereits auf Transport-
|
|
// Ebene, siehe BluetoothClassicTransport).
|
|
}
|
|
await Future<void>.delayed(_cycleInterval);
|
|
}
|
|
}
|
|
|
|
Future<void> _runCycle() async {
|
|
final gps = parseMspRawGps(await _client.request(MspCommands.rawGps));
|
|
final altitudeM =
|
|
parseMspAltitudeMeters(await _client.request(MspCommands.altitude));
|
|
|
|
_cycle++;
|
|
if (_cycle % _statusEveryNCycles == 0) {
|
|
_armed = parseMspInavStatusArmed(
|
|
await _client.request(MspCommands.inavStatus),
|
|
);
|
|
final navStatusPayload = await _client.request(MspCommands.navStatus);
|
|
_activeWaypointIndex = parseMspNavStatusActiveWaypoint(navStatusPayload);
|
|
final navMode = parseMspNavMode(navStatusPayload);
|
|
_navMode = navMode.mode;
|
|
_flightMode = navMode.formatted;
|
|
final analogPayload = await _client.request(MspCommands.inavAnalog);
|
|
_batteryPercent = parseMspInavAnalogBatteryPercent(analogPayload);
|
|
_batteryVoltage = parseMspInavAnalogVoltage(analogPayload);
|
|
_currentA = parseMspInavAnalogAmperage(analogPayload);
|
|
final linkStatsPayload = await _client.request(MspCommands.inavLinkStats);
|
|
_linkQuality = parseMspLinkQuality(linkStatsPayload);
|
|
_snrDb = parseMspLinkStatsSnr(linkStatsPayload);
|
|
}
|
|
|
|
if (!_framesController.isClosed) {
|
|
_framesController.add(TelemetryFrame(
|
|
lat: gps.lat,
|
|
lon: gps.lon,
|
|
hasFix: gps.hasFix,
|
|
numSat: gps.numSat,
|
|
altitudeM: altitudeM,
|
|
speedMs: gps.speedMs,
|
|
headingDeg: gps.headingDeg,
|
|
armed: _armed,
|
|
batteryPercent: _batteryPercent,
|
|
batteryVoltage: _batteryVoltage,
|
|
currentA: _currentA,
|
|
linkQuality: _linkQuality,
|
|
snrDb: _snrDb,
|
|
hdop: gps.hdop,
|
|
navMode: _navMode,
|
|
flightMode: _flightMode,
|
|
activeWaypointIndex: _activeWaypointIndex,
|
|
));
|
|
}
|
|
}
|
|
|
|
Future<void> dispose() async {
|
|
stop();
|
|
await _framesController.close();
|
|
}
|
|
}
|