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"
39#include "model/nmea_ctx_factory.h"
40#include "model/nmea_log.h"
41#include "model/own_ship.h"
42#include "model/routeman.h"
44#include "N2kMessages.h"
46#include "observable_globvar.h"
49#include "androidUTIL.h"
53 auto w = wxWindow::FindWindowByName(kDataMonitorWindowName);
54 auto log =
dynamic_cast<NmeaLog *
>(w);
59bool UpdateAutopilotN0183(
Routeman &routeman) {
60 NMEA0183 nmea0183 = routeman.GetNMEA0183();
61 RoutePoint *pActivePoint = routeman.GetpActivePoint();
65 if ((g_maxWPNameLength >= 3) && (g_maxWPNameLength <= 32))
66 maxName = g_maxWPNameLength;
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;
76 nmea0183.TalkerID =
"EC";
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();
86 if (pActivePoint->m_lat < 0.)
87 nmea0183.Rmb.DestinationPosition.Latitude.Set(-pActivePoint->m_lat,
"S");
89 nmea0183.Rmb.DestinationPosition.Latitude.Set(pActivePoint->m_lat,
"N");
91 if (pActivePoint->m_lon < 0.)
92 nmea0183.Rmb.DestinationPosition.Longitude.Set(-pActivePoint->m_lon,
"W");
94 nmea0183.Rmb.DestinationPosition.Longitude.Set(pActivePoint->m_lon,
"E");
96 nmea0183.Rmb.DestinationClosingVelocityKnots =
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";
104 int wp_len = maxName;
106 nmea0183.Rmb.To = pActivePoint->GetName().Truncate(wp_len);
108 routeman.GetpActiveRouteSegmentBeginPoint()->GetName().Truncate(
110 nmea0183.Rmb.Write(snt);
112 }
while (snt.Sentence.size() > 82 && wp_len > 0);
114 BroadcastNMEA0183Message(snt.Sentence, GetNmeaLog(),
115 routeman.GetMessageSentEventVar());
120 nmea0183.TalkerID = _T(
"EC");
123 nmea0183.Rmc.IsDataValid = NTrue;
124 if (!bGPSValid) nmea0183.Rmc.IsDataValid = NFalse;
127 nmea0183.Rmc.Position.Latitude.Set(-gLat, _T(
"S"));
129 nmea0183.Rmc.Position.Latitude.Set(gLat, _T(
"N"));
132 nmea0183.Rmc.Position.Longitude.Set(-gLon, _T(
"W"));
134 nmea0183.Rmc.Position.Longitude.Set(gLon, _T(
"E"));
136 nmea0183.Rmc.SpeedOverGroundKnots = r_Sog;
137 nmea0183.Rmc.TrackMadeGoodDegreesTrue = r_Cog;
139 if (!std::isnan(gVar)) {
141 nmea0183.Rmc.MagneticVariation = -gVar;
142 nmea0183.Rmc.MagneticVariationDirection = West;
144 nmea0183.Rmc.MagneticVariation = gVar;
145 nmea0183.Rmc.MagneticVariationDirection = East;
148 nmea0183.Rmc.MagneticVariation =
152 if (!gRmcTime.IsEmpty() && !gRmcDate.IsEmpty()) {
153 nmea0183.Rmc.UTCTime = gRmcTime;
154 nmea0183.Rmc.Date = gRmcDate;
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;
164 nmea0183.Rmc.FAAModeIndicator =
"A";
165 if (!bGPSValid) nmea0183.Rmc.FAAModeIndicator =
"N";
167 nmea0183.Rmc.Write(snt);
169 BroadcastNMEA0183Message(snt.Sentence, GetNmeaLog(),
170 routeman.GetMessageSentEventVar());
175 nmea0183.TalkerID = _T(
"EC");
179 nmea0183.Apb.IsLoranBlinkOK =
181 if (!bGPSValid) nmea0183.Apb.IsLoranBlinkOK = NFalse;
183 nmea0183.Apb.IsLoranCCycleLockOK = NTrue;
184 if (!bGPSValid) nmea0183.Apb.IsLoranCCycleLockOK = NFalse;
186 nmea0183.Apb.CrossTrackErrorMagnitude =
187 routeman.GetCurrentXTEToActivePoint();
189 if (routeman.GetXTEDir() < 0)
190 nmea0183.Apb.DirectionToSteer = Left;
192 nmea0183.Apb.DirectionToSteer = Right;
194 nmea0183.Apb.CrossTrackUnits = _T(
"N");
196 if (routeman.GetArrival())
197 nmea0183.Apb.IsArrivalCircleEntered = NTrue;
199 nmea0183.Apb.IsArrivalCircleEntered = NFalse;
203 nmea0183.Apb.IsPerpendicular = NFalse;
205 nmea0183.Apb.To = pActivePoint->GetName().Truncate(maxName);
208 DistanceBearingMercator(pActivePoint->m_lat, pActivePoint->m_lon,
209 routeman.GetpActiveRouteSegmentBeginPoint()->m_lat,
210 routeman.GetpActiveRouteSegmentBeginPoint()->m_lon,
213 if (g_bMagneticAPB && !std::isnan(gVar)) {
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.);
220 nmea0183.Apb.BearingOriginToDestination = brg1m;
221 nmea0183.Apb.BearingOriginToDestinationUnits = _T(
"M");
223 nmea0183.Apb.BearingPresentPositionToDestination = bapm;
224 nmea0183.Apb.BearingPresentPositionToDestinationUnits = _T(
"M");
226 nmea0183.Apb.HeadingToSteer = bapm;
227 nmea0183.Apb.HeadingToSteerUnits = _T(
"M");
229 nmea0183.Apb.BearingOriginToDestination = brg1;
230 nmea0183.Apb.BearingOriginToDestinationUnits = _T(
"T");
232 nmea0183.Apb.BearingPresentPositionToDestination =
233 routeman.GetCurrentBrgToActivePoint();
234 nmea0183.Apb.BearingPresentPositionToDestinationUnits = _T(
"T");
236 nmea0183.Apb.HeadingToSteer = routeman.GetCurrentBrgToActivePoint();
237 nmea0183.Apb.HeadingToSteerUnits = _T(
"T");
240 nmea0183.Apb.Write(snt);
241 BroadcastNMEA0183Message(snt.Sentence, GetNmeaLog(),
242 routeman.GetMessageSentEventVar());
247 nmea0183.TalkerID = _T(
"EC");
251 nmea0183.Xte.IsLoranBlinkOK =
253 if (!bGPSValid) nmea0183.Xte.IsLoranBlinkOK = NFalse;
255 nmea0183.Xte.IsLoranCCycleLockOK = NTrue;
256 if (!bGPSValid) nmea0183.Xte.IsLoranCCycleLockOK = NFalse;
258 nmea0183.Xte.CrossTrackErrorDistance =
259 routeman.GetCurrentXTEToActivePoint();
261 if (routeman.GetXTEDir() < 0)
262 nmea0183.Xte.DirectionToSteer = Left;
264 nmea0183.Xte.DirectionToSteer = Right;
266 nmea0183.Xte.CrossTrackUnits = _T(
"N");
268 nmea0183.Xte.Write(snt);
269 BroadcastNMEA0183Message(snt.Sentence, GetNmeaLog(),
270 routeman.GetMessageSentEventVar());
276bool UpdateAutopilotN2K(
Routeman &routeman) {
277 bool fail_any =
false;
280 auto ®istry = CommDriverRegistry::GetInstance();
281 const std::vector<DriverPtr> &drivers = registry.GetDrivers();
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 =
289 auto protocol_it = attributes.find(
"protocol");
290 if (protocol_it != attributes.end()) {
291 std::string protocol = protocol_it->second;
293 if (protocol ==
"nmea2000") {
300 if (!found)
return false;
305 drv_serial->AddTxPGN(129283);
306 drv_serial->AddTxPGN(129284);
307 drv_serial->AddTxPGN(129285);
310 fail_any |= !SendPGN129285(routeman, found);
311 fail_any |= !SendPGN129284(routeman, found);
312 fail_any |= !SendPGN129283(routeman, found);
314 return (fail_any == 0);
318 bool fail_any =
false;
322 if ((g_maxWPNameLength >= 3) && (g_maxWPNameLength <= 32))
323 maxName = g_maxWPNameLength;
328 char route_name[] =
"Route";
329 SetN2kPGN129285(msg129285, 0, 0, 0, N2kdir_forward, 0, route_name);
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();
337 fail_any |= !AppendN2kPGN129285(msg129285, 0, s, pLegBeginPoint->m_lat,
338 pLegBeginPoint->m_lon);
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);
347 if (fail_any)
return false;
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]);
354 std::make_shared<const Nmea2000Msg>(129285, payload, dest_addr, 6);
355 fail_any |= !driver->SendMessage(PGN129285, dest_addr);
357 return (fail_any == 0);
361 bool fail_any =
false;
363 RoutePoint *pActivePoint = routeman.GetpActivePoint();
367 if (!std::isnan(gCog) && !std::isnan(gSog)) {
368 double brg = routeman.GetCurrentBrgToActivePoint();
369 vmg = gSog * cos((brg - gCog) * PI / 180.);
371 wxTimeSpan tttg_span;
372 wxDateTime arrival_time = wxDateTime::Now();
374 double tttg_sec = (routeman.GetCurrentRngToActivePoint() / gSog) * 3600;
375 tttg_span = wxTimeSpan::Seconds((
long)tttg_sec);
376 arrival_time += tttg_span;
378 double time_days_1979 = arrival_time.GetTicks() / (3600. * 24.);
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);
389 routeman.GetCurrentRngToActivePoint() * 1852.,
396 routeman.GetCurrentSegmentCourse() * PI /
398 routeman.GetCurrentBrgToActivePoint() * PI /
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]);
411 std::make_shared<const Nmea2000Msg>(129284, payload, dest_addr, 6);
412 fail_any |= !driver->SendMessage(PGN129284, dest_addr);
414 return (fail_any == 0);
418 bool fail_any =
false;
420 RoutePoint *pActivePoint = routeman.GetpActivePoint();
422 SetN2kPGN129283(msg129283, 0,
425 routeman.GetCurrentXTEToActivePoint()
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]);
433 std::make_shared<const Nmea2000Msg>(129283, payload, dest_addr, 6);
434 fail_any |= !driver->SendMessage(PGN129283, dest_addr);
436 return (fail_any == 0);
Common interface for all drivers.
const std::string iface
Physical device for 0183, else a unique string.
Represents a waypoint or mark within the navigation system.
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.