41#include <wx/msw/winundef.h>
47#include <netinet/tcp.h>
55#include <wx/tokenzr.h>
56#include <wx/datetime.h>
61#include <wx/chartype.h>
63#include <wx/sckaddr.h>
71#define N_DOG_TIMEOUT 8
73using namespace std::literals::chrono_literals;
75static const int kNotFound = -1;
80 void SetMrqAddr(
unsigned int addr) {
81 m_mrq.imr_multiaddr.s_addr = addr;
82 m_mrq.imr_interface.s_addr = INADDR_ANY;
88circular_buffer::circular_buffer(
size_t size)
89 : buf_(std::unique_ptr<unsigned char[]>(new unsigned char[size])),
101bool circular_buffer::empty()
const {
103 return (!full_ && (head_ == tail_));
106bool circular_buffer::full()
const {
111void circular_buffer::put(
unsigned char item) {
112 std::lock_guard<std::mutex> lock(mutex_);
114 if (full_) tail_ = (tail_ + 1) % max_size_;
116 head_ = (head_ + 1) % max_size_;
118 full_ = head_ == tail_;
121unsigned char circular_buffer::get() {
122 std::lock_guard<std::mutex> lock(mutex_);
124 if (empty())
return 0;
127 auto val = buf_[tail_];
129 tail_ = (tail_ + 1) % max_size_;
136 : priority(
'\0'), source(
'\0'), destination(
'\0'), pgn(-1) {};
146 : wxEvent(
id, commandType) {};
150 void SetPayload(std::shared_ptr<std::vector<unsigned char>> data) {
153 std::shared_ptr<std::vector<unsigned char>> GetPayload() {
return m_payload; }
156 wxEvent* Clone()
const {
158 newevent->m_payload = this->m_payload;
163 std::shared_ptr<std::vector<unsigned char>> m_payload;
166static uint64_t PayloadToName(
const std::vector<unsigned char> payload) {
168 memcpy(&name,
reinterpret_cast<const void*
>(payload.data()),
sizeof(name));
176#define TIMER_SOCKET_N2KNET 7339
179EVT_TIMER(TIMER_SOCKET_N2KNET, CommDriverN2KNet::OnTimerSocket)
181EVT_SOCKET(DS_SERVERSOCKET_ID, CommDriverN2KNet::OnServerSocketEvent)
182EVT_TIMER(TIMER_SOCKET_N2KNET + 1, CommDriverN2KNet::OnSocketReadWatchdogTimer)
191 m_listener(listener),
192 m_stats_timer(*this, 2s),
193 m_net_port(wxString::Format("%i", params->NetworkPort)),
194 m_net_protocol(params->NetProtocol),
197 m_socket_server(NULL),
198 m_is_multicast(false),
200 m_portstring(params->GetDSPort()),
201 m_io_select(params->IOSelect),
202 m_connection_type(params->Type),
204 m_TX_available(false) {
205 m_addr.Hostname(params->NetworkAddress);
206 m_addr.Service(params->NetworkPort);
208 m_driver_stats.driver_bus = NavAddr::Bus::N2000;
209 m_driver_stats.driver_iface = params->GetStrippedDSPort();
211 m_socket_timer.SetOwner(
this, TIMER_SOCKET_N2KNET);
212 m_socketread_watchdog_timer.SetOwner(
this, TIMER_SOCKET_N2KNET + 1);
213 this->attributes[
"netAddress"] = params->NetworkAddress.ToStdString();
215 sprintf(port_char,
"%d", params->NetworkPort);
216 this->attributes[
"netPort"] = std::string(port_char);
217 this->attributes[
"userComment"] = params->UserComment.ToStdString();
221 Bind(wxEVT_COMMDRIVER_N2K_NET, &CommDriverN2KNet::handle_N2K_MSG,
this);
223 m_prodinfo_timer.Connect(
224 wxEVT_TIMER, wxTimerEventHandler(CommDriverN2KNet::OnProdInfoTimer), NULL,
232 rx_buffer =
new unsigned char[RX_BUFFER_SIZE_NET + 1];
237 m_n2k_format = N2KFormat_YD_RAW;
240 resume_listener.Init(SystemEvents::GetInstance().evt_resume,
246CommDriverN2KNet::~CommDriverN2KNet() {
247 delete m_mrq_container;
255 std::string Model_ID;
259std::unordered_map<uint8_t, product_info> prod_info_map;
261bool CommDriverN2KNet::HandleMgntMsg(uint64_t pgn,
262 std::vector<unsigned char>& payload) {
264 auto name = PayloadToName(payload);
266 std::make_shared<const Nmea2000Msg>(pgn, payload, GetAddress(name));
268 bool b_handled =
false;
271 uint8_t src_addr = payload.at(7);
272 if (src_addr == 75)
return false;
274 pr_info.Model_ID = std::string((
char*)&payload.data()[17], 32);
275 pr_info.RT_flag = m_TX_flag;
277 prod_info_map[src_addr] = pr_info;
282 uint8_t src_addr = payload.at(7);
292void CommDriverN2KNet::OnProdInfoTimer(wxTimerEvent& ev) {
294 bool b_found =
false;
295 for (
const auto& [key, value] : prod_info_map) {
296 auto prod_info = value;
297 if (prod_info.Model_ID.find(
"YDEN") != std::string::npos) {
302 if (prod_info.RT_flag ==
'T') b_found =
true;
307 if (b_found) m_TX_available =
true;
308 prod_info_map.clear();
312 auto p =
event.GetPayload();
313 std::vector<unsigned char>* payload = p.get();
317 unsigned char* c = (
unsigned char*)&pgn;
318 *c++ = payload->at(3);
319 *c++ = payload->at(4);
320 *c++ = payload->at(5);
324 auto name = PayloadToName(*payload);
326 std::make_shared<const Nmea2000Msg>(pgn, *payload, GetAddress(name));
327 m_driver_stats.
rx_count += payload->size();
328 m_listener.
Notify(std::move(msg));
331void CommDriverN2KNet::Open() {
333#if wxCHECK_VERSION(3, 0, 0)
335 ((
struct sockaddr_in*)GetAddr().GetAddressData())->sin_addr.s_addr;
338 ((
struct sockaddr_in*)GetAddr().GetAddress()->m_addr)->sin_addr.s_addr;
341 unsigned int addr = inet_addr(GetAddr().IPAddress().mb_str());
344 switch (m_net_protocol) {
346 OpenNetworkTCP(addr);
350 OpenNetworkUDP(addr);
359void CommDriverN2KNet::OpenNetworkUDP(
unsigned int addr) {
360 if (GetPortType() != DS_TYPE_OUTPUT) {
363 wxIPV4address conn_addr;
364 conn_addr.Service(GetNetPort());
365 conn_addr.AnyAddress();
367 new wxDatagramSocket(conn_addr, wxSOCKET_NOWAIT | wxSOCKET_REUSEADDR));
370 if ((ntohl(addr) & 0xf0000000) == 0xe0000000) {
372 m_mrq_container->SetMrqAddr(addr);
373 GetSock()->SetOption(IPPROTO_IP, IP_ADD_MEMBERSHIP,
374 &m_mrq_container->m_mrq,
375 sizeof(m_mrq_container->m_mrq));
378 GetSock()->SetEventHandler(*
this, DS_SOCKET_ID);
380 GetSock()->SetNotify(wxSOCKET_CONNECTION_FLAG | wxSOCKET_INPUT_FLAG |
382 GetSock()->Notify(TRUE);
383 GetSock()->SetTimeout(1);
384 m_driver_stats.available =
true;
388 if (GetPortType() != DS_TYPE_INPUT) {
389 wxIPV4address tconn_addr;
390 tconn_addr.Service(0);
391 tconn_addr.AnyAddress();
393 new wxDatagramSocket(tconn_addr, wxSOCKET_NOWAIT | wxSOCKET_REUSEADDR));
398 if ((!GetMulticast()) && (GetAddr().IPAddress().EndsWith(
"255"))) {
399 int broadcastEnable = 1;
400 bool bam = GetTSock()->SetOption(
401 SOL_SOCKET, SO_BROADCAST, &broadcastEnable,
sizeof(broadcastEnable));
403 m_driver_stats.available =
true;
407 SetConnectTime(wxDateTime::Now());
410void CommDriverN2KNet::OpenNetworkTCP(
unsigned int addr) {
411 int isServer = ((addr == INADDR_ANY) ? 1 : 0);
412 wxLogMessage(wxString::Format(
"Opening TCP Server %d", isServer));
415 SetSockServer(
new wxSocketServer(GetAddr(), wxSOCKET_REUSEADDR));
417 SetSock(
new wxSocketClient());
421 GetSockServer()->SetEventHandler(*
this, DS_SERVERSOCKET_ID);
422 GetSockServer()->SetNotify(wxSOCKET_CONNECTION_FLAG);
423 GetSockServer()->Notify(TRUE);
424 GetSockServer()->SetTimeout(1);
426 GetSock()->SetEventHandler(*
this, DS_SOCKET_ID);
427 int notify_flags = (wxSOCKET_CONNECTION_FLAG | wxSOCKET_LOST_FLAG);
428 if (GetPortType() != DS_TYPE_INPUT) notify_flags |= wxSOCKET_OUTPUT_FLAG;
429 if (GetPortType() != DS_TYPE_OUTPUT) notify_flags |= wxSOCKET_INPUT_FLAG;
430 GetSock()->SetNotify(notify_flags);
431 GetSock()->Notify(TRUE);
432 GetSock()->SetTimeout(1);
434 SetBrxConnectEvent(
false);
435 GetSocketTimer()->Start(100, wxTIMER_ONE_SHOT);
439 SetConnectTime(wxDateTime::Now());
442void CommDriverN2KNet::OnSocketReadWatchdogTimer(wxTimerEvent& event) {
445 if (m_dog_value <= 0) {
446 if (GetParams().NoDataReconnect) {
448 if (GetProtocol() == TCP) {
449 wxSocketClient* tcp_socket =
dynamic_cast<wxSocketClient*
>(GetSock());
450 if (tcp_socket) tcp_socket->Close();
452 int n_reconnect_delay = wxMax(N_DOG_TIMEOUT - 2, 2);
453 wxLogMessage(wxString::Format(
" Reconnection scheduled in %d seconds.",
455 GetSocketTimer()->Start(n_reconnect_delay * 1000, wxTIMER_ONE_SHOT);
458 GetSocketThreadWatchdogTimer()->Stop();
464void CommDriverN2KNet::OnTimerSocket() {
466 wxSocketClient* tcp_socket =
dynamic_cast<wxSocketClient*
>(GetSock());
468 if (tcp_socket->IsDisconnected()) {
469 wxLogDebug(
" Attempting reconnection...");
470 SetBrxConnectEvent(
false);
472 GetSocketThreadWatchdogTimer()->Stop();
473 tcp_socket->Connect(GetAddr(), FALSE);
476 int n_reconnect_delay = N_DOG_TIMEOUT;
477 GetSocketTimer()->Start(n_reconnect_delay * 1000, wxTIMER_ONE_SHOT);
482void CommDriverN2KNet::HandleResume() {
484 wxSocketClient* tcp_socket =
dynamic_cast<wxSocketClient*
>(GetSock());
486 GetSocketThreadWatchdogTimer()->Stop();
491 int n_reconnect_delay = wxMax(N_DOG_TIMEOUT - 2, 2);
492 wxLogMessage(wxString::Format(
" Reconnection scheduled in %d seconds.",
495 GetSocketTimer()->Start(n_reconnect_delay * 1000, wxTIMER_ONE_SHOT);
499bool CommDriverN2KNet::SendMessage(std::shared_ptr<const NavMsg> msg,
500 std::shared_ptr<const NavAddr> addr) {
501 if (!msg)
return false;
502 auto msg_n2k = std::dynamic_pointer_cast<const Nmea2000Msg>(msg);
503 auto dest_addr_n2k = std::static_pointer_cast<const NavAddr2000>(addr);
504 return SendN2KNetwork(msg_n2k, dest_addr_n2k);
507std::vector<unsigned char> CommDriverN2KNet::PrepareLogPayload(
508 std::shared_ptr<const Nmea2000Msg>& msg,
509 std::shared_ptr<const NavAddr2000> addr) {
510 std::vector<unsigned char> data;
511 data.push_back(0x94);
512 data.push_back(0x13);
513 data.push_back(msg->priority);
514 data.push_back(msg->PGN.pgn & 0xFF);
515 data.push_back((msg->PGN.pgn >> 8) & 0xFF);
516 data.push_back((msg->PGN.pgn >> 16) & 0xFF);
517 data.push_back(addr->address);
518 data.push_back(addr->address);
519 for (
size_t n = 0; n < msg->payload.size(); n++)
520 data.push_back(msg->payload[n]);
521 data.push_back(0x55);
525std::vector<unsigned char> CommDriverN2KNet::PushCompleteMsg(
526 const CanHeader header,
int position,
const can_frame frame) {
527 std::vector<unsigned char> data;
528 data.push_back(0x93);
529 data.push_back(0x13);
530 data.push_back(header.priority);
531 data.push_back(header.pgn & 0xFF);
532 data.push_back((header.pgn >> 8) & 0xFF);
533 data.push_back((header.pgn >> 16) & 0xFF);
534 data.push_back(header.destination);
535 data.push_back(header.source);
536 data.push_back(0xFF);
537 data.push_back(0xFF);
538 data.push_back(0xFF);
539 data.push_back(0xFF);
540 data.push_back(CAN_MAX_DLEN);
541 for (
size_t n = 0; n < CAN_MAX_DLEN; n++) data.push_back(frame.data[n]);
542 data.push_back(0x55);
546std::vector<unsigned char> CommDriverN2KNet::PushFastMsgFragment(
548 std::vector<unsigned char> data;
549 data.push_back(0x93);
550 data.push_back(fast_messages->entries[position].expected_length + 11);
551 data.push_back(header.priority);
552 data.push_back(header.pgn & 0xFF);
553 data.push_back((header.pgn >> 8) & 0xFF);
554 data.push_back((header.pgn >> 16) & 0xFF);
555 data.push_back(header.destination);
556 data.push_back(header.source);
557 data.push_back(0xFF);
558 data.push_back(0xFF);
559 data.push_back(0xFF);
560 data.push_back(0xFF);
561 data.push_back(fast_messages->entries[position].expected_length);
562 for (
size_t n = 0; n < fast_messages->entries[position].expected_length; n++)
563 data.push_back(fast_messages->entries[position].data[n]);
564 data.push_back(0x55);
565 fast_messages->
Remove(position);
575void CommDriverN2KNet::HandleCanFrameInput(can_frame frame) {
582 if (position == kNotFound) {
589 if ((frame.data[0] & 0x1F) == 0) {
591 ready = fast_messages->
InsertEntry(header, frame.data, position);
596 ready = fast_messages->
AppendEntry(header, frame.data, position);
600 std::vector<unsigned char> vec;
603 vec = PushFastMsgFragment(header, position);
606 vec = PushCompleteMsg(header, position, frame);
610 if (HandleMgntMsg(header.pgn, vec))
return;
614 auto payload = std::make_shared<std::vector<uint8_t>>(vec);
615 Nevent.SetPayload(payload);
616 AddPendingEvent(Nevent);
620bool isASCII(std::vector<unsigned char> packet) {
621 for (
unsigned char c : packet) {
622 if (!isascii(c))
return false;
627N2K_Format CommDriverN2KNet::DetectFormat(std::vector<unsigned char> packet) {
632 if (isASCII(packet)) {
633 std::string payload = std::string(packet.begin(), packet.end());
634 if (payload.find(
"$PCDIN") != std::string::npos) {
635 return N2KFormat_SeaSmart;
636 }
else if (payload.find(
"$MXPGN") != std::string::npos) {
640 return N2KFormat_MiniPlex;
641 }
else if (std::find(packet.begin(), packet.end(),
':') != packet.end()) {
642 return N2KFormat_Actisense_RAW_ASCII;
644 return N2KFormat_Actisense_N2K_ASCII;
647 if (packet[2] == 0x95)
648 return N2KFormat_Actisense_RAW;
649 else if (packet[2] == 0xd0)
650 return N2KFormat_Actisense_N2K;
651 else if (packet[2] == 0x93)
652 return N2KFormat_Actisense_NGT;
654 return N2KFormat_Undefined;
657bool CommDriverN2KNet::ProcessActisense_N2K(std::vector<unsigned char> packet) {
660 std::vector<unsigned char> data;
662 bool bGotESC =
false;
663 bool bGotSOT =
false;
665 while (!m_circle->empty()) {
666 uint8_t next_byte = m_circle->get();
670 if (next_byte == ESCAPE) {
671 data.push_back(next_byte);
673 }
else if (next_byte == ENDOFTEXT) {
677 unsigned int msg_length =
678 (uint32_t)data[1] + ((uint32_t)data[2] << 8);
681 if (msg_length == data.size() - 1) {
682 uint8_t destination = data[3];
683 uint8_t source = data[4];
685 uint8_t dprp = data[7];
688 uint8_t rAndDP = dprp & 3;
691 uint8_t pduFormat = data[6];
692 uint32_t pgn = (rAndDP << 16) + (pduFormat << 8);
698 std::vector<uint8_t> o_payload;
699 o_payload.push_back(0x93);
700 o_payload.push_back(0x13);
701 o_payload.push_back(priority);
702 o_payload.push_back(pgn & 0xFF);
703 o_payload.push_back((pgn >> 8) & 0xFF);
704 o_payload.push_back((pgn >> 16) & 0xFF);
705 o_payload.push_back(destination);
706 o_payload.push_back(source);
707 o_payload.push_back(0xFF);
708 o_payload.push_back(0xFF);
709 o_payload.push_back(0xFF);
710 o_payload.push_back(0xFF);
711 o_payload.push_back(data.size());
714 for (
size_t n = 13; n < data.size() - 1; n++)
715 o_payload.push_back(data[n]);
717 o_payload.push_back(0x55);
722 std::make_shared<std::vector<uint8_t>>(o_payload);
723 Nevent.SetPayload(n2k_payload);
724 AddPendingEvent(Nevent);
731 }
else if (next_byte == STARTOFTEXT) {
740 bGotESC = (next_byte == ESCAPE);
743 data.push_back(next_byte);
749 if (STARTOFTEXT == next_byte) {
755 bGotESC = (next_byte == ESCAPE);
760 data.push_back(next_byte);
769bool CommDriverN2KNet::ProcessActisense_RAW(std::vector<unsigned char> packet) {
774 std::vector<unsigned char> data;
776 bool bGotESC =
false;
777 bool bGotSOT =
false;
779 while (!m_circle->empty()) {
780 uint8_t next_byte = m_circle->get();
784 if (next_byte == ESCAPE) {
785 data.push_back(next_byte);
787 }
else if (next_byte == ENDOFTEXT) {
792 if (data.size() >= 8) {
793 size_t dLen = data[1];
795 if (dLen + 3 == data.size()) {
797 memcpy(&frame.can_id, &data.data()[4], 4);
800 memcpy(&frame.data, &data.data()[8], 8);
802 HandleCanFrameInput(frame);
810 }
else if (next_byte == STARTOFTEXT) {
819 bGotESC = (next_byte == ESCAPE);
822 data.push_back(next_byte);
828 if (STARTOFTEXT == next_byte) {
834 bGotESC = (next_byte == ESCAPE);
839 data.push_back(next_byte);
848bool CommDriverN2KNet::ProcessActisense_NGT(std::vector<unsigned char> packet) {
849 std::vector<unsigned char> data;
851 bool bGotESC =
false;
852 bool bGotSOT =
false;
854 while (!m_circle->empty()) {
855 uint8_t next_byte = m_circle->get();
859 if (next_byte == ESCAPE) {
860 data.push_back(next_byte);
862 }
else if (next_byte == ENDOFTEXT) {
865 auto n2k_payload = std::make_shared<std::vector<uint8_t>>(data);
866 Nevent.SetPayload(n2k_payload);
867 AddPendingEvent(Nevent);
873 }
else if (next_byte == STARTOFTEXT) {
882 bGotESC = (next_byte == ESCAPE);
885 data.push_back(next_byte);
891 if (STARTOFTEXT == next_byte) {
897 bGotESC = (next_byte == ESCAPE);
902 data.push_back(next_byte);
911bool CommDriverN2KNet::ProcessActisense_ASCII_RAW(
912 std::vector<unsigned char> packet) {
915 while (!m_circle->empty()) {
916 char b = m_circle->get();
917 if ((b != 0x0a) && (b != 0x0d)) {
925 wxString ss(m_sentence.c_str());
927 wxStringTokenizer tkz(ss,
" ");
930 wxString token = tkz.GetNextToken();
932 token = tkz.GetNextToken();
934 m_TX_flag = token[0];
937 token = tkz.GetNextToken();
939 token.ToLong(&canID, 16);
940 frame.can_id = canID;
943 unsigned char bytes[8];
945 for (
unsigned int i = 0; i < 8; i++) {
946 if (tkz.HasMoreTokens()) {
947 token = tkz.GetNextToken();
949 token.ToLong(&tui, 16);
950 bytes[i] = (uint8_t)tui;
953 memcpy(&frame.data, bytes, 8);
954 HandleCanFrameInput(frame);
960bool CommDriverN2KNet::ProcessActisense_ASCII_N2K(
961 std::vector<unsigned char> packet) {
963 std::string sentence;
965 while (!m_circle->empty()) {
966 char b = m_circle->get();
967 if ((b != 0x0a) && (b != 0x0d)) {
975 wxString ss(sentence.c_str());
976 wxStringTokenizer tkz(ss,
" ");
980 wxString time_header = tkz.GetNextToken();
982 wxString sprio_addr = tkz.GetNextToken();
984 sprio_addr.ToLong(&prio_addr, 16);
985 uint8_t priority = (uint8_t)prio_addr & 0X0F;
986 uint8_t destination = (uint8_t)(prio_addr >> 4) & 0X0FF;
987 uint8_t source = (uint8_t)(prio_addr >> 12) & 0X0FF;
990 wxString sPGN = tkz.GetNextToken();
992 sPGN.ToULong(&PGN, 16);
996 wxString sdata = tkz.GetNextToken();
997 std::vector<uint8_t> data;
998 for (
size_t i = 0; i < sdata.Length(); i += 2) {
1000 wxString stui = sdata.Mid(i, 2);
1001 stui.ToLong(&dv, 16);
1002 data.push_back((uint8_t)dv);
1006 std::vector<uint8_t> o_payload;
1007 o_payload.push_back(0x93);
1008 o_payload.push_back(0x13);
1009 o_payload.push_back(priority);
1010 o_payload.push_back(PGN & 0xFF);
1011 o_payload.push_back((PGN >> 8) & 0xFF);
1012 o_payload.push_back((PGN >> 16) & 0xFF);
1013 o_payload.push_back(destination);
1014 o_payload.push_back(source);
1015 o_payload.push_back(0xFF);
1016 o_payload.push_back(0xFF);
1017 o_payload.push_back(0xFF);
1018 o_payload.push_back(0xFF);
1019 o_payload.push_back(data.size());
1020 for (
size_t n = 0; n < data.size(); n++) o_payload.push_back(data[n]);
1021 o_payload.push_back(0x55);
1023 if (HandleMgntMsg(PGN, o_payload))
return false;
1027 auto n2k_payload = std::make_shared<std::vector<uint8_t>>(o_payload);
1028 Nevent.SetPayload(n2k_payload);
1029 AddPendingEvent(Nevent);
1035bool CommDriverN2KNet::ProcessSeaSmart(std::vector<unsigned char> packet) {
1036 while (!m_circle->empty()) {
1037 char b = m_circle->get();
1038 if ((b != 0x0a) && (b != 0x0d)) {
1046 wxString ss(m_sentence.c_str());
1048 wxStringTokenizer tkz(ss,
",");
1051 wxString token = tkz.GetNextToken();
1054 token = tkz.GetNextToken();
1056 token.ToULong(&PGN, 16);
1058 token = tkz.GetNextToken();
1059 unsigned long timestamp;
1060 token.ToULong(×tamp, 16);
1062 token = tkz.GetNextToken();
1063 unsigned long source;
1064 token.ToULong(&source, 16);
1066 token = tkz.GetNextToken();
1068 wxStringTokenizer datatkz(token,
"*");
1069 wxString data = datatkz.GetNextToken();
1072 std::vector<uint8_t> o_payload;
1073 o_payload.push_back(0x93);
1074 o_payload.push_back(0x13);
1075 o_payload.push_back(3);
1076 o_payload.push_back(PGN & 0xFF);
1077 o_payload.push_back((PGN >> 8) & 0xFF);
1078 o_payload.push_back((PGN >> 16) & 0xFF);
1079 o_payload.push_back(0xFF);
1080 o_payload.push_back((uint8_t)source);
1081 o_payload.push_back(timestamp & 0xFF);
1082 o_payload.push_back((timestamp >> 8) & 0xFF);
1083 o_payload.push_back((timestamp >> 16) & 0xFF);
1084 o_payload.push_back((timestamp >> 24) & 0xFF);
1085 o_payload.push_back((uint8_t)data.Length() / 2);
1086 for (
size_t i = 0; i < data.Length(); i += 2) {
1088 wxString sbyte = data.Mid(i, 2);
1089 sbyte.ToULong(&dv, 16);
1090 o_payload.push_back((uint8_t)dv);
1092 o_payload.push_back(0x55);
1094 if (HandleMgntMsg(PGN, o_payload))
return false;
1098 auto n2k_payload = std::make_shared<std::vector<uint8_t>>(o_payload);
1099 Nevent.SetPayload(n2k_payload);
1100 AddPendingEvent(Nevent);
1106bool CommDriverN2KNet::ProcessMiniPlex(std::vector<unsigned char> packet) {
1201 while (!m_circle->empty()) {
1202 char b = m_circle->get();
1203 if ((b != 0x0a) && (b != 0x0d)) {
1211 wxString ss(m_sentence.c_str());
1213 wxStringTokenizer tkz(ss,
",");
1216 wxString token = tkz.GetNextToken();
1219 token = tkz.GetNextToken();
1221 token.ToULong(&PGN, 16);
1223 token = tkz.GetNextToken();
1225 token.ToULong(&attr, 16);
1227 bool send_bit = (attr >> 15) != 0;
1229 uint8_t priority = (attr >> 12) & 0x07;
1232 uint8_t dlc = (attr >> 8) & 0x0F;
1235 uint8_t address = attr & 0xFF;
1237 token = tkz.GetNextToken();
1239 wxStringTokenizer datatkz(token,
"*");
1240 wxString data = datatkz.GetNextToken();
1248 memset(&frame.data, 0, 8);
1249 for (
size_t i = 0; i < data.Length(); i += 2) {
1251 wxString sbyte = data.Mid(data.Length() - i - 2, 2);
1252 sbyte.ToULong(&dv, 16);
1253 frame.data[i / 2] = ((uint8_t)dv);
1255 frame.can_id = (uint32_t)BuildCanID(priority, address, 0xFF, PGN);
1256 HandleCanFrameInput(frame);
1263#define RD_BUF_SIZE 4096
1266 switch (event.GetSocketEvent()) {
1267 case wxSOCKET_INPUT: {
1281 std::vector<unsigned char> data(RD_BUF_SIZE + 1);
1283 uint8_t next_byte = 0;
1285 event.GetSocket()->Read(&data.front(), RD_BUF_SIZE);
1286 if (!event.GetSocket()->Error()) {
1287 m_driver_stats.available =
true;
1288 size_t count =
event.GetSocket()->LastCount();
1301 for (
int i = 0; i < newdata; i++) {
1302 m_circle->put(data[i]);
1307 m_n2k_format = DetectFormat(data);
1309 switch (m_n2k_format) {
1310 case N2KFormat_Actisense_RAW_ASCII:
1311 ProcessActisense_ASCII_RAW(data);
1313 case N2KFormat_YD_RAW:
1314 ProcessActisense_ASCII_RAW(data);
1316 case N2KFormat_Actisense_N2K_ASCII:
1317 ProcessActisense_ASCII_N2K(data);
1319 case N2KFormat_Actisense_N2K:
1320 ProcessActisense_N2K(data);
1322 case N2KFormat_Actisense_RAW:
1323 ProcessActisense_RAW(data);
1325 case N2KFormat_Actisense_NGT:
1326 ProcessActisense_NGT(data);
1328 case N2KFormat_SeaSmart:
1329 ProcessSeaSmart(data);
1331 case N2KFormat_MiniPlex:
1332 ProcessMiniPlex(data);
1334 case N2KFormat_Undefined:
1341 m_dog_value = N_DOG_TIMEOUT;
1345 case wxSOCKET_LOST: {
1346 m_driver_stats.available =
false;
1347 if (GetProtocol() == TCP || GetProtocol() == GPSD) {
1348 if (GetBrxConnectEvent())
1349 wxLogMessage(wxString::Format(
"NetworkDataStream connection lost: %s",
1350 GetPort().c_str()));
1351 if (GetSockServer()) {
1352 GetSock()->Destroy();
1356 wxDateTime now = wxDateTime::Now();
1357 wxTimeSpan since_connect(
1359 if (GetConnectTime().IsValid()) since_connect = now - GetConnectTime();
1361 int retry_time = 5000;
1367 if (!GetBrxConnectEvent() && (since_connect.GetSeconds() < 5))
1370 GetSocketThreadWatchdogTimer()->Stop();
1371 GetSocketTimer()->Start(
1372 retry_time, wxTIMER_ONE_SHOT);
1377 case wxSOCKET_CONNECTION: {
1378 m_driver_stats.available =
true;
1379 if (GetProtocol() == GPSD) {
1383 char cmd[] =
"?WATCH={\"class\":\"WATCH\", \"nmea\":true}";
1384 GetSock()->Write(cmd, strlen(cmd));
1385 }
else if (GetProtocol() == TCP) {
1387 wxString::Format(
"TCP NetworkDataStream connection established: %s",
1388 GetPort().c_str()));
1389 m_dog_value = N_DOG_TIMEOUT;
1390 if (GetPortType() != DS_TYPE_OUTPUT) {
1392 if (GetParams().NoDataReconnect)
1393 GetSocketThreadWatchdogTimer()->Start(1000);
1395 if (GetPortType() != DS_TYPE_INPUT && GetSock()->IsOk())
1396 (void)SetOutputSocketOptions(GetSock());
1397 GetSocketTimer()->Stop();
1398 SetBrxConnectEvent(
true);
1401 SetConnectTime(wxDateTime::Now());
1410void CommDriverN2KNet::OnServerSocketEvent(wxSocketEvent& event) {
1411 switch (event.GetSocketEvent()) {
1412 case wxSOCKET_CONNECTION: {
1413 m_driver_stats.available =
true;
1414 SetSock(GetSockServer()->Accept(
false));
1417 GetSock()->SetTimeout(2);
1419 GetSock()->SetEventHandler(*
this, DS_SOCKET_ID);
1420 int notify_flags = (wxSOCKET_CONNECTION_FLAG | wxSOCKET_LOST_FLAG);
1421 if (GetPortType() != DS_TYPE_INPUT) {
1422 notify_flags |= wxSOCKET_OUTPUT_FLAG;
1423 (void)SetOutputSocketOptions(GetSock());
1425 if (GetPortType() != DS_TYPE_OUTPUT)
1426 notify_flags |= wxSOCKET_INPUT_FLAG;
1427 GetSock()->SetNotify(notify_flags);
1428 GetSock()->Notify(
true);
1439std::vector<unsigned char> MakeSimpleOutMsg(
1440 int data_format,
int pgn, std::vector<unsigned char>& payload) {
1441 std::vector<unsigned char> out_vec;
1443 switch (data_format) {
1444 case N2KFormat_YD_RAW:
1445 case N2KFormat_Actisense_RAW_ASCII: {
1447 unsigned can_id = BuildCanID(6, 0xff, 0xff, pgn);
1448 std::stringstream ss;
1449 ss << std::setfill(
'0') << std::setw(8) << std::hex << can_id;
1450 for (
unsigned char s : ss.str()) out_vec.push_back(s);
1451 out_vec.push_back(
' ');
1456 for (
unsigned char d : payload) {
1457 snprintf(tv, 4,
"%02X ", d);
1460 for (
unsigned char s : sspl) out_vec.push_back(s);
1464 out_vec.push_back(0x0d);
1465 out_vec.push_back(0x0a);
1468 case N2KFormat_Actisense_N2K_ASCII: {
1470 wxDateTime now = wxDateTime::Now();
1471 wxString stime = now.Format(
"%H%M%S");
1473 std::string sstime = stime.ToStdString();
1474 out_vec.push_back(
'A');
1475 for (
unsigned char s : sstime) out_vec.push_back(s);
1479 sdp.Printf(
"%02X%02X%1X ",
1481 (
unsigned char)0xFF, 0x6);
1482 std::string ssdp = sdp.ToStdString();
1483 for (
unsigned char s : ssdp) out_vec.push_back(s);
1487 spgn.Printf(
"%05X ", pgn);
1488 std::string sspgn = spgn.ToStdString();
1489 for (
unsigned char s : sspgn) out_vec.push_back(s);
1494 for (
unsigned char d : payload) {
1495 snprintf(tv, 3,
"%02X", d);
1498 for (
unsigned char s : sspl) out_vec.push_back(s);
1501 out_vec.push_back(0x0d);
1502 out_vec.push_back(0x0a);
1505 case N2KFormat_MiniPlex: {
1506 out_vec.push_back(
'$');
1507 out_vec.push_back(
'M');
1508 out_vec.push_back(
'X');
1509 out_vec.push_back(
'P');
1510 out_vec.push_back(
'G');
1511 out_vec.push_back(
'N');
1512 out_vec.push_back(
',');
1515 spgn.Printf(
"%06X,", pgn);
1516 std::string sspgn = spgn.ToStdString();
1517 for (
unsigned char c : sspgn) {
1518 out_vec.push_back(c);
1524 attr |= ((uint16_t)0x06) << 12;
1525 attr |= ((uint16_t)payload.size()) << 8;
1526 attr |= (uint16_t)0xFF;
1530 sattr.Printf(
"%04X,", attr);
1531 std::string ssattr = sattr.ToStdString();
1532 for (
unsigned char c : ssattr) {
1533 out_vec.push_back(c);
1537 for (
auto rit = payload.rbegin(); rit != payload.rend(); ++rit) {
1538 snprintf(tv, 3,
"%02X", *rit);
1539 out_vec.push_back(tv[0]);
1540 out_vec.push_back(tv[1]);
1544 for (
auto ci = ++out_vec.begin(); ci != out_vec.end(); ci++) {
1547 out_vec.push_back(
'*');
1548 snprintf(tv, 3,
"%02X", crc);
1549 out_vec.push_back(tv[0]);
1550 out_vec.push_back(tv[1]);
1553 out_vec.push_back(0x0d);
1554 out_vec.push_back(0x0a);
1565std::vector<std::vector<unsigned char>> CommDriverN2KNet::GetTxVector(
1566 const std::shared_ptr<const Nmea2000Msg>& msg,
1567 std::shared_ptr<const NavAddr2000> dest_addr) {
1568 std::vector<std::vector<unsigned char>> tx_vector;
1571 switch (m_n2k_format) {
1572 case N2KFormat_YD_RAW:
1574 case N2KFormat_Actisense_RAW_ASCII: {
1576 if (!IsFastMessagePGN(msg->PGN.pgn) && msg->payload.size() <= 8) {
1578 std::vector<unsigned char> header_vec;
1579 std::vector<unsigned char> out_vec;
1584 unsigned long can_id =
1585 BuildCanID(msg->priority, 0, dest_addr->address, msg->PGN.pgn);
1587 std::stringstream ss;
1588 ss << std::setfill(
'0') << std::setw(8) << std::hex << can_id;
1589 for (
unsigned char s : ss.str()) header_vec.push_back(s);
1590 header_vec.push_back(
' ');
1593 for (
unsigned char s : header_vec) out_vec.push_back(s);
1597 for (
unsigned int k = 0; k < msg->payload.size(); k++) {
1599 snprintf(tb, 4,
"%02X ", msg->payload.data()[k]);
1602 for (
unsigned char s : ssdata) out_vec.push_back(s);
1605 out_vec.push_back(0x0d);
1606 out_vec.push_back(0x0a);
1608 tx_vector.push_back(out_vec);
1610 std::vector<unsigned char> header_vec;
1611 std::vector<unsigned char> out_vec;
1616 wxDateTime now = wxDateTime::Now();
1617 wxString stime = now.Format(
"%H:%M:%S");
1619 std::string sstime = stime.ToStdString();
1620 for (
unsigned char s : sstime) header_vec.push_back(s);
1623 header_vec.push_back(
'T');
1624 header_vec.push_back(
' ');
1630 unsigned long can_id =
1631 BuildCanID(msg->priority, 0, dest_addr->address, msg->PGN.pgn);
1632 std::stringstream ss;
1633 ss << std::setfill(
'0') << std::setw(8) << std::hex << can_id;
1634 for (
unsigned char s : ss.str()) header_vec.push_back(s);
1635 header_vec.push_back(
' ');
1638 int payload_size = msg->payload.size();
1639 unsigned char temp[8];
1642 (payload_size > 6 ? (payload_size - 6 - 1) / 7 + 1 + 1 : 1);
1644 for (
int i = 0; i < nframes && result; i++) {
1645 temp[0] = i | m_order;
1647 temp[1] = msg->payload.size();
1649 for (
int j = 2; j < 8; j++) {
1650 temp[j] = msg->payload.data()[cur];
1656 for (; j < 8 && cur < payload_size; j++) {
1657 temp[j] = msg->payload.data()[cur];
1660 for (; j < 8; j++) {
1668 for (
unsigned char s : header_vec) out_vec.push_back(s);
1672 for (
unsigned int k = 0; k < 8; k++) {
1674 snprintf(tb, 4,
"%02X ", temp[k]);
1677 for (
unsigned char s : ssdata) out_vec.push_back(s);
1680 out_vec.push_back(0x0d);
1681 out_vec.push_back(0x0a);
1683 tx_vector.push_back(out_vec);
1687 case N2KFormat_Actisense_N2K_ASCII: {
1704 std::vector<unsigned char> ovec;
1707 wxDateTime now = wxDateTime::Now();
1708 wxString stime = now.Format(
"%H%M%S");
1710 std::string sstime = stime.ToStdString();
1711 ovec.push_back(
'A');
1712 for (
unsigned char s : sstime) ovec.push_back(s);
1716 sdp.Printf(
"%02X%02X%1X ",
1718 (
unsigned char)dest_addr->address,
1719 (
unsigned char)msg->priority);
1720 std::string ssdp = sdp.ToStdString();
1721 for (
unsigned char s : ssdp) ovec.push_back(s);
1725 spgn.Printf(
"%05X ", (
int)msg->PGN.pgn);
1726 std::string sspgn = spgn.ToStdString();
1727 for (
unsigned char s : sspgn) ovec.push_back(s);
1732 for (
unsigned char d : msg->payload) {
1733 snprintf(tv, 3,
"%02X", d);
1736 for (
unsigned char s : sspl) ovec.push_back(s);
1739 ovec.push_back(0x0d);
1740 ovec.push_back(0x0a);
1743 tx_vector.push_back(ovec);
1747 case N2KFormat_MiniPlex: {
1748 std::vector<unsigned char> ovec;
1749 if (!IsFastMessagePGN(msg->PGN.pgn) && msg->payload.size() < 8) {
1754 (msg->payload.size() > 6 ? (msg->payload.size() - 6 - 1) / 7 + 1 + 1
1756 for (
size_t i = 0; i < nframes; i++) {
1757 ovec.push_back(
'$');
1758 ovec.push_back(
'M');
1759 ovec.push_back(
'X');
1760 ovec.push_back(
'P');
1761 ovec.push_back(
'G');
1762 ovec.push_back(
'N');
1763 ovec.push_back(
',');
1766 spgn.Printf(
"%06X,", (
int)msg->PGN.pgn);
1767 std::string sspgn = spgn.ToStdString();
1768 for (
unsigned char c : sspgn) {
1774 if (i == nframes - 1) {
1775 len = msg->payload.size() + 1 - 6 - (nframes - 2) * 7;
1777 attr |= ((uint16_t)((uint8_t)msg->priority & 0x07)) << 12;
1778 attr |= ((uint16_t)len) << 8;
1779 attr |= (uint16_t)dest_addr->address;
1783 sattr.Printf(
"%04X,", attr);
1784 std::string ssattr = sattr.ToStdString();
1785 for (
unsigned char c : ssattr) {
1790 uint8_t databytes = i == 0 ? len - 2 : len - 1;
1791 std::vector<unsigned char> payload;
1792 for (uint8_t j = 0; j < databytes; j++) {
1793 payload.push_back(msg->payload[cur]);
1796 for (
auto rit = payload.rbegin(); rit != payload.rend(); ++rit) {
1797 snprintf(tv, 3,
"%02X", *rit);
1798 ovec.push_back(tv[0]);
1799 ovec.push_back(tv[1]);
1802 snprintf(tv, 3,
"%02X", (uint8_t)msg->payload.size());
1803 ovec.push_back(tv[0]);
1804 ovec.push_back(tv[1]);
1807 snprintf(tv, 3,
"%02X", (uint8_t)i | m_order);
1808 ovec.push_back(tv[0]);
1809 ovec.push_back(tv[1]);
1813 for (
auto ci = ++ovec.begin(); ci != ovec.end(); ci++) {
1816 ovec.push_back(
'*');
1817 snprintf(tv, 3,
"%02X", crc);
1818 ovec.push_back(tv[0]);
1819 ovec.push_back(tv[1]);
1822 ovec.push_back(0x0d);
1823 ovec.push_back(0x0a);
1829 tx_vector.push_back(ovec);
1836 case N2KFormat_Actisense_N2K:
1838 case N2KFormat_Actisense_RAW:
1840 case N2KFormat_Actisense_NGT:
1842 case N2KFormat_SeaSmart:
1853bool CommDriverN2KNet::PrepareForTX() {
1863 bool b_found =
false;
1870 if (m_n2k_format == N2KFormat_Actisense_N2K_ASCII)
return true;
1877 if (m_n2k_format == N2KFormat_MiniPlex)
return true;
1882 if (m_n2k_format == N2KFormat_SeaSmart)
return false;
1895 std::vector<unsigned char> payload;
1896 payload.push_back(0x14);
1897 payload.push_back(0xF0);
1898 payload.push_back(0x01);
1900 std::vector<std::vector<unsigned char>> out_data;
1901 std::vector<unsigned char> msg_vec =
1902 MakeSimpleOutMsg(N2KFormat_YD_RAW, 59904, payload);
1903 out_data.push_back(msg_vec);
1904 SendSentenceNetwork(out_data);
1907 m_prodinfo_timer.Start(200,
true);
1914bool CommDriverN2KNet::SendN2KNetwork(std::shared_ptr<const Nmea2000Msg>& msg,
1915 std::shared_ptr<const NavAddr2000> addr) {
1918 std::vector<std::vector<unsigned char>> out_data = GetTxVector(msg, addr);
1919 SendSentenceNetwork(out_data);
1920 m_driver_stats.
tx_count += msg->payload.size();
1923 std::vector<unsigned char> msg_payload = PrepareLogPayload(msg, addr);
1925 std::make_shared<const Nmea2000Msg>(msg->PGN.pgn, msg_payload, addr);
1926 auto msg_all = std::make_shared<const Nmea2000Msg>(1, msg_payload, addr);
1929 m_listener.
Notify(std::move(msg_one));
1930 m_listener.
Notify(std::move(msg_all));
1935bool CommDriverN2KNet::SendSentenceNetwork(
1936 std::vector<std::vector<unsigned char>> payload) {
1943 wxDatagramSocket* udp_socket;
1944 switch (GetProtocol()) {
1946 for (std::vector<unsigned char>& v : payload) {
1947 if (GetSock() && GetSock()->IsOk()) {
1948 m_driver_stats.available =
true;
1950 GetSock()->Write(v.data(), v.size());
1951 m_dog_value = N_DOG_TIMEOUT;
1952 if (GetSock()->Error()) {
1953 if (GetSockServer()) {
1954 GetSock()->Destroy();
1957 wxSocketClient* tcp_socket =
1958 dynamic_cast<wxSocketClient*
>(GetSock());
1959 if (tcp_socket) tcp_socket->Close();
1960 if (!GetSocketTimer()->IsRunning())
1961 GetSocketTimer()->Start(
1962 5000, wxTIMER_ONE_SHOT);
1963 GetSocketThreadWatchdogTimer()->Stop();
1969 m_driver_stats.available =
false;
1976 udp_socket =
dynamic_cast<wxDatagramSocket*
>(GetTSock());
1977 if (udp_socket && udp_socket->IsOk()) {
1978 udp_socket->SendTo(GetAddr(), payload.mb_str(), payload.size());
1979 if (udp_socket->Error()) ret =
false;
1994void CommDriverN2KNet::Close() {
1995 wxLogMessage(wxString::Format(
"Closing NMEA NetworkDataStream %s",
1996 GetNetPort().c_str()));
1997 m_stats_timer.Stop();
2001 m_sock->SetOption(IPPROTO_IP, IP_DROP_MEMBERSHIP, &m_mrq_container->m_mrq,
2002 sizeof(m_mrq_container->m_mrq));
2003 m_sock->Notify(FALSE);
2005 m_driver_stats.available =
false;
2009 m_tsock->Notify(FALSE);
2013 if (m_socket_server) {
2014 m_socket_server->Notify(FALSE);
2015 m_socket_server->Destroy();
2018 m_socket_timer.Stop();
2019 m_socketread_watchdog_timer.Stop();
2022bool CommDriverN2KNet::SetOutputSocketOptions(wxSocketBase* tsock) {
2031 int nagleDisable = 1;
2032 ret = tsock->SetOption(IPPROTO_TCP, TCP_NODELAY, &nagleDisable,
2033 sizeof(nagleDisable));
2039 unsigned long outbuf_size = 1024;
2040 return (tsock->SetOption(SOL_SOCKET, SO_SNDBUF, &outbuf_size,
2041 sizeof(outbuf_size)) &&
void OnSocketEvent(wxSocketEvent &event)
Interface for handling incoming messages.
virtual void Notify(std::shared_ptr< const NavMsg > message)=0
Handle a received message.
Track fast message fragments eventually forming complete messages.
int AddNewEntry(void)
Allocate a new, fresh entry and return index to it.
void Remove(int pos)
Remove entry at pos.
bool AppendEntry(const CanHeader hdr, const unsigned char *data, int index)
Append fragment to existing multipart message.
int FindMatchingEntry(const CanHeader header, const unsigned char sid)
Setter.
bool InsertEntry(const CanHeader header, const unsigned char *data, int index)
Insert a new entry, first part of a multipart message.
Custom event class for OpenCPN's notification system.
Nmea2000 IP network driver.
Driver registration container, a singleton.
Raw messages layer, supports sending and recieving navmsg messages.
std::string DsPortTypeToString(dsPortType type)
Return textual representation for use in driver ioDirection attribute.
GUI constant definitions.
unsigned tx_count
Number of bytes sent since program start.
unsigned rx_count
Number of bytes received since program start.
Suspend/resume and new devices events exchange point.