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 auto msg_n2k = std::dynamic_pointer_cast<const Nmea2000Msg>(msg);
502 auto dest_addr_n2k = std::static_pointer_cast<const NavAddr2000>(addr);
503 return SendN2KNetwork(msg_n2k, dest_addr_n2k);
506std::vector<unsigned char> CommDriverN2KNet::PrepareLogPayload(
507 std::shared_ptr<const Nmea2000Msg>& msg,
508 std::shared_ptr<const NavAddr2000> addr) {
509 std::vector<unsigned char> data;
510 data.push_back(0x94);
511 data.push_back(0x13);
512 data.push_back(msg->priority);
513 data.push_back(msg->PGN.pgn & 0xFF);
514 data.push_back((msg->PGN.pgn >> 8) & 0xFF);
515 data.push_back((msg->PGN.pgn >> 16) & 0xFF);
516 data.push_back(addr->address);
517 data.push_back(addr->address);
518 for (
size_t n = 0; n < msg->payload.size(); n++)
519 data.push_back(msg->payload[n]);
520 data.push_back(0x55);
524std::vector<unsigned char> CommDriverN2KNet::PushCompleteMsg(
525 const CanHeader header,
int position,
const can_frame frame) {
526 std::vector<unsigned char> data;
527 data.push_back(0x93);
528 data.push_back(0x13);
529 data.push_back(header.priority);
530 data.push_back(header.pgn & 0xFF);
531 data.push_back((header.pgn >> 8) & 0xFF);
532 data.push_back((header.pgn >> 16) & 0xFF);
533 data.push_back(header.destination);
534 data.push_back(header.source);
535 data.push_back(0xFF);
536 data.push_back(0xFF);
537 data.push_back(0xFF);
538 data.push_back(0xFF);
539 data.push_back(CAN_MAX_DLEN);
540 for (
size_t n = 0; n < CAN_MAX_DLEN; n++) data.push_back(frame.data[n]);
541 data.push_back(0x55);
545std::vector<unsigned char> CommDriverN2KNet::PushFastMsgFragment(
547 std::vector<unsigned char> data;
548 data.push_back(0x93);
549 data.push_back(fast_messages->entries[position].expected_length + 11);
550 data.push_back(header.priority);
551 data.push_back(header.pgn & 0xFF);
552 data.push_back((header.pgn >> 8) & 0xFF);
553 data.push_back((header.pgn >> 16) & 0xFF);
554 data.push_back(header.destination);
555 data.push_back(header.source);
556 data.push_back(0xFF);
557 data.push_back(0xFF);
558 data.push_back(0xFF);
559 data.push_back(0xFF);
560 data.push_back(fast_messages->entries[position].expected_length);
561 for (
size_t n = 0; n < fast_messages->entries[position].expected_length; n++)
562 data.push_back(fast_messages->entries[position].data[n]);
563 data.push_back(0x55);
564 fast_messages->
Remove(position);
574void CommDriverN2KNet::HandleCanFrameInput(can_frame frame) {
581 if (position == kNotFound) {
588 if ((frame.data[0] & 0x1F) == 0) {
590 ready = fast_messages->
InsertEntry(header, frame.data, position);
595 ready = fast_messages->
AppendEntry(header, frame.data, position);
599 std::vector<unsigned char> vec;
602 vec = PushFastMsgFragment(header, position);
605 vec = PushCompleteMsg(header, position, frame);
609 if (HandleMgntMsg(header.pgn, vec))
return;
613 auto payload = std::make_shared<std::vector<uint8_t>>(vec);
614 Nevent.SetPayload(payload);
615 AddPendingEvent(Nevent);
619bool isASCII(std::vector<unsigned char> packet) {
620 for (
unsigned char c : packet) {
621 if (!isascii(c))
return false;
626N2K_Format CommDriverN2KNet::DetectFormat(std::vector<unsigned char> packet) {
631 if (isASCII(packet)) {
632 std::string payload = std::string(packet.begin(), packet.end());
633 if (payload.find(
"$PCDIN") != std::string::npos) {
634 return N2KFormat_SeaSmart;
635 }
else if (payload.find(
"$MXPGN") != std::string::npos) {
639 return N2KFormat_MiniPlex;
640 }
else if (std::find(packet.begin(), packet.end(),
':') != packet.end()) {
641 return N2KFormat_Actisense_RAW_ASCII;
643 return N2KFormat_Actisense_N2K_ASCII;
646 if (packet[2] == 0x95)
647 return N2KFormat_Actisense_RAW;
648 else if (packet[2] == 0xd0)
649 return N2KFormat_Actisense_N2K;
650 else if (packet[2] == 0x93)
651 return N2KFormat_Actisense_NGT;
653 return N2KFormat_Undefined;
656bool CommDriverN2KNet::ProcessActisense_N2K(std::vector<unsigned char> packet) {
659 std::vector<unsigned char> data;
661 bool bGotESC =
false;
662 bool bGotSOT =
false;
664 while (!m_circle->empty()) {
665 uint8_t next_byte = m_circle->get();
669 if (next_byte == ESCAPE) {
670 data.push_back(next_byte);
672 }
else if (next_byte == ENDOFTEXT) {
676 unsigned int msg_length =
677 (uint32_t)data[1] + ((uint32_t)data[2] << 8);
680 if (msg_length == data.size() - 1) {
681 uint8_t destination = data[3];
682 uint8_t source = data[4];
684 uint8_t dprp = data[7];
687 uint8_t rAndDP = dprp & 3;
690 uint8_t pduFormat = data[6];
691 uint32_t pgn = (rAndDP << 16) + (pduFormat << 8);
697 std::vector<uint8_t> o_payload;
698 o_payload.push_back(0x93);
699 o_payload.push_back(0x13);
700 o_payload.push_back(priority);
701 o_payload.push_back(pgn & 0xFF);
702 o_payload.push_back((pgn >> 8) & 0xFF);
703 o_payload.push_back((pgn >> 16) & 0xFF);
704 o_payload.push_back(destination);
705 o_payload.push_back(source);
706 o_payload.push_back(0xFF);
707 o_payload.push_back(0xFF);
708 o_payload.push_back(0xFF);
709 o_payload.push_back(0xFF);
710 o_payload.push_back(data.size());
713 for (
size_t n = 13; n < data.size() - 1; n++)
714 o_payload.push_back(data[n]);
716 o_payload.push_back(0x55);
721 std::make_shared<std::vector<uint8_t>>(o_payload);
722 Nevent.SetPayload(n2k_payload);
723 AddPendingEvent(Nevent);
730 }
else if (next_byte == STARTOFTEXT) {
739 bGotESC = (next_byte == ESCAPE);
742 data.push_back(next_byte);
748 if (STARTOFTEXT == next_byte) {
754 bGotESC = (next_byte == ESCAPE);
759 data.push_back(next_byte);
768bool CommDriverN2KNet::ProcessActisense_RAW(std::vector<unsigned char> packet) {
773 std::vector<unsigned char> data;
775 bool bGotESC =
false;
776 bool bGotSOT =
false;
778 while (!m_circle->empty()) {
779 uint8_t next_byte = m_circle->get();
783 if (next_byte == ESCAPE) {
784 data.push_back(next_byte);
786 }
else if (next_byte == ENDOFTEXT) {
791 if (data.size() >= 8) {
792 size_t dLen = data[1];
794 if (dLen + 3 == data.size()) {
796 memcpy(&frame.can_id, &data.data()[4], 4);
799 memcpy(&frame.data, &data.data()[8], 8);
801 HandleCanFrameInput(frame);
809 }
else if (next_byte == STARTOFTEXT) {
818 bGotESC = (next_byte == ESCAPE);
821 data.push_back(next_byte);
827 if (STARTOFTEXT == next_byte) {
833 bGotESC = (next_byte == ESCAPE);
838 data.push_back(next_byte);
847bool CommDriverN2KNet::ProcessActisense_NGT(std::vector<unsigned char> packet) {
848 std::vector<unsigned char> data;
850 bool bGotESC =
false;
851 bool bGotSOT =
false;
853 while (!m_circle->empty()) {
854 uint8_t next_byte = m_circle->get();
858 if (next_byte == ESCAPE) {
859 data.push_back(next_byte);
861 }
else if (next_byte == ENDOFTEXT) {
864 auto n2k_payload = std::make_shared<std::vector<uint8_t>>(data);
865 Nevent.SetPayload(n2k_payload);
866 AddPendingEvent(Nevent);
872 }
else if (next_byte == STARTOFTEXT) {
881 bGotESC = (next_byte == ESCAPE);
884 data.push_back(next_byte);
890 if (STARTOFTEXT == next_byte) {
896 bGotESC = (next_byte == ESCAPE);
901 data.push_back(next_byte);
910bool CommDriverN2KNet::ProcessActisense_ASCII_RAW(
911 std::vector<unsigned char> packet) {
914 while (!m_circle->empty()) {
915 char b = m_circle->get();
916 if ((b != 0x0a) && (b != 0x0d)) {
924 wxString ss(m_sentence.c_str());
926 wxStringTokenizer tkz(ss,
" ");
929 wxString token = tkz.GetNextToken();
931 token = tkz.GetNextToken();
933 m_TX_flag = token[0];
936 token = tkz.GetNextToken();
938 token.ToLong(&canID, 16);
939 frame.can_id = canID;
942 unsigned char bytes[8];
944 for (
unsigned int i = 0; i < 8; i++) {
945 if (tkz.HasMoreTokens()) {
946 token = tkz.GetNextToken();
948 token.ToLong(&tui, 16);
949 bytes[i] = (uint8_t)tui;
952 memcpy(&frame.data, bytes, 8);
953 HandleCanFrameInput(frame);
959bool CommDriverN2KNet::ProcessActisense_ASCII_N2K(
960 std::vector<unsigned char> packet) {
962 std::string sentence;
964 while (!m_circle->empty()) {
965 char b = m_circle->get();
966 if ((b != 0x0a) && (b != 0x0d)) {
974 wxString ss(sentence.c_str());
975 wxStringTokenizer tkz(ss,
" ");
979 wxString time_header = tkz.GetNextToken();
981 wxString sprio_addr = tkz.GetNextToken();
983 sprio_addr.ToLong(&prio_addr, 16);
984 uint8_t priority = (uint8_t)prio_addr & 0X0F;
985 uint8_t destination = (uint8_t)(prio_addr >> 4) & 0X0FF;
986 uint8_t source = (uint8_t)(prio_addr >> 12) & 0X0FF;
989 wxString sPGN = tkz.GetNextToken();
991 sPGN.ToULong(&PGN, 16);
995 wxString sdata = tkz.GetNextToken();
996 std::vector<uint8_t> data;
997 for (
size_t i = 0; i < sdata.Length(); i += 2) {
999 wxString stui = sdata.Mid(i, 2);
1000 stui.ToLong(&dv, 16);
1001 data.push_back((uint8_t)dv);
1005 std::vector<uint8_t> o_payload;
1006 o_payload.push_back(0x93);
1007 o_payload.push_back(0x13);
1008 o_payload.push_back(priority);
1009 o_payload.push_back(PGN & 0xFF);
1010 o_payload.push_back((PGN >> 8) & 0xFF);
1011 o_payload.push_back((PGN >> 16) & 0xFF);
1012 o_payload.push_back(destination);
1013 o_payload.push_back(source);
1014 o_payload.push_back(0xFF);
1015 o_payload.push_back(0xFF);
1016 o_payload.push_back(0xFF);
1017 o_payload.push_back(0xFF);
1018 o_payload.push_back(data.size());
1019 for (
size_t n = 0; n < data.size(); n++) o_payload.push_back(data[n]);
1020 o_payload.push_back(0x55);
1022 if (HandleMgntMsg(PGN, o_payload))
return false;
1026 auto n2k_payload = std::make_shared<std::vector<uint8_t>>(o_payload);
1027 Nevent.SetPayload(n2k_payload);
1028 AddPendingEvent(Nevent);
1034bool CommDriverN2KNet::ProcessSeaSmart(std::vector<unsigned char> packet) {
1035 while (!m_circle->empty()) {
1036 char b = m_circle->get();
1037 if ((b != 0x0a) && (b != 0x0d)) {
1045 wxString ss(m_sentence.c_str());
1047 wxStringTokenizer tkz(ss,
",");
1050 wxString token = tkz.GetNextToken();
1053 token = tkz.GetNextToken();
1055 token.ToULong(&PGN, 16);
1057 token = tkz.GetNextToken();
1058 unsigned long timestamp;
1059 token.ToULong(×tamp, 16);
1061 token = tkz.GetNextToken();
1062 unsigned long source;
1063 token.ToULong(&source, 16);
1065 token = tkz.GetNextToken();
1067 wxStringTokenizer datatkz(token,
"*");
1068 wxString data = datatkz.GetNextToken();
1071 std::vector<uint8_t> o_payload;
1072 o_payload.push_back(0x93);
1073 o_payload.push_back(0x13);
1074 o_payload.push_back(3);
1075 o_payload.push_back(PGN & 0xFF);
1076 o_payload.push_back((PGN >> 8) & 0xFF);
1077 o_payload.push_back((PGN >> 16) & 0xFF);
1078 o_payload.push_back(0xFF);
1079 o_payload.push_back((uint8_t)source);
1080 o_payload.push_back(timestamp & 0xFF);
1081 o_payload.push_back((timestamp >> 8) & 0xFF);
1082 o_payload.push_back((timestamp >> 16) & 0xFF);
1083 o_payload.push_back((timestamp >> 24) & 0xFF);
1084 o_payload.push_back((uint8_t)data.Length() / 2);
1085 for (
size_t i = 0; i < data.Length(); i += 2) {
1087 wxString sbyte = data.Mid(i, 2);
1088 sbyte.ToULong(&dv, 16);
1089 o_payload.push_back((uint8_t)dv);
1091 o_payload.push_back(0x55);
1093 if (HandleMgntMsg(PGN, o_payload))
return false;
1097 auto n2k_payload = std::make_shared<std::vector<uint8_t>>(o_payload);
1098 Nevent.SetPayload(n2k_payload);
1099 AddPendingEvent(Nevent);
1105bool CommDriverN2KNet::ProcessMiniPlex(std::vector<unsigned char> packet) {
1200 while (!m_circle->empty()) {
1201 char b = m_circle->get();
1202 if ((b != 0x0a) && (b != 0x0d)) {
1210 wxString ss(m_sentence.c_str());
1212 wxStringTokenizer tkz(ss,
",");
1215 wxString token = tkz.GetNextToken();
1218 token = tkz.GetNextToken();
1220 token.ToULong(&PGN, 16);
1222 token = tkz.GetNextToken();
1224 token.ToULong(&attr, 16);
1226 bool send_bit = (attr >> 15) != 0;
1228 uint8_t priority = (attr >> 12) & 0x07;
1231 uint8_t dlc = (attr >> 8) & 0x0F;
1234 uint8_t address = attr & 0xFF;
1236 token = tkz.GetNextToken();
1238 wxStringTokenizer datatkz(token,
"*");
1239 wxString data = datatkz.GetNextToken();
1247 memset(&frame.data, 0, 8);
1248 for (
size_t i = 0; i < data.Length(); i += 2) {
1250 wxString sbyte = data.Mid(data.Length() - i - 2, 2);
1251 sbyte.ToULong(&dv, 16);
1252 frame.data[i / 2] = ((uint8_t)dv);
1254 frame.can_id = (uint32_t)BuildCanID(priority, address, 0xFF, PGN);
1255 HandleCanFrameInput(frame);
1262#define RD_BUF_SIZE 4096
1265 switch (event.GetSocketEvent()) {
1266 case wxSOCKET_INPUT: {
1280 std::vector<unsigned char> data(RD_BUF_SIZE + 1);
1282 uint8_t next_byte = 0;
1284 event.GetSocket()->Read(&data.front(), RD_BUF_SIZE);
1285 if (!event.GetSocket()->Error()) {
1286 m_driver_stats.available =
true;
1287 size_t count =
event.GetSocket()->LastCount();
1300 for (
int i = 0; i < newdata; i++) {
1301 m_circle->put(data[i]);
1306 m_n2k_format = DetectFormat(data);
1308 switch (m_n2k_format) {
1309 case N2KFormat_Actisense_RAW_ASCII:
1310 ProcessActisense_ASCII_RAW(data);
1312 case N2KFormat_YD_RAW:
1313 ProcessActisense_ASCII_RAW(data);
1315 case N2KFormat_Actisense_N2K_ASCII:
1316 ProcessActisense_ASCII_N2K(data);
1318 case N2KFormat_Actisense_N2K:
1319 ProcessActisense_N2K(data);
1321 case N2KFormat_Actisense_RAW:
1322 ProcessActisense_RAW(data);
1324 case N2KFormat_Actisense_NGT:
1325 ProcessActisense_NGT(data);
1327 case N2KFormat_SeaSmart:
1328 ProcessSeaSmart(data);
1330 case N2KFormat_MiniPlex:
1331 ProcessMiniPlex(data);
1333 case N2KFormat_Undefined:
1340 m_dog_value = N_DOG_TIMEOUT;
1344 case wxSOCKET_LOST: {
1345 m_driver_stats.available =
false;
1346 if (GetProtocol() == TCP || GetProtocol() == GPSD) {
1347 if (GetBrxConnectEvent())
1348 wxLogMessage(wxString::Format(
"NetworkDataStream connection lost: %s",
1349 GetPort().c_str()));
1350 if (GetSockServer()) {
1351 GetSock()->Destroy();
1355 wxDateTime now = wxDateTime::Now();
1356 wxTimeSpan since_connect(
1358 if (GetConnectTime().IsValid()) since_connect = now - GetConnectTime();
1360 int retry_time = 5000;
1366 if (!GetBrxConnectEvent() && (since_connect.GetSeconds() < 5))
1369 GetSocketThreadWatchdogTimer()->Stop();
1370 GetSocketTimer()->Start(
1371 retry_time, wxTIMER_ONE_SHOT);
1376 case wxSOCKET_CONNECTION: {
1377 m_driver_stats.available =
true;
1378 if (GetProtocol() == GPSD) {
1382 char cmd[] =
"?WATCH={\"class\":\"WATCH\", \"nmea\":true}";
1383 GetSock()->Write(cmd, strlen(cmd));
1384 }
else if (GetProtocol() == TCP) {
1386 wxString::Format(
"TCP NetworkDataStream connection established: %s",
1387 GetPort().c_str()));
1388 m_dog_value = N_DOG_TIMEOUT;
1389 if (GetPortType() != DS_TYPE_OUTPUT) {
1391 if (GetParams().NoDataReconnect)
1392 GetSocketThreadWatchdogTimer()->Start(1000);
1394 if (GetPortType() != DS_TYPE_INPUT && GetSock()->IsOk())
1395 (void)SetOutputSocketOptions(GetSock());
1396 GetSocketTimer()->Stop();
1397 SetBrxConnectEvent(
true);
1400 SetConnectTime(wxDateTime::Now());
1409void CommDriverN2KNet::OnServerSocketEvent(wxSocketEvent& event) {
1410 switch (event.GetSocketEvent()) {
1411 case wxSOCKET_CONNECTION: {
1412 m_driver_stats.available =
true;
1413 SetSock(GetSockServer()->Accept(
false));
1416 GetSock()->SetTimeout(2);
1418 GetSock()->SetEventHandler(*
this, DS_SOCKET_ID);
1419 int notify_flags = (wxSOCKET_CONNECTION_FLAG | wxSOCKET_LOST_FLAG);
1420 if (GetPortType() != DS_TYPE_INPUT) {
1421 notify_flags |= wxSOCKET_OUTPUT_FLAG;
1422 (void)SetOutputSocketOptions(GetSock());
1424 if (GetPortType() != DS_TYPE_OUTPUT)
1425 notify_flags |= wxSOCKET_INPUT_FLAG;
1426 GetSock()->SetNotify(notify_flags);
1427 GetSock()->Notify(
true);
1438std::vector<unsigned char> MakeSimpleOutMsg(
1439 int data_format,
int pgn, std::vector<unsigned char>& payload) {
1440 std::vector<unsigned char> out_vec;
1442 switch (data_format) {
1443 case N2KFormat_YD_RAW:
1444 case N2KFormat_Actisense_RAW_ASCII: {
1446 unsigned can_id = BuildCanID(6, 0xff, 0xff, pgn);
1447 std::stringstream ss;
1448 ss << std::setfill(
'0') << std::setw(8) << std::hex << can_id;
1449 for (
unsigned char s : ss.str()) out_vec.push_back(s);
1450 out_vec.push_back(
' ');
1455 for (
unsigned char d : payload) {
1456 snprintf(tv, 4,
"%02X ", d);
1459 for (
unsigned char s : sspl) out_vec.push_back(s);
1463 out_vec.push_back(0x0d);
1464 out_vec.push_back(0x0a);
1467 case N2KFormat_Actisense_N2K_ASCII: {
1469 wxDateTime now = wxDateTime::Now();
1470 wxString stime = now.Format(
"%H%M%S");
1472 std::string sstime = stime.ToStdString();
1473 out_vec.push_back(
'A');
1474 for (
unsigned char s : sstime) out_vec.push_back(s);
1478 sdp.Printf(
"%02X%02X%1X ",
1480 (
unsigned char)0xFF, 0x6);
1481 std::string ssdp = sdp.ToStdString();
1482 for (
unsigned char s : ssdp) out_vec.push_back(s);
1486 spgn.Printf(
"%05X ", pgn);
1487 std::string sspgn = spgn.ToStdString();
1488 for (
unsigned char s : sspgn) out_vec.push_back(s);
1493 for (
unsigned char d : payload) {
1494 snprintf(tv, 3,
"%02X", d);
1497 for (
unsigned char s : sspl) out_vec.push_back(s);
1500 out_vec.push_back(0x0d);
1501 out_vec.push_back(0x0a);
1504 case N2KFormat_MiniPlex: {
1505 out_vec.push_back(
'$');
1506 out_vec.push_back(
'M');
1507 out_vec.push_back(
'X');
1508 out_vec.push_back(
'P');
1509 out_vec.push_back(
'G');
1510 out_vec.push_back(
'N');
1511 out_vec.push_back(
',');
1514 spgn.Printf(
"%06X,", pgn);
1515 std::string sspgn = spgn.ToStdString();
1516 for (
unsigned char c : sspgn) {
1517 out_vec.push_back(c);
1523 attr |= ((uint16_t)0x06) << 12;
1524 attr |= ((uint16_t)payload.size()) << 8;
1525 attr |= (uint16_t)0xFF;
1529 sattr.Printf(
"%04X,", attr);
1530 std::string ssattr = sattr.ToStdString();
1531 for (
unsigned char c : ssattr) {
1532 out_vec.push_back(c);
1536 for (
auto rit = payload.rbegin(); rit != payload.rend(); ++rit) {
1537 snprintf(tv, 3,
"%02X", *rit);
1538 out_vec.push_back(tv[0]);
1539 out_vec.push_back(tv[1]);
1543 for (
auto ci = ++out_vec.begin(); ci != out_vec.end(); ci++) {
1546 out_vec.push_back(
'*');
1547 snprintf(tv, 3,
"%02X", crc);
1548 out_vec.push_back(tv[0]);
1549 out_vec.push_back(tv[1]);
1552 out_vec.push_back(0x0d);
1553 out_vec.push_back(0x0a);
1564std::vector<std::vector<unsigned char>> CommDriverN2KNet::GetTxVector(
1565 const std::shared_ptr<const Nmea2000Msg>& msg,
1566 std::shared_ptr<const NavAddr2000> dest_addr) {
1567 std::vector<std::vector<unsigned char>> tx_vector;
1570 switch (m_n2k_format) {
1571 case N2KFormat_YD_RAW:
1573 case N2KFormat_Actisense_RAW_ASCII: {
1575 if (!IsFastMessagePGN(msg->PGN.pgn) && msg->payload.size() <= 8) {
1577 std::vector<unsigned char> header_vec;
1578 std::vector<unsigned char> out_vec;
1583 unsigned long can_id =
1584 BuildCanID(msg->priority, 0, dest_addr->address, msg->PGN.pgn);
1586 std::stringstream ss;
1587 ss << std::setfill(
'0') << std::setw(8) << std::hex << can_id;
1588 for (
unsigned char s : ss.str()) header_vec.push_back(s);
1589 header_vec.push_back(
' ');
1592 for (
unsigned char s : header_vec) out_vec.push_back(s);
1596 for (
unsigned int k = 0; k < msg->payload.size(); k++) {
1598 snprintf(tb, 4,
"%02X ", msg->payload.data()[k]);
1601 for (
unsigned char s : ssdata) out_vec.push_back(s);
1604 out_vec.push_back(0x0d);
1605 out_vec.push_back(0x0a);
1607 tx_vector.push_back(out_vec);
1609 std::vector<unsigned char> header_vec;
1610 std::vector<unsigned char> out_vec;
1615 wxDateTime now = wxDateTime::Now();
1616 wxString stime = now.Format(
"%H:%M:%S");
1618 std::string sstime = stime.ToStdString();
1619 for (
unsigned char s : sstime) header_vec.push_back(s);
1622 header_vec.push_back(
'T');
1623 header_vec.push_back(
' ');
1629 unsigned long can_id =
1630 BuildCanID(msg->priority, 0, dest_addr->address, msg->PGN.pgn);
1631 std::stringstream ss;
1632 ss << std::setfill(
'0') << std::setw(8) << std::hex << can_id;
1633 for (
unsigned char s : ss.str()) header_vec.push_back(s);
1634 header_vec.push_back(
' ');
1637 int payload_size = msg->payload.size();
1638 unsigned char temp[8];
1641 (payload_size > 6 ? (payload_size - 6 - 1) / 7 + 1 + 1 : 1);
1643 for (
int i = 0; i < nframes && result; i++) {
1644 temp[0] = i | m_order;
1646 temp[1] = msg->payload.size();
1648 for (
int j = 2; j < 8; j++) {
1649 temp[j] = msg->payload.data()[cur];
1655 for (; j < 8 && cur < payload_size; j++) {
1656 temp[j] = msg->payload.data()[cur];
1659 for (; j < 8; j++) {
1667 for (
unsigned char s : header_vec) out_vec.push_back(s);
1671 for (
unsigned int k = 0; k < 8; k++) {
1673 snprintf(tb, 4,
"%02X ", temp[k]);
1676 for (
unsigned char s : ssdata) out_vec.push_back(s);
1679 out_vec.push_back(0x0d);
1680 out_vec.push_back(0x0a);
1682 tx_vector.push_back(out_vec);
1686 case N2KFormat_Actisense_N2K_ASCII: {
1703 std::vector<unsigned char> ovec;
1706 wxDateTime now = wxDateTime::Now();
1707 wxString stime = now.Format(
"%H%M%S");
1709 std::string sstime = stime.ToStdString();
1710 ovec.push_back(
'A');
1711 for (
unsigned char s : sstime) ovec.push_back(s);
1715 sdp.Printf(
"%02X%02X%1X ",
1717 (
unsigned char)dest_addr->address,
1718 (
unsigned char)msg->priority);
1719 std::string ssdp = sdp.ToStdString();
1720 for (
unsigned char s : ssdp) ovec.push_back(s);
1724 spgn.Printf(
"%05X ", (
int)msg->PGN.pgn);
1725 std::string sspgn = spgn.ToStdString();
1726 for (
unsigned char s : sspgn) ovec.push_back(s);
1731 for (
unsigned char d : msg->payload) {
1732 snprintf(tv, 3,
"%02X", d);
1735 for (
unsigned char s : sspl) ovec.push_back(s);
1738 ovec.push_back(0x0d);
1739 ovec.push_back(0x0a);
1742 tx_vector.push_back(ovec);
1746 case N2KFormat_MiniPlex: {
1747 std::vector<unsigned char> ovec;
1748 if (!IsFastMessagePGN(msg->PGN.pgn) && msg->payload.size() < 8) {
1753 (msg->payload.size() > 6 ? (msg->payload.size() - 6 - 1) / 7 + 1 + 1
1755 for (
size_t i = 0; i < nframes; i++) {
1756 ovec.push_back(
'$');
1757 ovec.push_back(
'M');
1758 ovec.push_back(
'X');
1759 ovec.push_back(
'P');
1760 ovec.push_back(
'G');
1761 ovec.push_back(
'N');
1762 ovec.push_back(
',');
1765 spgn.Printf(
"%06X,", (
int)msg->PGN.pgn);
1766 std::string sspgn = spgn.ToStdString();
1767 for (
unsigned char c : sspgn) {
1773 if (i == nframes - 1) {
1774 len = msg->payload.size() + 1 - 6 - (nframes - 2) * 7;
1776 attr |= ((uint16_t)((uint8_t)msg->priority & 0x07)) << 12;
1777 attr |= ((uint16_t)len) << 8;
1778 attr |= (uint16_t)dest_addr->address;
1782 sattr.Printf(
"%04X,", attr);
1783 std::string ssattr = sattr.ToStdString();
1784 for (
unsigned char c : ssattr) {
1789 uint8_t databytes = i == 0 ? len - 2 : len - 1;
1790 std::vector<unsigned char> payload;
1791 for (uint8_t j = 0; j < databytes; j++) {
1792 payload.push_back(msg->payload[cur]);
1795 for (
auto rit = payload.rbegin(); rit != payload.rend(); ++rit) {
1796 snprintf(tv, 3,
"%02X", *rit);
1797 ovec.push_back(tv[0]);
1798 ovec.push_back(tv[1]);
1801 snprintf(tv, 3,
"%02X", (uint8_t)msg->payload.size());
1802 ovec.push_back(tv[0]);
1803 ovec.push_back(tv[1]);
1806 snprintf(tv, 3,
"%02X", (uint8_t)i | m_order);
1807 ovec.push_back(tv[0]);
1808 ovec.push_back(tv[1]);
1812 for (
auto ci = ++ovec.begin(); ci != ovec.end(); ci++) {
1815 ovec.push_back(
'*');
1816 snprintf(tv, 3,
"%02X", crc);
1817 ovec.push_back(tv[0]);
1818 ovec.push_back(tv[1]);
1821 ovec.push_back(0x0d);
1822 ovec.push_back(0x0a);
1828 tx_vector.push_back(ovec);
1835 case N2KFormat_Actisense_N2K:
1837 case N2KFormat_Actisense_RAW:
1839 case N2KFormat_Actisense_NGT:
1841 case N2KFormat_SeaSmart:
1852bool CommDriverN2KNet::PrepareForTX() {
1862 bool b_found =
false;
1869 if (m_n2k_format == N2KFormat_Actisense_N2K_ASCII)
return true;
1876 if (m_n2k_format == N2KFormat_MiniPlex)
return true;
1881 if (m_n2k_format == N2KFormat_SeaSmart)
return false;
1894 std::vector<unsigned char> payload;
1895 payload.push_back(0x14);
1896 payload.push_back(0xF0);
1897 payload.push_back(0x01);
1899 std::vector<std::vector<unsigned char>> out_data;
1900 std::vector<unsigned char> msg_vec =
1901 MakeSimpleOutMsg(N2KFormat_YD_RAW, 59904, payload);
1902 out_data.push_back(msg_vec);
1903 SendSentenceNetwork(out_data);
1906 m_prodinfo_timer.Start(200,
true);
1913bool CommDriverN2KNet::SendN2KNetwork(std::shared_ptr<const Nmea2000Msg>& msg,
1914 std::shared_ptr<const NavAddr2000> addr) {
1917 std::vector<std::vector<unsigned char>> out_data = GetTxVector(msg, addr);
1918 SendSentenceNetwork(out_data);
1919 m_driver_stats.
tx_count += msg->payload.size();
1922 std::vector<unsigned char> msg_payload = PrepareLogPayload(msg, addr);
1924 std::make_shared<const Nmea2000Msg>(msg->PGN.pgn, msg_payload, addr);
1925 auto msg_all = std::make_shared<const Nmea2000Msg>(1, msg_payload, addr);
1928 m_listener.
Notify(std::move(msg_one));
1929 m_listener.
Notify(std::move(msg_all));
1934bool CommDriverN2KNet::SendSentenceNetwork(
1935 std::vector<std::vector<unsigned char>> payload) {
1942 wxDatagramSocket* udp_socket;
1943 switch (GetProtocol()) {
1945 for (std::vector<unsigned char>& v : payload) {
1946 if (GetSock() && GetSock()->IsOk()) {
1947 m_driver_stats.available =
true;
1949 GetSock()->Write(v.data(), v.size());
1950 m_dog_value = N_DOG_TIMEOUT;
1951 if (GetSock()->Error()) {
1952 if (GetSockServer()) {
1953 GetSock()->Destroy();
1956 wxSocketClient* tcp_socket =
1957 dynamic_cast<wxSocketClient*
>(GetSock());
1958 if (tcp_socket) tcp_socket->Close();
1959 if (!GetSocketTimer()->IsRunning())
1960 GetSocketTimer()->Start(
1961 5000, wxTIMER_ONE_SHOT);
1962 GetSocketThreadWatchdogTimer()->Stop();
1968 m_driver_stats.available =
false;
1975 udp_socket =
dynamic_cast<wxDatagramSocket*
>(GetTSock());
1976 if (udp_socket && udp_socket->IsOk()) {
1977 udp_socket->SendTo(GetAddr(), payload.mb_str(), payload.size());
1978 if (udp_socket->Error()) ret =
false;
1993void CommDriverN2KNet::Close() {
1994 wxLogMessage(wxString::Format(
"Closing NMEA NetworkDataStream %s",
1995 GetNetPort().c_str()));
1996 m_stats_timer.Stop();
2000 m_sock->SetOption(IPPROTO_IP, IP_DROP_MEMBERSHIP, &m_mrq_container->m_mrq,
2001 sizeof(m_mrq_container->m_mrq));
2002 m_sock->Notify(FALSE);
2004 m_driver_stats.available =
false;
2008 m_tsock->Notify(FALSE);
2012 if (m_socket_server) {
2013 m_socket_server->Notify(FALSE);
2014 m_socket_server->Destroy();
2017 m_socket_timer.Stop();
2018 m_socketread_watchdog_timer.Stop();
2021bool CommDriverN2KNet::SetOutputSocketOptions(wxSocketBase* tsock) {
2030 int nagleDisable = 1;
2031 ret = tsock->SetOption(IPPROTO_TCP, TCP_NODELAY, &nagleDisable,
2032 sizeof(nagleDisable));
2038 unsigned long outbuf_size = 1024;
2039 return (tsock->SetOption(SOL_SOCKET, SO_SNDBUF, &outbuf_size,
2040 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.