Point the fly-mode rubber band at home during Return-to-Home
iNAV freezes activeWaypointIndex at whatever mission waypoint was active when RTH engaged instead of updating it to reflect the new target - navigation.c derives NAV_Status.activeWpIndex unconditionally from posControl.activeWaypointIndex, and none of the RTH state-entry handlers touch that field. Drawing the guidance line to that stale index would point at a waypoint the aircraft may have already passed. Extracted the target selection into computeRubberBandTarget() (mission stats.dart) so the RTH special-case and the abort-back-to-waypoint-mode fallback are unit-tested rather than only living inline in the widget build method - since it re-evaluates navMode on every frame, aborting RTH switches the line back to the mission waypoint with no extra transition logic needed.
This commit is contained in:
@@ -98,3 +98,41 @@ FlyMissionStatus? computeFlyMissionStatus({
|
|||||||
etaToActiveWaypointSec: eta,
|
etaToActiveWaypointSec: eta,
|
||||||
);
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// `MW_GPS_MODE_RTH` (siehe MspNavMode._modeNames in msp_telemetry_codec.dart).
|
||||||
|
const navModeReturnToHome = 2;
|
||||||
|
|
||||||
|
/// Ziel des Gummiband-Pfeils von der Drohne zum aktuellen Flugziel im
|
||||||
|
/// Fly-Modus (Doku: MissionMap.rubberBandLine) - normalerweise der aktive
|
||||||
|
/// Missionswegpunkt, waehrend Return-to-Home ([navModeReturnToHome])
|
||||||
|
/// stattdessen [homePoint].
|
||||||
|
///
|
||||||
|
/// iNAV friert `activeWaypointIndex` beim RTH-Eintritt auf dem zuletzt
|
||||||
|
/// aktiven Missionswegpunkt ein, statt ihn auf den Homepoint zu setzen
|
||||||
|
/// (`navigation.c`: `NAV_Status.activeWpIndex` wird unbedingt aus
|
||||||
|
/// `posControl.activeWaypointIndex` abgeleitet, keiner der
|
||||||
|
/// `navOnEnteringState_NAV_STATE_RTH_*`-Handler fasst diesen Wert an) - ein
|
||||||
|
/// Gummiband zu diesem (ggf. laengst passierten) Wegpunkt waere im RTH-Fall
|
||||||
|
/// irrefuehrend. Da [navMode] bei jedem neuen Telemetrie-Frame neu geprueft
|
||||||
|
/// wird, schaltet ein Abbruch von RTH zurueck in den Wegpunktmodus das
|
||||||
|
/// Ergebnis von selbst wieder auf den Missionswegpunkt zurueck.
|
||||||
|
///
|
||||||
|
/// Liefert null, wenn kein sinnvolles Ziel bekannt ist (RTH ohne bereits
|
||||||
|
/// abgefragten Homepoint, oder [activeWaypointIndex] ausserhalb der Liste).
|
||||||
|
LatLng? computeRubberBandTarget({
|
||||||
|
required int navMode,
|
||||||
|
required int? activeWaypointIndex,
|
||||||
|
required List<Waypoint> waypoints,
|
||||||
|
LatLng? homePoint,
|
||||||
|
}) {
|
||||||
|
if (navMode == navModeReturnToHome) {
|
||||||
|
return homePoint;
|
||||||
|
}
|
||||||
|
if (activeWaypointIndex == null ||
|
||||||
|
activeWaypointIndex < 0 ||
|
||||||
|
activeWaypointIndex >= waypoints.length) {
|
||||||
|
return null;
|
||||||
|
}
|
||||||
|
final target = waypoints[activeWaypointIndex];
|
||||||
|
return LatLng(target.lat, target.lon);
|
||||||
|
}
|
||||||
|
|||||||
@@ -293,23 +293,25 @@ class _FlyScreenState extends ConsumerState<FlyScreen> {
|
|||||||
child: const DroneMarkerIcon(),
|
child: const DroneMarkerIcon(),
|
||||||
);
|
);
|
||||||
|
|
||||||
// Gestrichelte Linie von der Drohne zum aktiven Wegpunkt (Doku 3.4:
|
// Gestrichelte Linie von der Drohne zum aktuellen Flugziel (Doku 3.4:
|
||||||
// TelemetryFrame.activeWaypointIndex aus MSP_NAV_STATUS) - derselbe
|
// TelemetryFrame.activeWaypointIndex aus MSP_NAV_STATUS) - derselbe
|
||||||
// Darstellungsstil wie das Reticle-Gummiband im Plan-Modus (MissionMap.
|
// Darstellungsstil wie das Reticle-Gummiband im Plan-Modus (MissionMap.
|
||||||
// rubberBandLine), hier aber Drohne -> naechster Wegpunkt statt letzter
|
// rubberBandLine), hier aber Drohne -> Flugziel statt letzter Wegpunkt
|
||||||
// Wegpunkt -> Kartenmitte.
|
// -> Kartenmitte. Das Ziel selbst (Missionswegpunkt oder bei Return-to-
|
||||||
List<LatLng>? rubberBand;
|
// Home der Homepoint) kommt aus computeRubberBandTarget, siehe dortige
|
||||||
|
// Doku fuer den RTH-Sonderfall.
|
||||||
final activeWpIndex = telemetry?.activeWaypointIndex;
|
final activeWpIndex = telemetry?.activeWaypointIndex;
|
||||||
if (telemetry != null &&
|
List<LatLng>? rubberBand;
|
||||||
telemetry.hasFix &&
|
if (telemetry != null && telemetry.hasFix) {
|
||||||
activeWpIndex != null &&
|
final target = computeRubberBandTarget(
|
||||||
activeWpIndex >= 0 &&
|
navMode: telemetry.navMode,
|
||||||
activeWpIndex < waypoints.length) {
|
activeWaypointIndex: activeWpIndex,
|
||||||
final target = waypoints[activeWpIndex];
|
waypoints: waypoints,
|
||||||
rubberBand = [
|
homePoint: homePoint == null ? null : LatLng(homePoint.lat, homePoint.lon),
|
||||||
LatLng(telemetry.lat, telemetry.lon),
|
);
|
||||||
LatLng(target.lat, target.lon),
|
if (target != null) {
|
||||||
];
|
rubberBand = [LatLng(telemetry.lat, telemetry.lon), target];
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Live-Missionsstatus fuer die Details-Pille (Doku: "aktiver Wegpunkt
|
// Live-Missionsstatus fuer die Details-Pille (Doku: "aktiver Wegpunkt
|
||||||
|
|||||||
@@ -1,6 +1,7 @@
|
|||||||
import 'package:dmc_app/domain/mission/mission_stats.dart';
|
import 'package:dmc_app/domain/mission/mission_stats.dart';
|
||||||
import 'package:dmc_app/domain/waypoint/flat_waypoint_list.dart';
|
import 'package:dmc_app/domain/waypoint/flat_waypoint_list.dart';
|
||||||
import 'package:flutter_test/flutter_test.dart';
|
import 'package:flutter_test/flutter_test.dart';
|
||||||
|
import 'package:latlong2/latlong.dart';
|
||||||
|
|
||||||
Waypoint _wp(double lat, double lon, {double speed = 15}) =>
|
Waypoint _wp(double lat, double lon, {double speed = 15}) =>
|
||||||
Waypoint(lat: lat, lon: lon, altitudeM: 60, speedMs: speed, catchRadiusM: 60);
|
Waypoint(lat: lat, lon: lon, altitudeM: 60, speedMs: speed, catchRadiusM: 60);
|
||||||
@@ -110,4 +111,80 @@ void main() {
|
|||||||
expect(status!.etaToActiveWaypointSec, isNull);
|
expect(status!.etaToActiveWaypointSec, isNull);
|
||||||
});
|
});
|
||||||
});
|
});
|
||||||
|
|
||||||
|
group('computeRubberBandTarget', () {
|
||||||
|
final waypoints = [_wp(0, 0), _wp(0, 0.01), _wp(0, 0.02)];
|
||||||
|
const home = LatLng(1, 1);
|
||||||
|
|
||||||
|
test('zeigt normalerweise auf den aktiven Missionswegpunkt', () {
|
||||||
|
final target = computeRubberBandTarget(
|
||||||
|
navMode: 3, // Waypoint mission
|
||||||
|
activeWaypointIndex: 1,
|
||||||
|
waypoints: waypoints,
|
||||||
|
homePoint: home,
|
||||||
|
);
|
||||||
|
expect(target, const LatLng(0, 0.01));
|
||||||
|
});
|
||||||
|
|
||||||
|
test('zeigt bei Return-to-Home (navMode 2) auf den Homepoint statt auf '
|
||||||
|
'den (ggf. laengst passierten) eingefrorenen Missionswegpunkt', () {
|
||||||
|
final target = computeRubberBandTarget(
|
||||||
|
navMode: navModeReturnToHome,
|
||||||
|
activeWaypointIndex: 1,
|
||||||
|
waypoints: waypoints,
|
||||||
|
homePoint: home,
|
||||||
|
);
|
||||||
|
expect(target, home);
|
||||||
|
});
|
||||||
|
|
||||||
|
test('liefert null bei RTH ohne bekannten Homepoint statt eines '
|
||||||
|
'erfundenen Ziels', () {
|
||||||
|
final target = computeRubberBandTarget(
|
||||||
|
navMode: navModeReturnToHome,
|
||||||
|
activeWaypointIndex: 1,
|
||||||
|
waypoints: waypoints,
|
||||||
|
homePoint: null,
|
||||||
|
);
|
||||||
|
expect(target, isNull);
|
||||||
|
});
|
||||||
|
|
||||||
|
test('schaltet nach Abbruch von RTH (navMode wieder 3) automatisch '
|
||||||
|
'zurueck auf den Missionswegpunkt', () {
|
||||||
|
const rth = 2;
|
||||||
|
final duringRth = computeRubberBandTarget(
|
||||||
|
navMode: rth,
|
||||||
|
activeWaypointIndex: 1,
|
||||||
|
waypoints: waypoints,
|
||||||
|
homePoint: home,
|
||||||
|
);
|
||||||
|
final afterAbort = computeRubberBandTarget(
|
||||||
|
navMode: 3,
|
||||||
|
activeWaypointIndex: 1,
|
||||||
|
waypoints: waypoints,
|
||||||
|
homePoint: home,
|
||||||
|
);
|
||||||
|
expect(duringRth, home);
|
||||||
|
expect(afterAbort, const LatLng(0, 0.01));
|
||||||
|
});
|
||||||
|
|
||||||
|
test('activeWaypointIndex null oder ausserhalb der Liste ergibt null '
|
||||||
|
'(ausserhalb RTH)', () {
|
||||||
|
expect(
|
||||||
|
computeRubberBandTarget(
|
||||||
|
navMode: 3,
|
||||||
|
activeWaypointIndex: null,
|
||||||
|
waypoints: waypoints,
|
||||||
|
),
|
||||||
|
isNull,
|
||||||
|
);
|
||||||
|
expect(
|
||||||
|
computeRubberBandTarget(
|
||||||
|
navMode: 3,
|
||||||
|
activeWaypointIndex: 3,
|
||||||
|
waypoints: waypoints,
|
||||||
|
),
|
||||||
|
isNull,
|
||||||
|
);
|
||||||
|
});
|
||||||
|
});
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user