Implement waypoint mission upload (MSP_SET_WP) and Fly-mode send button
Fills in MspFlightControllerLink.uploadMission(), which previously just threw UnimplementedError, plus a new verifyMission() (both now on the generic FlightControllerLink interface, protocol-neutral by signature - MAVLink/ArduPilot get their own implementation later without touching callers). All iNAV-specific encoding lives in the new msp_waypoint_codec.dart: - encodeMspSetWaypoint(): the 21-byte MSP_SET_WP payload. Action/P1/P2/P3 byte layout was checked against the actual iNAV 9.1.0 source (navigation.c/navigation.h), not guessed - notably our generic `loiter` action has no configurable duration, so it maps to NAV_WP_ACTION_HOLD_TIME with the max representable p1 (int16 max, not 0xFFFF - that would read as -1 and end the hold immediately instead of never). - parseMspWpGetInfo(): decodes MSP_WP_GETINFO's validity/count fields, used by verifyMission() to confirm the FC actually accepted the full mission (Doku 2.2/4.5 Ready-to-Fly-Gate: "upload + verified"). uploadMission() sends one MSP_SET_WP per waypoint in order (iNAV has no batch command - WP#1 resets the FC's mission list, every next number must follow immediately, only the last carries NAV_WP_FLAG_LAST) and rejects missions above NAV_MAX_WAYPOINTS upfront instead of silently truncating. MissionSyncService now calls the real verifyMission() instead of always confirming, throwing MissionVerificationException when the FC doesn't confirm the mission. FlyScreen's footer swaps the warnings button for a send button (Doku: "ersetze den warnings button mit einem wp send button", pink horizontal PaperPlaneIcon, matching the existing paper-plane drone iconography) - warnings/event log stay reachable via the drone status pill's Warnings tab. BottomStatsBar/MissionFooterBar gained onSendTap/sending in place of the old forceShowWarningsButton. Verified end to end on the Pixel_10a emulator: tapping send with WLAN as the active connection type triggers the real WifiNetworkSpecifier flow through MspFlightControllerLink (correctly reports "no devices found" - expected, no real mLRS bridge on the emulator); the actual MSP_SET_WP/ MSP_WP_GETINFO wire behavior is covered by tests against a fake FC responder over LoopbackTransport instead. Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
This commit is contained in:
Constantin Leue
co-authored by
Claude Sonnet 5
Claude Sonnet 5
parent
5fa55ac67d
commit
13475cc4f3
@@ -122,6 +122,14 @@ abstract class FlightControllerLink {
|
||||
|
||||
Future<void> uploadMission(FlatWaypointList mission);
|
||||
|
||||
/// Prueft nach [uploadMission], ob der Flightcontroller die Mission
|
||||
/// tatsaechlich vollstaendig und gueltig uebernommen hat (Doku 2.2/4.5:
|
||||
/// Ready-to-Fly-Gate "Upload + verifiziert + disarmed") - protokollneutrale
|
||||
/// Ja/Nein-Antwort, jede Implementierung entscheidet selbst, wie sie das
|
||||
/// beim jeweiligen Flightcontroller abfragt (MSP: `MSP_WP_GETINFO`,
|
||||
/// MAVLink: `MISSION_REQUEST_LIST`/`MISSION_COUNT`-Runde).
|
||||
Future<bool> verifyMission(FlatWaypointList mission);
|
||||
|
||||
Future<void> setFlightMode(FlightMode mode);
|
||||
|
||||
Future<void> arm();
|
||||
|
||||
@@ -144,9 +144,20 @@ class MockFlightControllerLink implements FlightControllerLink {
|
||||
if (_armed) {
|
||||
throw StateError('Mission-Upload verweigert: Drohne ist armed.');
|
||||
}
|
||||
_uploadedWaypointCount = mission.waypoints.length;
|
||||
_activeWaypointIndex = mission.waypoints.isEmpty ? null : 0;
|
||||
}
|
||||
|
||||
/// Kein separates FC-seitiges "gueltig"-Flag zu simulieren (anders als
|
||||
/// MspFlightControllerLink, das echte MSP_WP_GETINFO-Antworten abfragt) -
|
||||
/// der Mock haelt die zuletzt hochgeladene Wegpunktzahl und vergleicht sie
|
||||
/// hier direkt, das reicht fuer UI-Entwicklung ohne Hardware.
|
||||
int? _uploadedWaypointCount;
|
||||
|
||||
@override
|
||||
Future<bool> verifyMission(FlatWaypointList mission) async =>
|
||||
_uploadedWaypointCount == mission.waypoints.length;
|
||||
|
||||
@override
|
||||
Future<void> setFlightMode(FlightMode mode) async {}
|
||||
|
||||
|
||||
@@ -14,6 +14,22 @@ abstract final class MspCommands {
|
||||
/// (`fc_msp.c`, `case MSP_RAW_GPS`)
|
||||
static const int rawGps = 106;
|
||||
|
||||
/// Response (4 Byte): u8 reserved, u8 maxWaypoints (NAV_MAX_WAYPOINTS),
|
||||
/// u8 isWaypointListValid (`posControl.waypointListValid`), u8
|
||||
/// waypointCount (`getWaypointCount()`) - Grundlage der Upload-
|
||||
/// Verifikation (Doku 2.2/4.5: "Upload + verifiziert"), siehe
|
||||
/// msp_waypoint_codec.dart. (`fc_msp.c`, `case MSP_WP_GETINFO`)
|
||||
static const int wpGetInfo = 20;
|
||||
|
||||
/// Request-Payload (21 Byte, siehe msp_waypoint_codec.dart
|
||||
/// `encodeMspSetWaypoint`): u8 wp_no, u8 action, i32 lat(1e-7 deg),
|
||||
/// i32 lon(1e-7 deg), i32 alt(cm), i16 p1, i16 p2, i16 p3, u8 flag. Nur
|
||||
/// WP#1..NAV_MAX_WAYPOINTS gueltig; WP#1 setzt die Missionsliste des FC
|
||||
/// zurueck (neue Mission), jede weitere WP# muss exakt die naechste sein
|
||||
/// (`fc_msp.c` `case MSP_SET_WP`, `navigation.c` `setWaypoint()` - kein
|
||||
/// Batch-Kommando, ein Aufruf pro Wegpunkt).
|
||||
static const int setWp = 209;
|
||||
|
||||
/// Response: i32 estAlt(cm), i16 vario(cm/s), i32 baroAlt(cm).
|
||||
/// (`fc_msp.c`, `case MSP_ALTITUDE`)
|
||||
static const int altitude = 109;
|
||||
|
||||
@@ -5,6 +5,7 @@ import 'msp_client.dart';
|
||||
import 'msp_commands.dart';
|
||||
import 'msp_telemetry_codec.dart';
|
||||
import 'msp_telemetry_poller.dart';
|
||||
import 'msp_waypoint_codec.dart';
|
||||
|
||||
/// iNAV-Anbindung via MSP (Architektur-Doku 3.1), umgesetzt gegen den
|
||||
/// tatsaechlichen iNAV-9.1.0-Quellcode (Doku Kommunikationsschicht Abschnitt
|
||||
@@ -17,11 +18,11 @@ import 'msp_telemetry_poller.dart';
|
||||
/// oder spaeter WLAN/UDP steckt, ist fuer diese Klasse unsichtbar.
|
||||
///
|
||||
/// Setzt die in der Doku vorgegebene Reihenfolge um: Rahmen kodieren/
|
||||
/// dekodieren + Telemetrie lesen (dieser Schritt). Missionsupload,
|
||||
/// Flugmodus-Wechsel und Armen sind bewusst noch nicht umgesetzt (Doku 4,
|
||||
/// "Reihenfolge der Umsetzung": "Erst danach Missionsupload") - das sind
|
||||
/// eigene, sicherheitsrelevante Schritte (u.a. `MSP_SET_WP`), die eigene
|
||||
/// Sorgfalt brauchen.
|
||||
/// dekodieren, Telemetrie lesen, Missionsupload (`MSP_SET_WP`, siehe
|
||||
/// msp_waypoint_codec.dart fuer das Byte-Layout und die
|
||||
/// Aktions-P1/P2/P3-Belegung). Flugmodus-Wechsel und Armen sind weiterhin
|
||||
/// bewusst nicht umgesetzt - eigene, sicherheitsrelevante Schritte, die
|
||||
/// eigene Sorgfalt brauchen.
|
||||
class MspFlightControllerLink implements FlightControllerLink {
|
||||
MspFlightControllerLink({required this.transport});
|
||||
|
||||
@@ -35,9 +36,12 @@ class MspFlightControllerLink implements FlightControllerLink {
|
||||
supportsMultiMission: true,
|
||||
supportsInFlightUpload: false,
|
||||
// Konservativer iNAV-Default (navigation.h: NAV_MAX_WAYPOINTS = 15).
|
||||
// Viele Boards/Targets erlauben mehr; ohne eine MSP_WP_GETINFO-
|
||||
// Abfrage (Teil des noch nicht umgesetzten Missions-Uploads) nicht
|
||||
// praeziser bestimmbar.
|
||||
// Viele Boards/Targets erlauben mehr; uploadMission() fragt das nicht
|
||||
// live per MSP_WP_GETINFO ab (dessen maxWaypoints-Feld wird nur zur
|
||||
// Verifikation nach dem Upload gelesen, siehe verifyMission()) -
|
||||
// ein client-seitiger Vorab-Check gegen diesen Default verhindert
|
||||
// trotzdem, dass wir versuchen, mehr Wegpunkte zu senden, als der FC
|
||||
// WP-Nummern dafuer reserviert (WP#1..#15).
|
||||
maxWaypoints: 15,
|
||||
);
|
||||
|
||||
@@ -68,11 +72,46 @@ class MspFlightControllerLink implements FlightControllerLink {
|
||||
}
|
||||
|
||||
@override
|
||||
Future<void> uploadMission(FlatWaypointList mission) =>
|
||||
throw UnimplementedError(
|
||||
'Missions-Upload (MSP_SET_WP) ist noch nicht umgesetzt - Doku '
|
||||
'Kommunikationsschicht 4 sieht das erst nach dem Telemetrie-Schritt vor.',
|
||||
Future<void> uploadMission(FlatWaypointList mission) async {
|
||||
final client = _client;
|
||||
if (client == null) {
|
||||
throw StateError('uploadMission() vor connect() aufgerufen.');
|
||||
}
|
||||
final waypoints = mission.waypoints;
|
||||
if (waypoints.isEmpty) {
|
||||
throw ArgumentError('Mission ohne Wegpunkte kann nicht hochgeladen werden.');
|
||||
}
|
||||
if (waypoints.length > capabilities.maxWaypoints) {
|
||||
throw ArgumentError(
|
||||
'Mission hat ${waypoints.length} Wegpunkte, iNAV erlaubt hier '
|
||||
'maximal ${capabilities.maxWaypoints} (NAV_MAX_WAYPOINTS).',
|
||||
);
|
||||
}
|
||||
|
||||
// MSP_SET_WP kennt kein Batch-Kommando - der FC nimmt WP-fuer-WP nur
|
||||
// entweder WP#1 (startet damit eine neue Mission, resetWaypointList())
|
||||
// oder direkt den naechsten erwarteten WP# an (navigation.c
|
||||
// setWaypoint()). Erst das NAV_WP_FLAG_LAST auf dem letzten Wegpunkt
|
||||
// macht die Mission fuer den FC gueltig, siehe verifyMission().
|
||||
for (var i = 0; i < waypoints.length; i++) {
|
||||
final payload = encodeMspSetWaypoint(
|
||||
waypoints[i],
|
||||
wireIndex: i + 1,
|
||||
isLast: i == waypoints.length - 1,
|
||||
);
|
||||
await client.request(MspCommands.setWp, payload: payload);
|
||||
}
|
||||
}
|
||||
|
||||
@override
|
||||
Future<bool> verifyMission(FlatWaypointList mission) async {
|
||||
final client = _client;
|
||||
if (client == null) {
|
||||
throw StateError('verifyMission() vor connect() aufgerufen.');
|
||||
}
|
||||
final info = parseMspWpGetInfo(await client.request(MspCommands.wpGetInfo));
|
||||
return info.isValid && info.waypointCount == mission.waypoints.length;
|
||||
}
|
||||
|
||||
@override
|
||||
Future<void> setFlightMode(FlightMode mode) => throw UnimplementedError(
|
||||
|
||||
@@ -0,0 +1,138 @@
|
||||
import 'dart:typed_data';
|
||||
|
||||
import '../../domain/waypoint/flat_waypoint_list.dart';
|
||||
|
||||
/// `navWaypointActions_e` (Doku Kommunikationsschicht v2 Abschnitt 6,
|
||||
/// geprueft gegen den iNAV-9.1.0-Quellcode: `navigation.h`). Nur die vier
|
||||
/// Werte, auf die unser protokollneutrales [WaypointAction] tatsaechlich
|
||||
/// abbildet, sind hier benannt - RTH/SET_POI/SET_HEAD kennt unser
|
||||
/// Domainmodell (noch) nicht.
|
||||
abstract final class MspNavWpAction {
|
||||
static const int waypoint = 0x01;
|
||||
static const int holdTime = 0x03;
|
||||
static const int jump = 0x06;
|
||||
static const int land = 0x08;
|
||||
}
|
||||
|
||||
/// `navWaypointFlags_e` (`navigation.h`). [last] markiert den letzten
|
||||
/// Wegpunkt einer Mission - erst dieses Flag setzt
|
||||
/// `posControl.waypointListValid` auf dem FC, siehe encodeMspSetWaypoint-Doku.
|
||||
abstract final class MspNavWpFlag {
|
||||
static const int none = 0;
|
||||
static const int last = 0xA5;
|
||||
}
|
||||
|
||||
/// p1 ist `int16_t` auf der Leitung (Doku: `navWaypoint_t`) - groesster
|
||||
/// darstellbarer positiver Wert, nicht 0xFFFF (das waere als int16 -1 und
|
||||
/// wuerde NAV_WP_ACTION_HOLD_TIME sofort - statt dauerhaft - abschliessen
|
||||
/// lassen, siehe navigation.c `navOnEnteringState_NAV_STATE_WAYPOINT_...`:
|
||||
/// "p1 <= 0" beendet das Halten sofort).
|
||||
const int _mspInt16Max = 0x7FFF;
|
||||
|
||||
/// Kodiert einen einzelnen Wegpunkt als 21-Byte MSP_SET_WP-Payload
|
||||
/// (`MspCommands.setWp`, Doku Kommunikationsschicht v2 Abschnitt 6).
|
||||
///
|
||||
/// [wireIndex] ist die 1-basierte FC-interne Wegpunktnummer (WP#1..#15),
|
||||
/// NICHT der 0-basierte Index in der App-eigenen Liste - iNAV reserviert
|
||||
/// WP#0 fuer "Home" und WP#255 fuer einen Direkt-Sprung im Poshold-Modus
|
||||
/// (`navigation.c` `setWaypoint()`). Der Aufrufer (MspFlightControllerLink)
|
||||
/// ist dafuer verantwortlich, WP#1 zuerst und danach jede weitere Nummer
|
||||
/// lueckenlos aufsteigend zu senden - der FC nimmt sonst gar nichts an.
|
||||
///
|
||||
/// [isLast] setzt [MspNavWpFlag.last] auf dem letzten Wegpunkt - ohne dieses
|
||||
/// Flag bleibt `posControl.waypointListValid` auf dem FC false, selbst wenn
|
||||
/// alle Wegpunkte einzeln angekommen sind (siehe `MspCommands.wpGetInfo`,
|
||||
/// zur Verifikation nach dem Upload).
|
||||
///
|
||||
/// Aktions-P1/P2/P3-Belegung (`navigation.c` `getActiveSpeed()`/
|
||||
/// `setupJumpCounters()`, Aktion fuer Aktion geprueft statt geraten):
|
||||
/// - WAYPOINT/LAND: p1 = Sollgeschwindigkeit (cm/s), p2 unbenutzt (0).
|
||||
/// - Unser generisches `loiter` hat im Domainmodell keine eigene Dauer
|
||||
/// (HTML-Referenz zeigt es als endloses Kreisen um den Punkt mit dem
|
||||
/// `loiterRadius` des Drohnenprofils, kein Zeitfeld) - kodiert als
|
||||
/// NAV_WP_ACTION_HOLD_TIME mit dem groesstmoeglichen p1 (`_mspInt16Max`
|
||||
/// Sekunden, ueber 9 Stunden - laenger als jeder realistische Flug), p2 =
|
||||
/// Sollgeschwindigkeit (cm/s, dort das P1-Aequivalent fuer HOLD_TIME).
|
||||
/// - JUMP: p1 = 1-basierte Ziel-WP# (der FC zieht beim Empfang selbst 1 ab,
|
||||
/// `setWaypoint()`: "make index (vice WP #)"), p2 = statische
|
||||
/// Wiederholzahl (-1 = endlos, `jumpRepeatCount == null`). p3 haelt bei
|
||||
/// JUMP den volatilen Wiederholzaehler - der FC initialisiert ihn beim
|
||||
/// Missionsstart selbst aus p2 (`setupJumpCounters()`), wir spiegeln das
|
||||
/// hier nur defensiv.
|
||||
/// - Bei allen anderen Aktionen ist p3 das Hoehenmodus-Bitfeld
|
||||
/// (`NAV_WP_ALTMODE`): 0 = relativ zum Startpunkt - passend zu unserem
|
||||
/// `altitudeM` (siehe TelemetryFrame.altitudeM-Doku: "keine absolute Hoehe
|
||||
/// ueber Meeresspiegel"), 1 waere absolut (AMSL) und wird hier nie gesetzt.
|
||||
Uint8List encodeMspSetWaypoint(
|
||||
Waypoint waypoint, {
|
||||
required int wireIndex,
|
||||
required bool isLast,
|
||||
}) {
|
||||
final speedP1 = (waypoint.speedMs * 100).round().clamp(0, _mspInt16Max);
|
||||
|
||||
final int action;
|
||||
final int p1;
|
||||
final int p2;
|
||||
final int p3;
|
||||
switch (waypoint.action) {
|
||||
case WaypointAction.none:
|
||||
action = MspNavWpAction.waypoint;
|
||||
p1 = speedP1;
|
||||
p2 = 0;
|
||||
p3 = 0;
|
||||
case WaypointAction.loiter:
|
||||
action = MspNavWpAction.holdTime;
|
||||
p1 = _mspInt16Max;
|
||||
p2 = speedP1;
|
||||
p3 = 0;
|
||||
case WaypointAction.landing:
|
||||
action = MspNavWpAction.land;
|
||||
p1 = speedP1;
|
||||
p2 = 0;
|
||||
p3 = 0;
|
||||
case WaypointAction.jump:
|
||||
action = MspNavWpAction.jump;
|
||||
final target = ((waypoint.jumpTargetIndex ?? 0) + 1)
|
||||
.clamp(1, _mspInt16Max);
|
||||
final repeat = (waypoint.jumpRepeatCount ?? -1).clamp(-1, _mspInt16Max);
|
||||
p1 = target;
|
||||
p2 = repeat;
|
||||
p3 = repeat;
|
||||
}
|
||||
|
||||
final payload = ByteData(21);
|
||||
payload.setUint8(0, wireIndex);
|
||||
payload.setUint8(1, action);
|
||||
payload.setInt32(2, (waypoint.lat * 1e7).round(), Endian.little);
|
||||
payload.setInt32(6, (waypoint.lon * 1e7).round(), Endian.little);
|
||||
payload.setInt32(10, (waypoint.altitudeM * 100).round(), Endian.little);
|
||||
payload.setInt16(14, p1, Endian.little);
|
||||
payload.setInt16(16, p2, Endian.little);
|
||||
payload.setInt16(18, p3, Endian.little);
|
||||
payload.setUint8(20, isLast ? MspNavWpFlag.last : MspNavWpFlag.none);
|
||||
return payload.buffer.asUint8List();
|
||||
}
|
||||
|
||||
/// Antwort auf `MspCommands.wpGetInfo` (4 Byte): ob der FC die aktuell
|
||||
/// hochgeladene Mission als vollstaendig/gueltig fuehrt, und wie viele
|
||||
/// Wegpunkte er dafuer zaehlt - Grundlage der Upload-Verifikation (Doku
|
||||
/// 2.2/4.5).
|
||||
class MspWpGetInfo {
|
||||
const MspWpGetInfo({
|
||||
required this.maxWaypoints,
|
||||
required this.isValid,
|
||||
required this.waypointCount,
|
||||
});
|
||||
|
||||
final int maxWaypoints;
|
||||
final bool isValid;
|
||||
final int waypointCount;
|
||||
}
|
||||
|
||||
MspWpGetInfo parseMspWpGetInfo(Uint8List payload) {
|
||||
return MspWpGetInfo(
|
||||
maxWaypoints: payload[1],
|
||||
isValid: payload[2] != 0,
|
||||
waypointCount: payload[3],
|
||||
);
|
||||
}
|
||||
Reference in New Issue
Block a user