OpenCPN Partial API docs
Loading...
Searching...
No Matches
autopilot_output.cpp
Go to the documentation of this file.
1/***************************************************************************
2 * Copyright (C) 2025 by David S. Register *
3 * *
4 * This program is free software; you can redistribute it and/or modify *
5 * it under the terms of the GNU General Public License as published by *
6 * the Free Software Foundation; either version 2 of the License, or *
7 * (at your option) any later version. *
8 * *
9 * This program is distributed in the hope that it will be useful, *
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
12 * GNU General Public License for more details. *
13 * *
14 * You should have received a copy of the GNU General Public License *
15 * along with this program; if not, see <https://www.gnu.org/licenses/>. *
16 **************************************************************************/
17
24#include <cmath>
25#include <memory>
26#include <vector>
27
28#include <wx/wxprec.h>
29
34#include "model/comm_vars.h"
35#include "model/config_vars.h"
36#include "model/georef.h"
37#include "model/gui.h"
39#include "model/nmea_log.h"
40#include "model/own_ship.h"
41#include "model/routeman.h"
42#include "N2kMsg.h"
43#include "N2kMessages.h"
44
45#include "observable_globvar.h"
46
47#ifdef __ANDROID__
48#include "androidUTIL.h"
49#endif
50
51static NmeaLog *GetNmeaLog() {
52 auto w = wxWindow::FindWindowByName(kDataMonitorWindowName);
53 auto log = dynamic_cast<NmeaLog *>(w);
54 assert(log);
55 return log;
56}
57
58static void SendRmc(NMEA0183 &nmea0183, Routeman &routeman) {
59 SENTENCE snt;
60 nmea0183.Rmc.IsDataValid = bGPSValid ? NTrue : NFalse;
61
62 nmea0183.Rmc.Position.Latitude.Set(std::fabs(gLat), gLat < 0 ? "S" : "N");
63 nmea0183.Rmc.Position.Longitude.Set(std::fabs(gLon), gLon < 0 ? "W" : "E");
64
65 nmea0183.Rmc.SpeedOverGroundKnots = std::isnan(gSog) ? 0.0 : gSog;
66 nmea0183.Rmc.TrackMadeGoodDegreesTrue = std::isnan(gCog) ? 0.0 : gCog;
67
68 if (!std::isnan(gVar)) {
69 nmea0183.Rmc.MagneticVariation = std::fabs(gVar);
70 nmea0183.Rmc.MagneticVariationDirection = gVar < 0 ? West : East;
71 } else {
72 // A signal to NMEA converter, gVAR is unknown
73 nmea0183.Rmc.MagneticVariation = 361.;
74 }
75
76 // Send GPS time to autopilot if available else send local system time
77 if (!gRmcTime.IsEmpty() && !gRmcDate.IsEmpty()) {
78 nmea0183.Rmc.UTCTime = gRmcTime;
79 nmea0183.Rmc.Date = gRmcDate;
80 } else {
81 wxDateTime now = wxDateTime::Now();
82 wxDateTime utc = now.ToUTC();
83 wxString time = utc.Format("%H%M%S");
84 nmea0183.Rmc.UTCTime = time;
85 wxString date = utc.Format("%d%m%y");
86 nmea0183.Rmc.Date = date;
87 }
88
89 nmea0183.Rmc.FAAModeIndicator = "A";
90 if (!bGPSValid) nmea0183.Rmc.FAAModeIndicator = "N";
91
92 nmea0183.Rmc.Write(snt);
93
94 BroadcastNMEA0183Message(snt.Sentence, GetNmeaLog(),
95 routeman.GetMessageSentEventVar());
96}
97
98static void SendDummyRmb(NMEA0183 &nmea0183, Routeman &routeman) {
99 nmea0183.Rmb.IsDataValid = NTrue;
100 nmea0183.Rmb.CrossTrackError = 0;
101 nmea0183.Rmb.DirectionToSteer = Left;
102 nmea0183.Rmb.RangeToDestinationNauticalMiles = 0;
103 nmea0183.Rmb.BearingToDestinationDegreesTrue = 0;
104 nmea0183.Rmb.DestinationPosition.Latitude.Set(0, "N");
105 nmea0183.Rmb.DestinationPosition.Longitude.Set(0, "E");
106 nmea0183.Rmb.DestinationClosingVelocityKnots = 0;
107 nmea0183.Rmb.IsArrivalCircleEntered = NFalse;
108 nmea0183.Rmb.FAAModeIndicator = "A";
109 nmea0183.Rmb.To = "";
110 nmea0183.Rmb.From = "";
111
112 SENTENCE snt;
113 nmea0183.Rmb.Write(snt);
114 BroadcastNMEA0183Message(snt.Sentence, GetNmeaLog(),
115 routeman.GetMessageSentEventVar());
116}
117
118static void SendRmb(NMEA0183 &nmea0183, Routeman &routeman) {
119 SENTENCE snt;
120 RoutePoint *pActivePoint = routeman.GetpActivePoint();
121 const int maxName = 6;
122
123 nmea0183.Rmb.IsDataValid = bGPSValid ? NTrue : NFalse;
124 nmea0183.Rmb.CrossTrackError = routeman.GetCurrentXTEToActivePoint();
125 nmea0183.Rmb.DirectionToSteer = routeman.GetXTEDir() < 0 ? Left : Right;
126 nmea0183.Rmb.RangeToDestinationNauticalMiles =
127 routeman.GetCurrentRngToActivePoint();
128 nmea0183.Rmb.BearingToDestinationDegreesTrue =
129 routeman.GetCurrentBrgToActivePoint();
130
131 using std::fabs;
132 const char *ns = pActivePoint->m_lat < 0 ? "S" : "N";
133 nmea0183.Rmb.DestinationPosition.Latitude.Set(fabs(pActivePoint->m_lat), ns);
134 const char *ew = pActivePoint->m_lon < 0 ? "W" : "E";
135 nmea0183.Rmb.DestinationPosition.Longitude.Set(fabs(pActivePoint->m_lon), ew);
136
137 double sog = std::isnan(gSog) ? 0.0 : gSog;
138 double cog = std::isnan(gCog) ? 0.0 : gCog;
139 nmea0183.Rmb.DestinationClosingVelocityKnots =
140 sog * cos((cog - routeman.GetCurrentBrgToActivePoint()) * PI / 180.0);
141 nmea0183.Rmb.IsArrivalCircleEntered = routeman.GetArrival() ? NTrue : NFalse;
142 nmea0183.Rmb.FAAModeIndicator = bGPSValid ? "A" : "N";
143 // RMB is close to NMEA0183 length limit
144 // Restrict WP names further if necessary
145 int wp_len = maxName;
146 do {
147 nmea0183.Rmb.To = pActivePoint->GetName().Truncate(wp_len);
148 nmea0183.Rmb.From =
149 routeman.GetpActiveRouteSegmentBeginPoint()->GetName().Truncate(wp_len);
150 nmea0183.Rmb.Write(snt);
151 wp_len -= 1;
152 } while (snt.Sentence.size() > 82 && wp_len > 0);
153
154 BroadcastNMEA0183Message(snt.Sentence, GetNmeaLog(),
155 routeman.GetMessageSentEventVar());
156}
157
159 if (routeman.GetpActivePoint()) return false;
160 NMEA0183 nmea0183 = routeman.GetNMEA0183();
161 SendRmc(nmea0183, routeman);
162 SendDummyRmb(nmea0183, routeman);
163 return true;
164}
165
166bool UpdateAutopilotN0183(Routeman &routeman) {
167 NMEA0183 nmea0183 = routeman.GetNMEA0183();
168 RoutePoint *pActivePoint = routeman.GetpActivePoint();
169
170 // Set max WP name length
171 int maxName = 6;
172 if ((g_maxWPNameLength >= 3) && (g_maxWPNameLength <= 32))
173 maxName = g_maxWPNameLength;
174
175 SendRmb(nmea0183, routeman);
176 SendRmc(nmea0183, routeman);
177
178 // APB
179 {
180 SENTENCE snt;
181
182 nmea0183.Apb.IsLoranBlinkOK =
183 NTrue; // considered as "generic invalid fix" flag
184 if (!bGPSValid) nmea0183.Apb.IsLoranBlinkOK = NFalse;
185
186 nmea0183.Apb.IsLoranCCycleLockOK = NTrue;
187 if (!bGPSValid) nmea0183.Apb.IsLoranCCycleLockOK = NFalse;
188
189 nmea0183.Apb.CrossTrackErrorMagnitude =
190 routeman.GetCurrentXTEToActivePoint();
191
192 if (routeman.GetXTEDir() < 0)
193 nmea0183.Apb.DirectionToSteer = Left;
194 else
195 nmea0183.Apb.DirectionToSteer = Right;
196
197 nmea0183.Apb.CrossTrackUnits = "N";
198
199 if (routeman.GetArrival())
200 nmea0183.Apb.IsArrivalCircleEntered = NTrue;
201 else
202 nmea0183.Apb.IsArrivalCircleEntered = NFalse;
203
204 // We never pass the perpendicular, since we declare arrival before
205 // reaching this point
206 nmea0183.Apb.IsPerpendicular = NFalse;
207
208 nmea0183.Apb.To = pActivePoint->GetName().Truncate(maxName);
209
210 double brg1, dist1;
211 DistanceBearingMercator(pActivePoint->m_lat, pActivePoint->m_lon,
212 routeman.GetpActiveRouteSegmentBeginPoint()->m_lat,
213 routeman.GetpActiveRouteSegmentBeginPoint()->m_lon,
214 &brg1, &dist1);
215
216 if (g_bMagneticAPB && !std::isnan(gVar)) {
217 double brg1m =
218 ((brg1 - gVar) >= 0.) ? (brg1 - gVar) : (brg1 - gVar + 360.);
219 double bapm = ((routeman.GetCurrentBrgToActivePoint() - gVar) >= 0.)
220 ? (routeman.GetCurrentBrgToActivePoint() - gVar)
221 : (routeman.GetCurrentBrgToActivePoint() - gVar + 360.);
222
223 nmea0183.Apb.BearingOriginToDestination = brg1m;
224 nmea0183.Apb.BearingOriginToDestinationUnits = "M";
225
226 nmea0183.Apb.BearingPresentPositionToDestination = bapm;
227 nmea0183.Apb.BearingPresentPositionToDestinationUnits = "M";
228
229 nmea0183.Apb.HeadingToSteer = bapm;
230 nmea0183.Apb.HeadingToSteerUnits = "M";
231 } else {
232 nmea0183.Apb.BearingOriginToDestination = brg1;
233 nmea0183.Apb.BearingOriginToDestinationUnits = "T";
234
235 nmea0183.Apb.BearingPresentPositionToDestination =
236 routeman.GetCurrentBrgToActivePoint();
237 nmea0183.Apb.BearingPresentPositionToDestinationUnits = "T";
238
239 nmea0183.Apb.HeadingToSteer = routeman.GetCurrentBrgToActivePoint();
240 nmea0183.Apb.HeadingToSteerUnits = "T";
241 }
242
243 nmea0183.Apb.Write(snt);
244 BroadcastNMEA0183Message(snt.Sentence, GetNmeaLog(),
245 routeman.GetMessageSentEventVar());
246 }
247
248 // XTE
249 {
250 SENTENCE snt;
251
252 nmea0183.Xte.IsLoranBlinkOK =
253 NTrue; // considered as "generic invalid fix" flag
254 if (!bGPSValid) nmea0183.Xte.IsLoranBlinkOK = NFalse;
255
256 nmea0183.Xte.IsLoranCCycleLockOK = NTrue;
257 if (!bGPSValid) nmea0183.Xte.IsLoranCCycleLockOK = NFalse;
258
259 nmea0183.Xte.CrossTrackErrorDistance =
260 routeman.GetCurrentXTEToActivePoint();
261
262 if (routeman.GetXTEDir() < 0)
263 nmea0183.Xte.DirectionToSteer = Left;
264 else
265 nmea0183.Xte.DirectionToSteer = Right;
266
267 nmea0183.Xte.CrossTrackUnits = "N";
268
269 nmea0183.Xte.Write(snt);
270 BroadcastNMEA0183Message(snt.Sentence, GetNmeaLog(),
271 routeman.GetMessageSentEventVar());
272 }
273
274 return true;
275}
276
277bool UpdateAutopilotN2K(Routeman &routeman) {
278 bool fail_any = false;
279
280 // Get a suitable N2K Driver
281 auto &registry = CommDriverRegistry::GetInstance();
282 const std::vector<DriverPtr> &drivers = registry.GetDrivers();
283
284 AbstractCommDriver *found = nullptr;
285 for (auto key : routeman.GetOutpuDriverArray()) {
286 for (auto &d : drivers) {
287 if (d->Key() == key) {
288 std::unordered_map<std::string, std::string> attributes =
289 GetAttributes(key);
290 auto protocol_it = attributes.find("protocol");
291 if (protocol_it != attributes.end()) {
292 std::string protocol = protocol_it->second;
293
294 if (protocol == "nmea2000") {
295 found = d.get();
296 }
297 }
298 }
299 }
300 }
301 if (!found) return false;
302
303 // N2K serial drivers require maintenance of an enabled PGN TX list
304 auto drv_serial = dynamic_cast<CommDriverN2KSerial *>(found);
305 if (drv_serial) {
306 drv_serial->AddTxPGN(129283);
307 drv_serial->AddTxPGN(129284);
308 drv_serial->AddTxPGN(129285);
309 }
310 if (routeman.IsAnyRouteActive()) {
311 fail_any |= !SendPGN129285(routeman, found);
312 fail_any |= !SendPGN129284(routeman, found);
313 fail_any |= !SendPGN129283(routeman, found);
314 }
315
316 return (fail_any == 0);
317}
318
319bool SendPGN129285(Routeman &routeman, AbstractCommDriver *driver) {
320 bool fail_any = false;
321
322 // Set max WP name length
323 int maxName = 6;
324 if ((g_maxWPNameLength >= 3) && (g_maxWPNameLength <= 32))
325 maxName = g_maxWPNameLength;
326
327 // Create and transmit PGN 129285
328 // The basic PGN129285 head
329 tN2kMsg msg129285;
330 char route_name[] = "Route";
331 SetN2kPGN129285(msg129285, 0, 0, 0, N2kdir_forward, 0, route_name);
332
333 // Append start point of current leg
334 RoutePoint *pLegBeginPoint = routeman.GetpActiveRouteSegmentBeginPoint();
335 wxString start_point_name = pLegBeginPoint->GetName().Truncate(maxName);
336 std::string sname = start_point_name.ToStdString();
337 char *s = (char *)sname.c_str();
338
339 fail_any |= !AppendN2kPGN129285(msg129285, 0, s, pLegBeginPoint->m_lat,
340 pLegBeginPoint->m_lon);
341 // Append destination point of current leg
342 RoutePoint *pActivePoint = routeman.GetpActivePoint();
343 wxString destination_name = pActivePoint->GetName().Truncate(maxName);
344 std::string dname = destination_name.ToStdString();
345 char *d = (char *)dname.c_str();
346 fail_any |= !AppendN2kPGN129285(msg129285, 1, d, pActivePoint->m_lat,
347 pActivePoint->m_lon);
348
349 if (fail_any) return false;
350
351 auto dest_addr = std::make_shared<const NavAddr2000>(driver->iface, 255);
352 std::vector<uint8_t> payload;
353 for (int i = 0; i < msg129285.DataLen; i++)
354 payload.push_back(msg129285.Data[i]);
355 auto PGN129285 =
356 std::make_shared<const Nmea2000Msg>(129285, payload, dest_addr, 6);
357 fail_any |= !driver->SendMessage(PGN129285, dest_addr);
358
359 return (fail_any == 0);
360}
361
362bool SendPGN129284(Routeman &routeman, AbstractCommDriver *driver) {
363 bool fail_any = false;
364 tN2kMsg msg129284;
365 RoutePoint *pActivePoint = routeman.GetpActivePoint();
366
367 // Calculate closing velocity and ETA
368 double vmg = 0.;
369 if (!std::isnan(gCog) && !std::isnan(gSog)) {
370 double brg = routeman.GetCurrentBrgToActivePoint();
371 vmg = gSog * cos((brg - gCog) * PI / 180.);
372 }
373 wxTimeSpan tttg_span;
374 wxDateTime arrival_time = wxDateTime::Now();
375 if (vmg > 0.) {
376 double tttg_sec = (routeman.GetCurrentRngToActivePoint() / gSog) * 3600;
377 tttg_span = wxTimeSpan::Seconds((long)tttg_sec);
378 arrival_time += tttg_span;
379 }
380 double time_days_1979 = arrival_time.GetTicks() / (3600. * 24.);
381
382 // ETA time_seconds, expressed as seconds since midnight
383 // ETA date, expressed as whole days since 1 January 1970
384 double eta_time_days;
385 double eta_time_seconds = modf(time_days_1979, &eta_time_days);
386 int16_t eta_time_days_16 = static_cast<uint16_t>(eta_time_days);
387
388 SetN2kPGN129284(
389 msg129284,
390 0xFF, // SID
391 routeman.GetCurrentRngToActivePoint() * 1852., // DistanceToWaypoint
392 N2khr_true, // tN2kHeadingReference BearingReference
393 false, // PerpendicularCrossed
394 false, // ArrivalCircleEntered
395 N2kdct_RhumbLine, // tN2kDistanceCalculationType CalculationType
396 eta_time_seconds, // double ETATime,
397 eta_time_days_16, // int16_t ETADate,
398 routeman.GetCurrentSegmentCourse() * PI /
399 180., // BearingOriginToDestinationWaypoint,
400 routeman.GetCurrentBrgToActivePoint() * PI /
401 180., // BearingPositionToDestinationWaypoint,
402 0, // uint8_t OriginWaypointNumber,
403 1, // uint8_t DestinationWaypointNumber,
404 pActivePoint->m_lat, // double DestinationLatitude,
405 pActivePoint->m_lon, // double DestinationLongitude,
406 vmg); // double WaypointClosingVelocity);
407
408 auto dest_addr = std::make_shared<const NavAddr2000>(driver->iface, 255);
409 std::vector<uint8_t> payload;
410 for (int i = 0; i < msg129284.DataLen; i++)
411 payload.push_back(msg129284.Data[i]);
412 auto PGN129284 =
413 std::make_shared<const Nmea2000Msg>(129284, payload, dest_addr, 6);
414 fail_any |= !driver->SendMessage(PGN129284, dest_addr);
415
416 return (fail_any == 0);
417}
418
419bool SendPGN129283(Routeman &routeman, AbstractCommDriver *driver) {
420 bool fail_any = false;
421 tN2kMsg msg129283;
422 RoutePoint *pActivePoint = routeman.GetpActivePoint();
423 // N2K, distance units are metres, therefore convert from Nm
424 double xte = routeman.GetCurrentXTEToActivePoint() * 1852.;
425 // N2K, -ve xte, means to the left of the course, steer right, Reverse of
426 // current Routeman logic
427 if (routeman.GetXTEDir() > 0) {
428 xte = -xte;
429 }
430 SetN2kPGN129283(msg129283, 0,
431 N2kxtem_Autonomous, // tN2kXTEMode XTEMode,
432 false, // bool NavigationTerminated,
433 xte // double XTE
434 );
435
436 auto dest_addr = std::make_shared<const NavAddr2000>(driver->iface, 255);
437 std::vector<uint8_t> payload;
438 for (int i = 0; i < msg129283.DataLen; i++)
439 payload.push_back(msg129283.Data[i]);
440 auto PGN129283 =
441 std::make_shared<const Nmea2000Msg>(129283, payload, dest_addr, 6);
442 fail_any |= !driver->SendMessage(PGN129283, dest_addr);
443
444 return (fail_any == 0);
445}
bool SendNoRouteRmbRmc(Routeman &routeman)
Send RMC + a faked RMB when there is no active route.
Autopilot output support.
Common interface for all drivers.
Definition comm_driver.h:65
const std::string iface
Physical device for 0183, else a unique string.
Definition comm_driver.h:95
NMEA Log Interface.
Definition nmea_log.h:70
Represents a waypoint or mark within the navigation system.
Definition route_point.h:71
Nmea2000 serial driver.
Driver registration container, a singleton.
NMEA Data Multiplexer Object.
Variables maintained by comm stack, read-only access for others.
Global variables stored in configuration file.
OpenCPN Georef utility.
Hooks into gui available in model.
Wrapper for creating an NmeaContext based on global vars.
Basic DataMonitor logging interface: LogLine (reflects a line in the log) and NmeaLog,...
Global variables Listen()/Notify() wrapper.
bool bGPSValid
Indicate whether the Global Navigation Satellite System (GNSS) has a valid position.
Definition own_ship.cpp:34
double gVar
Magnetic variation in degrees.
Definition own_ship.cpp:32
double gLat
Vessel's current latitude in decimal degrees.
Definition own_ship.cpp:26
double gCog
Course over ground in degrees (0-359.99).
Definition own_ship.cpp:28
double gSog
Speed over ground in knots.
Definition own_ship.cpp:29
double gLon
Vessel's current longitude in decimal degrees.
Definition own_ship.cpp:27
Position, course, speed, etc.
const std::unordered_map< std::string, std::string > GetAttributes(DriverHandle handle)
Query a specific driver for attributes.
Route Manager.