add telemetry messages: rx link quality, battery percentage, flight mode, relative altitude

This commit is contained in:
Constantin Leue
2026-08-02 21:09:33 +02:00
parent 11f894919b
commit 35c4129eb7
9 changed files with 186 additions and 11 deletions
@@ -34,6 +34,9 @@ class TelemetryFrame {
required this.speedMs,
required this.headingDeg,
required this.armed,
required this.batteryPercent,
required this.linkQuality,
required this.flightMode,
this.activeWaypointIndex,
});
@@ -43,6 +46,11 @@ class TelemetryFrame {
final double lon;
final bool hasFix;
final int numSat;
/// Barometrisch/GPS-fusionierte Hoehe relativ zum Referenzpunkt der
/// Positionsschaetzung (in der Praxis: Einschaltort, siehe
/// MspNavMode-/estAlt-Doku in msp_telemetry_codec.dart) - keine absolute
/// Hoehe ueber Meeresspiegel.
final double altitudeM;
final double speedMs;
@@ -50,6 +58,17 @@ class TelemetryFrame {
/// Drohnen-Marker (Doku 3.10, [DroneMarkerIcon]).
final double headingDeg;
final bool armed;
/// Akku-Ladestand in Prozent (0-100).
final int batteryPercent;
/// Empfaenger-Linkqualitaet in Prozent (0-100).
final int linkQuality;
/// Menschenlesbare Kurzbeschreibung des Navigationsmodus (z.B. "Waypoint
/// mission (en route)", "Return to home (climbing)", "Idle") - siehe
/// MspNavMode.formatted in msp_telemetry_codec.dart.
final String flightMode;
final int? activeWaypointIndex;
}
@@ -11,6 +11,7 @@ class MockFlightControllerLink implements FlightControllerLink {
StreamController<TelemetryFrame>? _telemetryController;
Timer? _ticker;
double _heading = 0;
double _batteryPercent = 100;
@override
FcCapabilities get capabilities => const FcCapabilities(
@@ -28,6 +29,9 @@ class MockFlightControllerLink implements FlightControllerLink {
// sichtbar ueberprueft werden kann (Doku 9: UI-Entwicklung ohne
// Hardware).
_heading = (_heading + 5) % 360;
// Langsam sinkender Akkustand, damit die Anzeige auch ohne Hardware
// sichtbar auf Werteaenderungen reagiert (Doku 9).
_batteryPercent = (_batteryPercent - 0.1).clamp(0, 100);
_telemetryController?.add(TelemetryFrame(
lat: 52.5,
lon: 13.4,
@@ -37,6 +41,11 @@ class MockFlightControllerLink implements FlightControllerLink {
speedMs: 18,
headingDeg: _heading,
armed: _armed,
batteryPercent: _batteryPercent.round(),
linkQuality: 95,
flightMode: _activeWaypointIndex != null
? 'Waypoint mission (en route)'
: 'Idle',
activeWaypointIndex: _activeWaypointIndex,
));
});
+16
View File
@@ -28,6 +28,22 @@ abstract final class MspCommands {
/// (`fc_msp.c`, `case MSP2_INAV_STATUS`; ID aus
/// `msp_protocol_v2_inav.h`: `#define MSP2_INAV_STATUS 0x2000`)
static const int inavStatus = 0x2000;
/// Response (24 Byte): u8 flags, u16 batteryVoltage(0.01V),
/// u16 amperage(0.01A), u32 power, u32 mAhDrawn, u32 mWhDrawn,
/// u32 remainingCapacity, u8 batteryPercentage(0-100, bereits fertig
/// berechnet ueber `calculateBatteryPercentage()`), u16 legacyRssi
/// (0-1023, veraltete Skala - siehe stattdessen [inavLinkStats]).
/// (`fc_msp.c`, `case MSP2_INAV_ANALOG`; ID aus
/// `msp_protocol_v2_inav.h`: `#define MSP2_INAV_ANALOG 0x2002`)
static const int inavAnalog = 0x2002;
/// Response (3 Byte): u8 uplinkRssiDbm (negiert gespeichert als
/// `-rxLinkStatistics.uplinkRSSI`), u8 uplinkLinkQuality (0-100%,
/// `rxLinkStatistics.uplinkLQ` - das ist die "Receiver Link Quality"),
/// i8 uplinkSnr. (`fc_msp.c`, `case MSP2_INAV_GET_LINK_STATS`; ID aus
/// `msp_protocol_v2_inav.h`: `#define MSP2_INAV_GET_LINK_STATS 0x2103`)
static const int inavLinkStats = 0x2103;
}
/// Bits von `armingFlags` (Doku: `fc/runtime_config.h`, `armingFlags_e`).
@@ -64,3 +64,61 @@ int? parseMspNavStatusActiveWaypoint(Uint8List payload) {
final activeWpNumber = payload[3];
return activeWpNumber == 0 ? null : activeWpNumber;
}
/// Parst `mode`/`state` aus der `MSP_NAV_STATUS`-Antwort (Byte 0/1, siehe
/// [MspCommands.navStatus]) - Rohwerte aus `navSystemStatus_Mode_e`/
/// `navSystemStatus_State_e` (`navigation.h`, iNAV 9.1.0). [formatted]
/// bildet daraus eine fuer die UI lesbare Kurzbeschreibung (Doku: "drone
/// flight mode" im Settings-Telemetriepanel).
class MspNavMode {
const MspNavMode({required this.mode, required this.state});
final int mode;
final int state;
/// `MW_GPS_MODE_*`-Namen (0=None, 1=Hold, 2=RTH, 3=Nav/Waypoint,
/// 15=Emergency) - mode 0 heisst nur "keine GPS-Nav-Uebersteuerung aktiv",
/// nicht "Manual/Acro": die reine Fluglage (Angle/Horizon/Acro) ist ueber
/// MSP_NAV_STATUS nicht sichtbar (dafuer waere MSP_BOXNAMES+boxModeFlags
/// noetig, hier bewusst nicht umgesetzt).
static const _modeNames = {0: 'Idle', 1: 'Position hold', 2: 'Return to home', 3: 'Waypoint mission', 15: 'Emergency'};
/// `MW_NAV_STATE_*`-Namen, nur die fuer die Kurzanzeige relevanten.
static const _stateNames = {
1: 'starting',
2: 'en route',
3: 'holding',
4: 'holding (timed)',
5: 'en route to WP',
6: 'next waypoint',
7: 'jump',
8: 'landing',
9: 'landing',
10: 'landed',
11: 'landing',
12: 'descending',
13: 'hover above home',
14: 'emergency landing',
15: 'climbing',
};
String get formatted {
final modeLabel = _modeNames[mode] ?? 'Unknown ($mode)';
final stateLabel = _stateNames[state];
return stateLabel == null ? modeLabel : '$modeLabel ($stateLabel)';
}
}
MspNavMode parseMspNavMode(Uint8List payload) {
return MspNavMode(mode: payload[0], state: payload[1]);
}
/// Parst die `MSP2_INAV_ANALOG`-Antwort und liefert den Akku-Ladestand in
/// Prozent (Offset 21, `calculateBatteryPercentage()` - bereits fertig auf
/// 0-100 berechnet, siehe [MspCommands.inavAnalog]).
int parseMspInavAnalogBatteryPercent(Uint8List payload) => payload[21];
/// Parst die `MSP2_INAV_GET_LINK_STATS`-Antwort und liefert die
/// Empfaenger-Linkqualitaet in Prozent (Offset 1, `rxLinkStatistics.uplinkLQ`
/// - bereits 0-100, siehe [MspCommands.inavLinkStats]).
int parseMspLinkQuality(Uint8List payload) => payload[1];
@@ -14,13 +14,11 @@ import 'msp_telemetry_codec.dart';
///
/// - Position/Lage/Hoehe (5-10 Hz): jeden Zyklus (`MSP_RAW_GPS` +
/// `MSP_ALTITUDE`).
/// - Status/Flugmodus (2 Hz): jeden [_statusEveryNCycles]-ten Zyklus
/// (`MSP2_INAV_STATUS` + `MSP_NAV_STATUS`).
///
/// Spannung/Strom (1 Hz laut Doku) fehlt hier bewusst noch - [TelemetryFrame]
/// hat aktuell keine Felder dafuer; sobald die UI das braucht, ist ein
/// dritter, noch selteners abgefragter Zweig (`MSP2_INAV_ANALOG`) trivial
/// ergaenzt.
/// - 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`
@@ -40,6 +38,9 @@ class MspTelemetryPoller {
int _cycle = 0;
bool _armed = false;
int? _activeWaypointIndex;
int _batteryPercent = 0;
int _linkQuality = 0;
String _flightMode = 'Idle';
Stream<TelemetryFrame> get frames => _framesController.stream;
@@ -78,8 +79,14 @@ class MspTelemetryPoller {
_armed = parseMspInavStatusArmed(
await _client.request(MspCommands.inavStatus),
);
_activeWaypointIndex = parseMspNavStatusActiveWaypoint(
await _client.request(MspCommands.navStatus),
final navStatusPayload = await _client.request(MspCommands.navStatus);
_activeWaypointIndex = parseMspNavStatusActiveWaypoint(navStatusPayload);
_flightMode = parseMspNavMode(navStatusPayload).formatted;
_batteryPercent = parseMspInavAnalogBatteryPercent(
await _client.request(MspCommands.inavAnalog),
);
_linkQuality = parseMspLinkQuality(
await _client.request(MspCommands.inavLinkStats),
);
}
@@ -93,6 +100,9 @@ class MspTelemetryPoller {
speedMs: gps.speedMs,
headingDeg: gps.headingDeg,
armed: _armed,
batteryPercent: _batteryPercent,
linkQuality: _linkQuality,
flightMode: _flightMode,
activeWaypointIndex: _activeWaypointIndex,
));
}