From d7d917de4ee51a30e836a6d59c4ebc2a7312d7bf Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Fri, 30 Jan 2026 14:25:18 +0000 Subject: [PATCH] Fix autopilot NMEA units and deviation Co-authored-by: governer --- lib/instruments/autopilot.dart | 154 ++++++++++++++++++--------------- 1 file changed, 86 insertions(+), 68 deletions(-) diff --git a/lib/instruments/autopilot.dart b/lib/instruments/autopilot.dart index c2ef46e..3a84ada 100644 --- a/lib/instruments/autopilot.dart +++ b/lib/instruments/autopilot.dart @@ -8,28 +8,37 @@ import 'package:avaremp/nmea/rmb_packet.dart'; import 'package:avaremp/nmea/rmc_packet.dart'; import 'package:avaremp/plan/waypoint.dart'; import 'package:avaremp/storage.dart'; +import 'package:latlong2/latlong.dart'; import '../io/gps.dart'; class AutoPilot { + static const double _metersPerNauticalMile = 1852.0; + static const double _metersPerSecondToKnots = 1.94384; + static final Distance _distanceCalculator = const Distance(calculator: Haversine()); + static String apCreateSentences() { + final position = Storage().position; + final LatLng currentPosition = Gps.toLatLng(position); + final double speedKnots = _speedKnots(position.speed); + // Create NMEA packet #1 RMCPacket rmcPacket = RMCPacket( - Storage().position.timestamp.millisecondsSinceEpoch, - Storage().position.latitude, - Storage().position.longitude, - GeoCalculations.convertSpeed(Storage().position.speed), - Storage().position.heading, + position.timestamp.millisecondsSinceEpoch, + position.latitude, + position.longitude, + speedKnots, + position.heading, Storage().area.variation, ); String apText = rmcPacket.packet; // Create NMEA packet #2 GGAPacket ggaPacket = GGAPacket( - Storage().position.timestamp.millisecondsSinceEpoch, - Storage().position.latitude, - Storage().position.longitude, - Storage().position.altitude, + position.timestamp.millisecondsSinceEpoch, + position.latitude, + position.longitude, + position.altitude, 6, // just assume as no way to get Storage().area.geoAltitude, 0 // just assume as no way to get @@ -41,66 +50,67 @@ class AutoPilot { // there is no concept of destination without plan in X. if (wp != null) { - int indexCurrent = destinations.indexWhere((element) => element == wp.destination); - if (indexCurrent > 0) { - Destination next = destinations[indexCurrent]; - Destination prev = destinations[indexCurrent - 1]; - String startID = prev.locationID; - String endID = next.locationID; - - // Limit our station IDs to 5 chars max so we don't exceed the 80 char - // sentence limit. A "GPS" fix has a temp name that is quite long - if (startID.length > 5) { - startID = "gSRC"; - } - - if (endID.length > 5) { - endID = "gDST"; - } - - double brgOrig = GeoCalculations().calculateBearing( - prev.coordinate, - next.coordinate); - - double bearing = GeoCalculations().calculateBearing( - Gps.toLatLng(Storage().position), - next.coordinate); - - double distance = GeoCalculations().calculateDistance( - Gps.toLatLng(Storage().position), - next.coordinate); - - // Calculate how many miles we are to the side of the course line - double deviation = distance * sin(_angularDifference(brgOrig, bearing)); - - // If we are to the left of the course line, then make our deviation negative. - if (_leftOfCourseLine(bearing, brgOrig)) { - deviation = -deviation; - } - - // We now have all the info to create NMEA packet #3 - RMBPacket rmbPacket = RMBPacket( - distance, - bearing, - next.coordinate.longitude, - next.coordinate.latitude, - endID, - startID, - deviation, - GeoCalculations.convertSpeed(Storage().position.speed), - destinations.length == indexCurrent + 1, - ); - apText += rmbPacket.packet; - - // Now for the final NMEA packet - BODPacket bodPacket = BODPacket( - endID, - startID, - brgOrig, - GeoCalculations.getMagneticHeading(brgOrig, (next.geoVariation == null ? Storage().area.variation : next.geoVariation!)), - ); - apText += bodPacket.packet; + Destination next = wp.destination; + Destination? prev = Storage().route.getPreviousDestination(); + String startID = prev?.locationID ?? ""; + String endID = next.locationID; + + // Limit our station IDs to 5 chars max so we don't exceed the 80 char + // sentence limit. A "GPS" fix has a temp name that is quite long + if (startID.length > 5) { + startID = "gSRC"; + } + + if (endID.length > 5) { + endID = "gDST"; } + + LatLng origin = prev?.coordinate ?? currentPosition; + double brgOrig = GeoCalculations().calculateBearing( + origin, + next.coordinate); + + double bearing = GeoCalculations().calculateBearing( + currentPosition, + next.coordinate); + + double distance = _distanceNm( + currentPosition, + next.coordinate); + + // Calculate how many nm we are to the side of the course line + double deviation = distance * sin(GeoCalculations.toRadians(_angularDifference(brgOrig, bearing))); + + // If we are to the left of the course line, then make our deviation negative. + if (_leftOfCourseLine(bearing, brgOrig)) { + deviation = -deviation; + } + + int indexCurrent = destinations.indexWhere((element) => element == next); + bool planComplete = indexCurrent >= 0 && destinations.length == indexCurrent + 1; + + // We now have all the info to create NMEA packet #3 + RMBPacket rmbPacket = RMBPacket( + distance, + bearing, + next.coordinate.longitude, + next.coordinate.latitude, + endID, + startID, + deviation, + speedKnots, + planComplete, + ); + apText += rmbPacket.packet; + + // Now for the final NMEA packet + BODPacket bodPacket = BODPacket( + endID, + startID, + brgOrig, + GeoCalculations.getMagneticHeading(brgOrig, (next.geoVariation == null ? Storage().area.variation : next.geoVariation!)), + ); + apText += bodPacket.packet; } return apText; } @@ -125,4 +135,12 @@ class AutoPilot { // brgCourse will be > 180 at this point return (bT > bC || bT < bC - 180); } + + static double _speedKnots(double metersPerSecond) { + return metersPerSecond * _metersPerSecondToKnots; + } + + static double _distanceNm(LatLng from, LatLng to) { + return _distanceCalculator(from, to) / _metersPerNauticalMile; + } } \ No newline at end of file