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;
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();
85 if (pActivePoint->m_lat < 0.)
86 nmea0183.Rmb.DestinationPosition.Latitude.Set(-pActivePoint->m_lat,
"S");
88 nmea0183.Rmb.DestinationPosition.Latitude.Set(pActivePoint->m_lat,
"N");
90 if (pActivePoint->m_lon < 0.)
91 nmea0183.Rmb.DestinationPosition.Longitude.Set(-pActivePoint->m_lon,
"W");
93 nmea0183.Rmb.DestinationPosition.Longitude.Set(pActivePoint->m_lon,
"E");
95 nmea0183.Rmb.DestinationClosingVelocityKnots =
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";
103 int wp_len = maxName;
105 nmea0183.Rmb.To = pActivePoint->GetName().Truncate(wp_len);
107 routeman.GetpActiveRouteSegmentBeginPoint()->GetName().Truncate(
109 nmea0183.Rmb.Write(snt);
111 }
while (snt.Sentence.size() > 82 && wp_len > 0);
113 BroadcastNMEA0183Message(snt.Sentence, GetNmeaLog(),
114 routeman.GetMessageSentEventVar());
120 nmea0183.Rmc.IsDataValid = NTrue;
121 if (!bGPSValid) nmea0183.Rmc.IsDataValid = NFalse;
124 nmea0183.Rmc.Position.Latitude.Set(-gLat, _T(
"S"));
126 nmea0183.Rmc.Position.Latitude.Set(gLat, _T(
"N"));
129 nmea0183.Rmc.Position.Longitude.Set(-gLon, _T(
"W"));
131 nmea0183.Rmc.Position.Longitude.Set(gLon, _T(
"E"));
133 nmea0183.Rmc.SpeedOverGroundKnots = r_Sog;
134 nmea0183.Rmc.TrackMadeGoodDegreesTrue = r_Cog;
136 if (!std::isnan(gVar)) {
138 nmea0183.Rmc.MagneticVariation = -gVar;
139 nmea0183.Rmc.MagneticVariationDirection = West;
141 nmea0183.Rmc.MagneticVariation = gVar;
142 nmea0183.Rmc.MagneticVariationDirection = East;
145 nmea0183.Rmc.MagneticVariation =
149 if (!gRmcTime.IsEmpty() && !gRmcDate.IsEmpty()) {
150 nmea0183.Rmc.UTCTime = gRmcTime;
151 nmea0183.Rmc.Date = gRmcDate;
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;
161 nmea0183.Rmc.FAAModeIndicator =
"A";
162 if (!bGPSValid) nmea0183.Rmc.FAAModeIndicator =
"N";
164 nmea0183.Rmc.Write(snt);
166 BroadcastNMEA0183Message(snt.Sentence, GetNmeaLog(),
167 routeman.GetMessageSentEventVar());
174 nmea0183.Apb.IsLoranBlinkOK =
176 if (!bGPSValid) nmea0183.Apb.IsLoranBlinkOK = NFalse;
178 nmea0183.Apb.IsLoranCCycleLockOK = NTrue;
179 if (!bGPSValid) nmea0183.Apb.IsLoranCCycleLockOK = NFalse;
181 nmea0183.Apb.CrossTrackErrorMagnitude =
182 routeman.GetCurrentXTEToActivePoint();
184 if (routeman.GetXTEDir() < 0)
185 nmea0183.Apb.DirectionToSteer = Left;
187 nmea0183.Apb.DirectionToSteer = Right;
189 nmea0183.Apb.CrossTrackUnits = _T(
"N");
191 if (routeman.GetArrival())
192 nmea0183.Apb.IsArrivalCircleEntered = NTrue;
194 nmea0183.Apb.IsArrivalCircleEntered = NFalse;
198 nmea0183.Apb.IsPerpendicular = NFalse;
200 nmea0183.Apb.To = pActivePoint->GetName().Truncate(maxName);
203 DistanceBearingMercator(pActivePoint->m_lat, pActivePoint->m_lon,
204 routeman.GetpActiveRouteSegmentBeginPoint()->m_lat,
205 routeman.GetpActiveRouteSegmentBeginPoint()->m_lon,
208 if (g_bMagneticAPB && !std::isnan(gVar)) {
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.);
215 nmea0183.Apb.BearingOriginToDestination = brg1m;
216 nmea0183.Apb.BearingOriginToDestinationUnits = _T(
"M");
218 nmea0183.Apb.BearingPresentPositionToDestination = bapm;
219 nmea0183.Apb.BearingPresentPositionToDestinationUnits = _T(
"M");
221 nmea0183.Apb.HeadingToSteer = bapm;
222 nmea0183.Apb.HeadingToSteerUnits = _T(
"M");
224 nmea0183.Apb.BearingOriginToDestination = brg1;
225 nmea0183.Apb.BearingOriginToDestinationUnits = _T(
"T");
227 nmea0183.Apb.BearingPresentPositionToDestination =
228 routeman.GetCurrentBrgToActivePoint();
229 nmea0183.Apb.BearingPresentPositionToDestinationUnits = _T(
"T");
231 nmea0183.Apb.HeadingToSteer = routeman.GetCurrentBrgToActivePoint();
232 nmea0183.Apb.HeadingToSteerUnits = _T(
"T");
235 nmea0183.Apb.Write(snt);
236 BroadcastNMEA0183Message(snt.Sentence, GetNmeaLog(),
237 routeman.GetMessageSentEventVar());
244 nmea0183.Xte.IsLoranBlinkOK =
246 if (!bGPSValid) nmea0183.Xte.IsLoranBlinkOK = NFalse;
248 nmea0183.Xte.IsLoranCCycleLockOK = NTrue;
249 if (!bGPSValid) nmea0183.Xte.IsLoranCCycleLockOK = NFalse;
251 nmea0183.Xte.CrossTrackErrorDistance =
252 routeman.GetCurrentXTEToActivePoint();
254 if (routeman.GetXTEDir() < 0)
255 nmea0183.Xte.DirectionToSteer = Left;
257 nmea0183.Xte.DirectionToSteer = Right;
259 nmea0183.Xte.CrossTrackUnits = _T(
"N");
261 nmea0183.Xte.Write(snt);
262 BroadcastNMEA0183Message(snt.Sentence, GetNmeaLog(),
263 routeman.GetMessageSentEventVar());
269bool UpdateAutopilotN2K(
Routeman &routeman) {
270 bool fail_any =
false;
273 auto ®istry = CommDriverRegistry::GetInstance();
274 const std::vector<DriverPtr> &drivers = registry.GetDrivers();
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 =
282 auto protocol_it = attributes.find(
"protocol");
283 if (protocol_it != attributes.end()) {
284 std::string protocol = protocol_it->second;
286 if (protocol ==
"nmea2000") {
293 if (!found)
return false;
298 drv_serial->AddTxPGN(129283);
299 drv_serial->AddTxPGN(129284);
300 drv_serial->AddTxPGN(129285);
303 fail_any |= !SendPGN129285(routeman, found);
304 fail_any |= !SendPGN129284(routeman, found);
305 fail_any |= !SendPGN129283(routeman, found);
307 return (fail_any == 0);
311 bool fail_any =
false;
315 if ((g_maxWPNameLength >= 3) && (g_maxWPNameLength <= 32))
316 maxName = g_maxWPNameLength;
321 char route_name[] =
"Route";
322 SetN2kPGN129285(msg129285, 0, 0, 0, N2kdir_forward, 0, route_name);
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();
330 fail_any |= !AppendN2kPGN129285(msg129285, 0, s, pLegBeginPoint->m_lat,
331 pLegBeginPoint->m_lon);
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);
340 if (fail_any)
return false;
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]);
347 std::make_shared<const Nmea2000Msg>(129285, payload, dest_addr, 6);
348 fail_any |= !driver->SendMessage(PGN129285, dest_addr);
350 return (fail_any == 0);
354 bool fail_any =
false;
356 RoutePoint *pActivePoint = routeman.GetpActivePoint();
360 if (!std::isnan(gCog) && !std::isnan(gSog)) {
361 double brg = routeman.GetCurrentBrgToActivePoint();
362 vmg = gSog * cos((brg - gCog) * PI / 180.);
364 wxTimeSpan tttg_span;
365 wxDateTime arrival_time = wxDateTime::Now();
367 double tttg_sec = (routeman.GetCurrentRngToActivePoint() / gSog) * 3600;
368 tttg_span = wxTimeSpan::Seconds((
long)tttg_sec);
369 arrival_time += tttg_span;
371 double time_days_1979 = arrival_time.GetTicks() / (3600. * 24.);
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);
382 routeman.GetCurrentRngToActivePoint() * 1852.,
389 routeman.GetCurrentSegmentCourse() * PI /
391 routeman.GetCurrentBrgToActivePoint() * PI /
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]);
404 std::make_shared<const Nmea2000Msg>(129284, payload, dest_addr, 6);
405 fail_any |= !driver->SendMessage(PGN129284, dest_addr);
407 return (fail_any == 0);
411 bool fail_any =
false;
413 RoutePoint *pActivePoint = routeman.GetpActivePoint();
415 double xte = routeman.GetCurrentXTEToActivePoint() * 1852.;
418 if (routeman.GetXTEDir() > 0) {
421 SetN2kPGN129283(msg129283, 0,
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]);
432 std::make_shared<const Nmea2000Msg>(129283, payload, dest_addr, 6);
433 fail_any |= !driver->SendMessage(PGN129283, dest_addr);
435 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.