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