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 SENTENCE snt;
77 nmea0183.Rmb.IsDataValid = bGPSValid ? NTrue : NFalse;
78 nmea0183.Rmb.CrossTrackError = routeman.GetCurrentXTEToActivePoint();
79 nmea0183.Rmb.DirectionToSteer = routeman.GetXTEDir() < 0 ? Left : Right;
80 nmea0183.Rmb.RangeToDestinationNauticalMiles =
81 routeman.GetCurrentRngToActivePoint();
82 nmea0183.Rmb.BearingToDestinationDegreesTrue =
83 routeman.GetCurrentBrgToActivePoint();
84
85 if (pActivePoint->m_lat < 0.)
86 nmea0183.Rmb.DestinationPosition.Latitude.Set(-pActivePoint->m_lat, "S");
87 else
88 nmea0183.Rmb.DestinationPosition.Latitude.Set(pActivePoint->m_lat, "N");
89
90 if (pActivePoint->m_lon < 0.)
91 nmea0183.Rmb.DestinationPosition.Longitude.Set(-pActivePoint->m_lon, "W");
92 else
93 nmea0183.Rmb.DestinationPosition.Longitude.Set(pActivePoint->m_lon, "E");
94
95 nmea0183.Rmb.DestinationClosingVelocityKnots =
96 r_Sog *
97 cos((r_Cog - routeman.GetCurrentBrgToActivePoint()) * PI / 180.0);
98 nmea0183.Rmb.IsArrivalCircleEntered =
99 routeman.GetArrival() ? NTrue : NFalse;
100 nmea0183.Rmb.FAAModeIndicator = bGPSValid ? "A" : "N";
101 // RMB is close to NMEA0183 length limit
102 // Restrict WP names further if necessary
103 int wp_len = maxName;
104 do {
105 nmea0183.Rmb.To = pActivePoint->GetName().Truncate(wp_len);
106 nmea0183.Rmb.From =
107 routeman.GetpActiveRouteSegmentBeginPoint()->GetName().Truncate(
108 wp_len);
109 nmea0183.Rmb.Write(snt);
110 wp_len -= 1;
111 } while (snt.Sentence.size() > 82 && wp_len > 0);
112
113 BroadcastNMEA0183Message(snt.Sentence, GetNmeaLog(),
114 routeman.GetMessageSentEventVar());
115 }
116
117 // RMC
118 {
119 SENTENCE snt;
120 nmea0183.Rmc.IsDataValid = NTrue;
121 if (!bGPSValid) nmea0183.Rmc.IsDataValid = NFalse;
122
123 if (gLat < 0.)
124 nmea0183.Rmc.Position.Latitude.Set(-gLat, _T("S"));
125 else
126 nmea0183.Rmc.Position.Latitude.Set(gLat, _T("N"));
127
128 if (gLon < 0.)
129 nmea0183.Rmc.Position.Longitude.Set(-gLon, _T("W"));
130 else
131 nmea0183.Rmc.Position.Longitude.Set(gLon, _T("E"));
132
133 nmea0183.Rmc.SpeedOverGroundKnots = r_Sog;
134 nmea0183.Rmc.TrackMadeGoodDegreesTrue = r_Cog;
135
136 if (!std::isnan(gVar)) {
137 if (gVar < 0.) {
138 nmea0183.Rmc.MagneticVariation = -gVar;
139 nmea0183.Rmc.MagneticVariationDirection = West;
140 } else {
141 nmea0183.Rmc.MagneticVariation = gVar;
142 nmea0183.Rmc.MagneticVariationDirection = East;
143 }
144 } else
145 nmea0183.Rmc.MagneticVariation =
146 361.; // A signal to NMEA converter, gVAR is unknown
147
148 // Send GPS time to autopilot if available else send local system time
149 if (!gRmcTime.IsEmpty() && !gRmcDate.IsEmpty()) {
150 nmea0183.Rmc.UTCTime = gRmcTime;
151 nmea0183.Rmc.Date = gRmcDate;
152 } else {
153 wxDateTime now = wxDateTime::Now();
154 wxDateTime utc = now.ToUTC();
155 wxString time = utc.Format(_T("%H%M%S"));
156 nmea0183.Rmc.UTCTime = time;
157 wxString date = utc.Format(_T("%d%m%y"));
158 nmea0183.Rmc.Date = date;
159 }
160
161 nmea0183.Rmc.FAAModeIndicator = "A";
162 if (!bGPSValid) nmea0183.Rmc.FAAModeIndicator = "N";
163
164 nmea0183.Rmc.Write(snt);
165
166 BroadcastNMEA0183Message(snt.Sentence, GetNmeaLog(),
167 routeman.GetMessageSentEventVar());
168 }
169
170 // APB
171 {
172 SENTENCE snt;
173
174 nmea0183.Apb.IsLoranBlinkOK =
175 NTrue; // considered as "generic invalid fix" flag
176 if (!bGPSValid) nmea0183.Apb.IsLoranBlinkOK = NFalse;
177
178 nmea0183.Apb.IsLoranCCycleLockOK = NTrue;
179 if (!bGPSValid) nmea0183.Apb.IsLoranCCycleLockOK = NFalse;
180
181 nmea0183.Apb.CrossTrackErrorMagnitude =
182 routeman.GetCurrentXTEToActivePoint();
183
184 if (routeman.GetXTEDir() < 0)
185 nmea0183.Apb.DirectionToSteer = Left;
186 else
187 nmea0183.Apb.DirectionToSteer = Right;
188
189 nmea0183.Apb.CrossTrackUnits = _T("N");
190
191 if (routeman.GetArrival())
192 nmea0183.Apb.IsArrivalCircleEntered = NTrue;
193 else
194 nmea0183.Apb.IsArrivalCircleEntered = NFalse;
195
196 // We never pass the perpendicular, since we declare arrival before
197 // reaching this point
198 nmea0183.Apb.IsPerpendicular = NFalse;
199
200 nmea0183.Apb.To = pActivePoint->GetName().Truncate(maxName);
201
202 double brg1, dist1;
203 DistanceBearingMercator(pActivePoint->m_lat, pActivePoint->m_lon,
204 routeman.GetpActiveRouteSegmentBeginPoint()->m_lat,
205 routeman.GetpActiveRouteSegmentBeginPoint()->m_lon,
206 &brg1, &dist1);
207
208 if (g_bMagneticAPB && !std::isnan(gVar)) {
209 double brg1m =
210 ((brg1 - gVar) >= 0.) ? (brg1 - gVar) : (brg1 - gVar + 360.);
211 double bapm = ((routeman.GetCurrentBrgToActivePoint() - gVar) >= 0.)
212 ? (routeman.GetCurrentBrgToActivePoint() - gVar)
213 : (routeman.GetCurrentBrgToActivePoint() - gVar + 360.);
214
215 nmea0183.Apb.BearingOriginToDestination = brg1m;
216 nmea0183.Apb.BearingOriginToDestinationUnits = _T("M");
217
218 nmea0183.Apb.BearingPresentPositionToDestination = bapm;
219 nmea0183.Apb.BearingPresentPositionToDestinationUnits = _T("M");
220
221 nmea0183.Apb.HeadingToSteer = bapm;
222 nmea0183.Apb.HeadingToSteerUnits = _T("M");
223 } else {
224 nmea0183.Apb.BearingOriginToDestination = brg1;
225 nmea0183.Apb.BearingOriginToDestinationUnits = _T("T");
226
227 nmea0183.Apb.BearingPresentPositionToDestination =
228 routeman.GetCurrentBrgToActivePoint();
229 nmea0183.Apb.BearingPresentPositionToDestinationUnits = _T("T");
230
231 nmea0183.Apb.HeadingToSteer = routeman.GetCurrentBrgToActivePoint();
232 nmea0183.Apb.HeadingToSteerUnits = _T("T");
233 }
234
235 nmea0183.Apb.Write(snt);
236 BroadcastNMEA0183Message(snt.Sentence, GetNmeaLog(),
237 routeman.GetMessageSentEventVar());
238 }
239
240 // XTE
241 {
242 SENTENCE snt;
243
244 nmea0183.Xte.IsLoranBlinkOK =
245 NTrue; // considered as "generic invalid fix" flag
246 if (!bGPSValid) nmea0183.Xte.IsLoranBlinkOK = NFalse;
247
248 nmea0183.Xte.IsLoranCCycleLockOK = NTrue;
249 if (!bGPSValid) nmea0183.Xte.IsLoranCCycleLockOK = NFalse;
250
251 nmea0183.Xte.CrossTrackErrorDistance =
252 routeman.GetCurrentXTEToActivePoint();
253
254 if (routeman.GetXTEDir() < 0)
255 nmea0183.Xte.DirectionToSteer = Left;
256 else
257 nmea0183.Xte.DirectionToSteer = Right;
258
259 nmea0183.Xte.CrossTrackUnits = _T("N");
260
261 nmea0183.Xte.Write(snt);
262 BroadcastNMEA0183Message(snt.Sentence, GetNmeaLog(),
263 routeman.GetMessageSentEventVar());
264 }
265
266 return true;
267}
268
269bool UpdateAutopilotN2K(Routeman &routeman) {
270 bool fail_any = false;
271
272 // Get a suitable N2K Driver
273 auto &registry = CommDriverRegistry::GetInstance();
274 const std::vector<DriverPtr> &drivers = registry.GetDrivers();
275
276 AbstractCommDriver *found = nullptr;
277 for (auto key : routeman.GetOutpuDriverArray()) {
278 for (auto &d : drivers) {
279 if (d->Key() == key) {
280 std::unordered_map<std::string, std::string> attributes =
281 GetAttributes(key);
282 auto protocol_it = attributes.find("protocol");
283 if (protocol_it != attributes.end()) {
284 std::string protocol = protocol_it->second;
285
286 if (protocol == "nmea2000") {
287 found = d.get();
288 }
289 }
290 }
291 }
292 }
293 if (!found) return false;
294
295 // N2K serial drivers require maintenance of an enabled PGN TX list
296 auto drv_serial = dynamic_cast<CommDriverN2KSerial *>(found);
297 if (drv_serial) {
298 drv_serial->AddTxPGN(129283);
299 drv_serial->AddTxPGN(129284);
300 drv_serial->AddTxPGN(129285);
301 }
302
303 fail_any |= !SendPGN129285(routeman, found);
304 fail_any |= !SendPGN129284(routeman, found);
305 fail_any |= !SendPGN129283(routeman, found);
306
307 return (fail_any == 0);
308}
309
310bool SendPGN129285(Routeman &routeman, AbstractCommDriver *driver) {
311 bool fail_any = false;
312
313 // Set max WP name length
314 int maxName = 6;
315 if ((g_maxWPNameLength >= 3) && (g_maxWPNameLength <= 32))
316 maxName = g_maxWPNameLength;
317
318 // Create and transmit PGN 129285
319 // The basic PGN129285 head
320 tN2kMsg msg129285;
321 char route_name[] = "Route";
322 SetN2kPGN129285(msg129285, 0, 0, 0, N2kdir_forward, 0, route_name);
323
324 // Append start point of current leg
325 RoutePoint *pLegBeginPoint = routeman.GetpActiveRouteSegmentBeginPoint();
326 wxString start_point_name = pLegBeginPoint->GetName().Truncate(maxName);
327 std::string sname = start_point_name.ToStdString();
328 char *s = (char *)sname.c_str();
329
330 fail_any |= !AppendN2kPGN129285(msg129285, 0, s, pLegBeginPoint->m_lat,
331 pLegBeginPoint->m_lon);
332 // Append destination point of current leg
333 RoutePoint *pActivePoint = routeman.GetpActivePoint();
334 wxString destination_name = pActivePoint->GetName().Truncate(maxName);
335 std::string dname = destination_name.ToStdString();
336 char *d = (char *)dname.c_str();
337 fail_any |= !AppendN2kPGN129285(msg129285, 1, d, pActivePoint->m_lat,
338 pActivePoint->m_lon);
339
340 if (fail_any) return false;
341
342 auto dest_addr = std::make_shared<const NavAddr2000>(driver->iface, 255);
343 std::vector<uint8_t> payload;
344 for (int i = 0; i < msg129285.DataLen; i++)
345 payload.push_back(msg129285.Data[i]);
346 auto PGN129285 =
347 std::make_shared<const Nmea2000Msg>(129285, payload, dest_addr, 6);
348 fail_any |= !driver->SendMessage(PGN129285, dest_addr);
349
350 return (fail_any == 0);
351}
352
353bool SendPGN129284(Routeman &routeman, AbstractCommDriver *driver) {
354 bool fail_any = false;
355 tN2kMsg msg129284;
356 RoutePoint *pActivePoint = routeman.GetpActivePoint();
357
358 // Calculate closing velocity and ETA
359 double vmg = 0.;
360 if (!std::isnan(gCog) && !std::isnan(gSog)) {
361 double brg = routeman.GetCurrentBrgToActivePoint();
362 vmg = gSog * cos((brg - gCog) * PI / 180.);
363 }
364 wxTimeSpan tttg_span;
365 wxDateTime arrival_time = wxDateTime::Now();
366 if (vmg > 0.) {
367 double tttg_sec = (routeman.GetCurrentRngToActivePoint() / gSog) * 3600;
368 tttg_span = wxTimeSpan::Seconds((long)tttg_sec);
369 arrival_time += tttg_span;
370 }
371 double time_days_1979 = arrival_time.GetTicks() / (3600. * 24.);
372
373 // ETA time_seconds, expressed as seconds since midnight
374 // ETA date, expressed as whole days since 1 January 1970
375 double eta_time_days;
376 double eta_time_seconds = modf(time_days_1979, &eta_time_days);
377 int16_t eta_time_days_16 = static_cast<uint16_t>(eta_time_days);
378
379 SetN2kPGN129284(
380 msg129284,
381 0xFF, // SID
382 routeman.GetCurrentRngToActivePoint() * 1852., // DistanceToWaypoint
383 N2khr_true, // tN2kHeadingReference BearingReference
384 false, // PerpendicularCrossed
385 false, // ArrivalCircleEntered
386 N2kdct_RhumbLine, // tN2kDistanceCalculationType CalculationType
387 eta_time_seconds, // double ETATime,
388 eta_time_days_16, // int16_t ETADate,
389 routeman.GetCurrentSegmentCourse() * PI /
390 180., // BearingOriginToDestinationWaypoint,
391 routeman.GetCurrentBrgToActivePoint() * PI /
392 180., // BearingPositionToDestinationWaypoint,
393 0, // uint8_t OriginWaypointNumber,
394 1, // uint8_t DestinationWaypointNumber,
395 pActivePoint->m_lat, // double DestinationLatitude,
396 pActivePoint->m_lon, // double DestinationLongitude,
397 vmg); // double WaypointClosingVelocity);
398
399 auto dest_addr = std::make_shared<const NavAddr2000>(driver->iface, 255);
400 std::vector<uint8_t> payload;
401 for (int i = 0; i < msg129284.DataLen; i++)
402 payload.push_back(msg129284.Data[i]);
403 auto PGN129284 =
404 std::make_shared<const Nmea2000Msg>(129284, payload, dest_addr, 6);
405 fail_any |= !driver->SendMessage(PGN129284, dest_addr);
406
407 return (fail_any == 0);
408}
409
410bool SendPGN129283(Routeman &routeman, AbstractCommDriver *driver) {
411 bool fail_any = false;
412 tN2kMsg msg129283;
413 RoutePoint *pActivePoint = routeman.GetpActivePoint();
414 // N2K, distance units are metres, therefore convert from Nm
415 double xte = routeman.GetCurrentXTEToActivePoint() * 1852.;
416 // N2K, -ve xte, means to the left of the course, steer right, Reverse of
417 // current Routeman logic
418 if (routeman.GetXTEDir() > 0) {
419 xte = -xte;
420 }
421 SetN2kPGN129283(msg129283, 0,
422 N2kxtem_Autonomous, // tN2kXTEMode XTEMode,
423 false, // bool NavigationTerminated,
424 xte // double XTE
425 );
426
427 auto dest_addr = std::make_shared<const NavAddr2000>(driver->iface, 255);
428 std::vector<uint8_t> payload;
429 for (int i = 0; i < msg129283.DataLen; i++)
430 payload.push_back(msg129283.Data[i]);
431 auto PGN129283 =
432 std::make_shared<const Nmea2000Msg>(129283, payload, dest_addr, 6);
433 fail_any |= !driver->SendMessage(PGN129283, dest_addr);
434
435 return (fail_any == 0);
436}
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.