34#include <wx/msw/winundef.h>
44#include <wx/tokenzr.h>
45#include <wx/datetime.h>
55#include <netinet/tcp.h>
62#include <wx/chartype.h>
64#include <wx/sckaddr.h>
66#include "model/comm_drv_n2k_net.h"
68#include "model/idents.h"
71#define N_DOG_TIMEOUT 8
73static const int kNotFound = -1;
78 void SetMrqAddr(
unsigned int addr) {
79 m_mrq.imr_multiaddr.s_addr = addr;
80 m_mrq.imr_interface.s_addr = INADDR_ANY;
86circular_buffer::circular_buffer(
size_t size)
87 : buf_(std::unique_ptr<unsigned char[]>(new unsigned char[size])),
99bool circular_buffer::empty()
const {
101 return (!full_ && (head_ == tail_));
104bool circular_buffer::full()
const {
109void circular_buffer::put(
unsigned char item) {
110 std::lock_guard<std::mutex> lock(mutex_);
112 if (full_) tail_ = (tail_ + 1) % max_size_;
114 head_ = (head_ + 1) % max_size_;
116 full_ = head_ == tail_;
119unsigned char circular_buffer::get() {
120 std::lock_guard<std::mutex> lock(mutex_);
122 if (empty())
return 0;
125 auto val = buf_[tail_];
127 tail_ = (tail_ + 1) % max_size_;
134 : priority(
'\0'), source(
'\0'), destination(
'\0'), pgn(-1) {};
144 : wxEvent(
id, commandType) {};
148 void SetPayload(std::shared_ptr<std::vector<unsigned char>> data) {
151 std::shared_ptr<std::vector<unsigned char>> GetPayload() {
return m_payload; }
154 wxEvent* Clone()
const {
156 newevent->m_payload = this->m_payload;
161 std::shared_ptr<std::vector<unsigned char>> m_payload;
164static uint64_t PayloadToName(
const std::vector<unsigned char> payload) {
166 memcpy(&name,
reinterpret_cast<const void*
>(payload.data()),
sizeof(name));
174#define TIMER_SOCKET_N2KNET 7339
177EVT_TIMER(TIMER_SOCKET_N2KNET, CommDriverN2KNet::OnTimerSocket)
179EVT_SOCKET(DS_SERVERSOCKET_ID, CommDriverN2KNet::OnServerSocketEvent)
180EVT_TIMER(TIMER_SOCKET_N2KNET + 1, CommDriverN2KNet::OnSocketReadWatchdogTimer)
189 m_listener(listener),
190 m_net_port(wxString::Format("%i", params->NetworkPort)),
191 m_net_protocol(params->NetProtocol),
194 m_socket_server(NULL),
195 m_is_multicast(false),
197 m_portstring(params->GetDSPort()),
198 m_io_select(params->IOSelect),
199 m_connection_type(params->Type),
201 m_TX_available(false)
204 m_addr.Hostname(params->NetworkAddress);
205 m_addr.Service(params->NetworkPort);
207 m_socket_timer.SetOwner(
this, TIMER_SOCKET_N2KNET);
208 m_socketread_watchdog_timer.SetOwner(
this, TIMER_SOCKET_N2KNET + 1);
209 this->attributes[
"netAddress"] = params->NetworkAddress.ToStdString();
211 sprintf(port_char,
"%d", params->NetworkPort);
212 this->attributes[
"netPort"] = std::string(port_char);
213 this->attributes[
"userComment"] = params->UserComment.ToStdString();
214 this->attributes[
"ioDirection"] = std::string(
"IN/OUT");
217 Bind(wxEVT_COMMDRIVER_N2K_NET, &CommDriverN2KNet::handle_N2K_MSG,
this);
219 m_prodinfo_timer.Connect(
220 wxEVT_TIMER, wxTimerEventHandler(CommDriverN2KNet::OnProdInfoTimer), NULL,
228 rx_buffer =
new unsigned char[RX_BUFFER_SIZE_NET + 1];
233 m_n2k_format = N2KFormat_YD_RAW;
236 resume_listener.Init(SystemEvents::GetInstance().evt_resume,
242CommDriverN2KNet::~CommDriverN2KNet() {
243 delete m_mrq_container;
251 std::string Model_ID;
255std::unordered_map<uint8_t, product_info> prod_info_map;
257bool CommDriverN2KNet::HandleMgntMsg(uint64_t pgn,
258 std::vector<unsigned char>& payload) {
260 auto name = PayloadToName(payload);
262 std::make_shared<const Nmea2000Msg>(pgn, payload, GetAddress(name));
264 bool b_handled =
false;
267 uint8_t src_addr = payload.at(7);
268 if (src_addr == 75)
return false;
270 pr_info.Model_ID = std::string((
char*)&payload.data()[17], 32);
271 pr_info.RT_flag = m_TX_flag;
273 prod_info_map[src_addr] = pr_info;
278 uint8_t src_addr = payload.at(7);
288void CommDriverN2KNet::OnProdInfoTimer(wxTimerEvent& ev) {
290 bool b_found =
false;
291 for (
const auto& [key, value] : prod_info_map) {
292 auto prod_info = value;
293 if (prod_info.Model_ID.find(
"YDEN") != std::string::npos) {
298 if (prod_info.RT_flag ==
'T') b_found =
true;
303 if (b_found) m_TX_available =
true;
304 prod_info_map.clear();
308 auto p =
event.GetPayload();
309 std::vector<unsigned char>* payload = p.get();
313 unsigned char* c = (
unsigned char*)&pgn;
314 *c++ = payload->at(3);
315 *c++ = payload->at(4);
316 *c++ = payload->at(5);
320 auto name = PayloadToName(*payload);
322 std::make_shared<const Nmea2000Msg>(pgn, *payload, GetAddress(name));
324 std::make_shared<const Nmea2000Msg>(1, *payload, GetAddress(name));
326 m_listener.
Notify(std::move(msg));
327 m_listener.
Notify(std::move(msg_all));
330void CommDriverN2KNet::Open(
void) {
332#if wxCHECK_VERSION(3, 0, 0)
334 ((
struct sockaddr_in*)GetAddr().GetAddressData())->sin_addr.s_addr;
337 ((
struct sockaddr_in*)GetAddr().GetAddress()->m_addr)->sin_addr.s_addr;
340 unsigned int addr = inet_addr(GetAddr().IPAddress().mb_str());
343 switch (m_net_protocol) {
345 OpenNetworkTCP(addr);
349 OpenNetworkUDP(addr);
358void CommDriverN2KNet::OpenNetworkUDP(
unsigned int addr) {
359 if (GetPortType() != DS_TYPE_OUTPUT) {
362 wxIPV4address conn_addr;
363 conn_addr.Service(GetNetPort());
364 conn_addr.AnyAddress();
366 new wxDatagramSocket(conn_addr, wxSOCKET_NOWAIT | wxSOCKET_REUSEADDR));
369 if ((ntohl(addr) & 0xf0000000) == 0xe0000000) {
371 m_mrq_container->SetMrqAddr(addr);
372 GetSock()->SetOption(IPPROTO_IP, IP_ADD_MEMBERSHIP,
373 &m_mrq_container->m_mrq,
374 sizeof(m_mrq_container->m_mrq));
377 GetSock()->SetEventHandler(*
this, DS_SOCKET_ID);
379 GetSock()->SetNotify(wxSOCKET_CONNECTION_FLAG | wxSOCKET_INPUT_FLAG |
381 GetSock()->Notify(TRUE);
382 GetSock()->SetTimeout(1);
386 if (GetPortType() != DS_TYPE_INPUT) {
387 wxIPV4address tconn_addr;
388 tconn_addr.Service(0);
389 tconn_addr.AnyAddress();
391 new wxDatagramSocket(tconn_addr, wxSOCKET_NOWAIT | wxSOCKET_REUSEADDR));
396 if ((!GetMulticast()) && (GetAddr().IPAddress().EndsWith(_T(
"255")))) {
397 int broadcastEnable = 1;
398 bool bam = GetTSock()->SetOption(
399 SOL_SOCKET, SO_BROADCAST, &broadcastEnable,
sizeof(broadcastEnable));
404 SetConnectTime(wxDateTime::Now());
407void CommDriverN2KNet::OpenNetworkTCP(
unsigned int addr) {
408 int isServer = ((addr == INADDR_ANY) ? 1 : 0);
409 wxLogMessage(wxString::Format(_T(
"Opening TCP Server %d"), isServer));
412 SetSockServer(
new wxSocketServer(GetAddr(), wxSOCKET_REUSEADDR));
414 SetSock(
new wxSocketClient());
418 GetSockServer()->SetEventHandler(*
this, DS_SERVERSOCKET_ID);
419 GetSockServer()->SetNotify(wxSOCKET_CONNECTION_FLAG);
420 GetSockServer()->Notify(TRUE);
421 GetSockServer()->SetTimeout(1);
423 GetSock()->SetEventHandler(*
this, DS_SOCKET_ID);
424 int notify_flags = (wxSOCKET_CONNECTION_FLAG | wxSOCKET_LOST_FLAG);
425 if (GetPortType() != DS_TYPE_INPUT) notify_flags |= wxSOCKET_OUTPUT_FLAG;
426 if (GetPortType() != DS_TYPE_OUTPUT) notify_flags |= wxSOCKET_INPUT_FLAG;
427 GetSock()->SetNotify(notify_flags);
428 GetSock()->Notify(TRUE);
429 GetSock()->SetTimeout(1);
431 SetBrxConnectEvent(
false);
432 GetSocketTimer()->Start(100, wxTIMER_ONE_SHOT);
436 SetConnectTime(wxDateTime::Now());
439void CommDriverN2KNet::OnSocketReadWatchdogTimer(wxTimerEvent& event) {
442 if (m_dog_value <= 0) {
443 if (GetParams().NoDataReconnect) {
445 if (GetProtocol() == TCP) {
446 wxSocketClient* tcp_socket =
dynamic_cast<wxSocketClient*
>(GetSock());
447 if (tcp_socket) tcp_socket->Close();
449 int n_reconnect_delay = wxMax(N_DOG_TIMEOUT - 2, 2);
450 wxLogMessage(wxString::Format(
" Reconnection scheduled in %d seconds.",
452 GetSocketTimer()->Start(n_reconnect_delay * 1000, wxTIMER_ONE_SHOT);
455 GetSocketThreadWatchdogTimer()->Stop();
461void CommDriverN2KNet::OnTimerSocket() {
463 wxSocketClient* tcp_socket =
dynamic_cast<wxSocketClient*
>(GetSock());
465 if (tcp_socket->IsDisconnected()) {
466 wxLogDebug(
" Attempting reconnection...");
467 SetBrxConnectEvent(
false);
469 GetSocketThreadWatchdogTimer()->Stop();
470 tcp_socket->Connect(GetAddr(), FALSE);
473 int n_reconnect_delay = N_DOG_TIMEOUT;
474 GetSocketTimer()->Start(n_reconnect_delay * 1000, wxTIMER_ONE_SHOT);
479void CommDriverN2KNet::HandleResume() {
481 wxSocketClient* tcp_socket =
dynamic_cast<wxSocketClient*
>(GetSock());
483 GetSocketThreadWatchdogTimer()->Stop();
488 int n_reconnect_delay = wxMax(N_DOG_TIMEOUT - 2, 2);
489 wxLogMessage(wxString::Format(
" Reconnection scheduled in %d seconds.",
492 GetSocketTimer()->Start(n_reconnect_delay * 1000, wxTIMER_ONE_SHOT);
496bool CommDriverN2KNet::SendMessage(std::shared_ptr<const NavMsg> msg,
497 std::shared_ptr<const NavAddr> addr) {
498 auto msg_n2k = std::dynamic_pointer_cast<const Nmea2000Msg>(msg);
499 auto dest_addr_n2k = std::static_pointer_cast<const NavAddr2000>(addr);
500 return SendN2KNetwork(msg_n2k, dest_addr_n2k);
503std::vector<unsigned char> CommDriverN2KNet::PrepareLogPayload(
504 std::shared_ptr<const Nmea2000Msg>& msg,
505 std::shared_ptr<const NavAddr2000> addr) {
506 std::vector<unsigned char> data;
507 data.push_back(0x94);
508 data.push_back(0x13);
509 data.push_back(msg->priority);
510 data.push_back(msg->PGN.pgn & 0xFF);
511 data.push_back((msg->PGN.pgn >> 8) & 0xFF);
512 data.push_back((msg->PGN.pgn >> 16) & 0xFF);
513 data.push_back(addr->address);
514 data.push_back(addr->address);
515 for (
size_t n = 0; n < msg->payload.size(); n++)
516 data.push_back(msg->payload[n]);
517 data.push_back(0x55);
521std::vector<unsigned char> CommDriverN2KNet::PushCompleteMsg(
522 const CanHeader header,
int position,
const can_frame frame) {
523 std::vector<unsigned char> data;
524 data.push_back(0x93);
525 data.push_back(0x13);
526 data.push_back(header.priority);
527 data.push_back(header.pgn & 0xFF);
528 data.push_back((header.pgn >> 8) & 0xFF);
529 data.push_back((header.pgn >> 16) & 0xFF);
530 data.push_back(header.destination);
531 data.push_back(header.source);
532 data.push_back(0xFF);
533 data.push_back(0xFF);
534 data.push_back(0xFF);
535 data.push_back(0xFF);
536 data.push_back(CAN_MAX_DLEN);
537 for (
size_t n = 0; n < CAN_MAX_DLEN; n++) data.push_back(frame.data[n]);
538 data.push_back(0x55);
542std::vector<unsigned char> CommDriverN2KNet::PushFastMsgFragment(
544 std::vector<unsigned char> data;
545 data.push_back(0x93);
546 data.push_back(fast_messages->entries[position].expected_length + 11);
547 data.push_back(header.priority);
548 data.push_back(header.pgn & 0xFF);
549 data.push_back((header.pgn >> 8) & 0xFF);
550 data.push_back((header.pgn >> 16) & 0xFF);
551 data.push_back(header.destination);
552 data.push_back(header.source);
553 data.push_back(0xFF);
554 data.push_back(0xFF);
555 data.push_back(0xFF);
556 data.push_back(0xFF);
557 data.push_back(fast_messages->entries[position].expected_length);
558 for (
size_t n = 0; n < fast_messages->entries[position].expected_length; n++)
559 data.push_back(fast_messages->entries[position].data[n]);
560 data.push_back(0x55);
561 fast_messages->
Remove(position);
571void CommDriverN2KNet::HandleCanFrameInput(can_frame frame) {
578 if (position == kNotFound) {
585 if ((frame.data[0] & 0x1F) == 0) {
587 ready = fast_messages->
InsertEntry(header, frame.data, position);
592 ready = fast_messages->
AppendEntry(header, frame.data, position);
596 std::vector<unsigned char> vec;
599 vec = PushFastMsgFragment(header, position);
602 vec = PushCompleteMsg(header, position, frame);
606 if (HandleMgntMsg(header.pgn, vec))
return;
610 auto payload = std::make_shared<std::vector<uint8_t>>(vec);
611 Nevent.SetPayload(payload);
612 AddPendingEvent(Nevent);
616bool isASCII(std::vector<unsigned char> packet) {
617 for (
unsigned char c : packet) {
618 if (!isascii(c))
return false;
623N2K_Format CommDriverN2KNet::DetectFormat(std::vector<unsigned char> packet) {
628 if (isASCII(packet)) {
629 std::string payload = std::string(packet.begin(), packet.end());
630 if (payload.find(
"$PCDIN") != std::string::npos) {
631 return N2KFormat_SeaSmart;
632 }
else if (payload.find(
"$MXPGN") != std::string::npos) {
636 return N2KFormat_MiniPlex;
637 }
else if (std::find(packet.begin(), packet.end(),
':') != packet.end()) {
638 return N2KFormat_Actisense_RAW_ASCII;
640 return N2KFormat_Actisense_N2K_ASCII;
643 if (packet[2] == 0x95)
644 return N2KFormat_Actisense_RAW;
645 else if (packet[2] == 0xd0)
646 return N2KFormat_Actisense_N2K;
647 else if (packet[2] == 0x93)
648 return N2KFormat_Actisense_NGT;
650 return N2KFormat_Undefined;
653bool CommDriverN2KNet::ProcessActisense_N2K(std::vector<unsigned char> packet) {
656 std::vector<unsigned char> data;
658 bool bGotESC =
false;
659 bool bGotSOT =
false;
661 while (!m_circle->empty()) {
662 uint8_t next_byte = m_circle->get();
666 if (next_byte == ESCAPE) {
667 data.push_back(next_byte);
669 }
else if (next_byte == ENDOFTEXT) {
673 unsigned int msg_length =
674 (uint32_t)data[1] + ((uint32_t)data[2] << 8);
677 if (msg_length == data.size() - 1) {
678 uint8_t destination = data[3];
679 uint8_t source = data[4];
681 uint8_t dprp = data[7];
684 uint8_t rAndDP = dprp & 3;
687 uint8_t pduFormat = data[6];
688 uint32_t pgn = (rAndDP << 16) + (pduFormat << 8);
694 std::vector<uint8_t> o_payload;
695 o_payload.push_back(0x93);
696 o_payload.push_back(0x13);
697 o_payload.push_back(priority);
698 o_payload.push_back(pgn & 0xFF);
699 o_payload.push_back((pgn >> 8) & 0xFF);
700 o_payload.push_back((pgn >> 16) & 0xFF);
701 o_payload.push_back(destination);
702 o_payload.push_back(source);
703 o_payload.push_back(0xFF);
704 o_payload.push_back(0xFF);
705 o_payload.push_back(0xFF);
706 o_payload.push_back(0xFF);
707 o_payload.push_back(data.size());
710 for (
size_t n = 13; n < data.size() - 1; n++)
711 o_payload.push_back(data[n]);
713 o_payload.push_back(0x55);
718 std::make_shared<std::vector<uint8_t>>(o_payload);
719 Nevent.SetPayload(n2k_payload);
720 AddPendingEvent(Nevent);
727 }
else if (next_byte == STARTOFTEXT) {
736 bGotESC = (next_byte == ESCAPE);
739 data.push_back(next_byte);
745 if (STARTOFTEXT == next_byte) {
751 bGotESC = (next_byte == ESCAPE);
756 data.push_back(next_byte);
765bool CommDriverN2KNet::ProcessActisense_RAW(std::vector<unsigned char> packet) {
770 std::vector<unsigned char> data;
772 bool bGotESC =
false;
773 bool bGotSOT =
false;
775 while (!m_circle->empty()) {
776 uint8_t next_byte = m_circle->get();
780 if (next_byte == ESCAPE) {
781 data.push_back(next_byte);
783 }
else if (next_byte == ENDOFTEXT) {
788 if (data.size() >= 8) {
789 size_t dLen = data[1];
791 if (dLen + 3 == data.size()) {
793 memcpy(&frame.can_id, &data.data()[4], 4);
796 memcpy(&frame.data, &data.data()[8], 8);
798 HandleCanFrameInput(frame);
806 }
else if (next_byte == STARTOFTEXT) {
815 bGotESC = (next_byte == ESCAPE);
818 data.push_back(next_byte);
824 if (STARTOFTEXT == next_byte) {
830 bGotESC = (next_byte == ESCAPE);
835 data.push_back(next_byte);
844bool CommDriverN2KNet::ProcessActisense_NGT(std::vector<unsigned char> packet) {
845 std::vector<unsigned char> data;
847 bool bGotESC =
false;
848 bool bGotSOT =
false;
850 while (!m_circle->empty()) {
851 uint8_t next_byte = m_circle->get();
855 if (next_byte == ESCAPE) {
856 data.push_back(next_byte);
858 }
else if (next_byte == ENDOFTEXT) {
861 auto n2k_payload = std::make_shared<std::vector<uint8_t>>(data);
862 Nevent.SetPayload(n2k_payload);
863 AddPendingEvent(Nevent);
869 }
else if (next_byte == STARTOFTEXT) {
878 bGotESC = (next_byte == ESCAPE);
881 data.push_back(next_byte);
887 if (STARTOFTEXT == next_byte) {
893 bGotESC = (next_byte == ESCAPE);
898 data.push_back(next_byte);
907bool CommDriverN2KNet::ProcessActisense_ASCII_RAW(
908 std::vector<unsigned char> packet) {
911 while (!m_circle->empty()) {
912 char b = m_circle->get();
913 if ((b != 0x0a) && (b != 0x0d)) {
921 wxString ss(m_sentence.c_str());
923 wxStringTokenizer tkz(ss,
" ");
926 wxString token = tkz.GetNextToken();
928 token = tkz.GetNextToken();
930 m_TX_flag = token[0];
933 token = tkz.GetNextToken();
935 token.ToLong(&canID, 16);
936 frame.can_id = canID;
939 unsigned char bytes[8];
941 for (
unsigned int i = 0; i < 8; i++) {
942 if (tkz.HasMoreTokens()) {
943 token = tkz.GetNextToken();
945 token.ToLong(&tui, 16);
946 bytes[i] = (uint8_t)tui;
949 memcpy(&frame.data, bytes, 8);
950 HandleCanFrameInput(frame);
956bool CommDriverN2KNet::ProcessActisense_ASCII_N2K(
957 std::vector<unsigned char> packet) {
959 std::string sentence;
961 while (!m_circle->empty()) {
962 char b = m_circle->get();
963 if ((b != 0x0a) && (b != 0x0d)) {
971 wxString ss(sentence.c_str());
972 wxStringTokenizer tkz(ss,
" ");
976 wxString time_header = tkz.GetNextToken();
978 wxString sprio_addr = tkz.GetNextToken();
980 sprio_addr.ToLong(&prio_addr, 16);
981 uint8_t priority = (uint8_t)prio_addr & 0X0F;
982 uint8_t destination = (uint8_t)(prio_addr >> 4) & 0X0FF;
983 uint8_t source = (uint8_t)(prio_addr >> 12) & 0X0FF;
986 wxString sPGN = tkz.GetNextToken();
988 sPGN.ToULong(&PGN, 16);
992 wxString sdata = tkz.GetNextToken();
993 std::vector<uint8_t> data;
994 for (
size_t i = 0; i < sdata.Length(); i += 2) {
996 wxString stui = sdata.Mid(i, 2);
997 stui.ToLong(&dv, 16);
998 data.push_back((uint8_t)dv);
1002 std::vector<uint8_t> o_payload;
1003 o_payload.push_back(0x93);
1004 o_payload.push_back(0x13);
1005 o_payload.push_back(priority);
1006 o_payload.push_back(PGN & 0xFF);
1007 o_payload.push_back((PGN >> 8) & 0xFF);
1008 o_payload.push_back((PGN >> 16) & 0xFF);
1009 o_payload.push_back(destination);
1010 o_payload.push_back(source);
1011 o_payload.push_back(0xFF);
1012 o_payload.push_back(0xFF);
1013 o_payload.push_back(0xFF);
1014 o_payload.push_back(0xFF);
1015 o_payload.push_back(data.size());
1016 for (
size_t n = 0; n < data.size(); n++) o_payload.push_back(data[n]);
1017 o_payload.push_back(0x55);
1019 if (HandleMgntMsg(PGN, o_payload))
return false;
1023 auto n2k_payload = std::make_shared<std::vector<uint8_t>>(o_payload);
1024 Nevent.SetPayload(n2k_payload);
1025 AddPendingEvent(Nevent);
1031bool CommDriverN2KNet::ProcessSeaSmart(std::vector<unsigned char> packet) {
1032 while (!m_circle->empty()) {
1033 char b = m_circle->get();
1034 if ((b != 0x0a) && (b != 0x0d)) {
1042 wxString ss(m_sentence.c_str());
1044 wxStringTokenizer tkz(ss,
",");
1047 wxString token = tkz.GetNextToken();
1050 token = tkz.GetNextToken();
1052 token.ToULong(&PGN, 16);
1054 token = tkz.GetNextToken();
1055 unsigned long timestamp;
1056 token.ToULong(×tamp, 16);
1058 token = tkz.GetNextToken();
1059 unsigned long source;
1060 token.ToULong(&source, 16);
1062 token = tkz.GetNextToken();
1064 wxStringTokenizer datatkz(token,
"*");
1065 wxString data = datatkz.GetNextToken();
1068 std::vector<uint8_t> o_payload;
1069 o_payload.push_back(0x93);
1070 o_payload.push_back(0x13);
1071 o_payload.push_back(3);
1072 o_payload.push_back(PGN & 0xFF);
1073 o_payload.push_back((PGN >> 8) & 0xFF);
1074 o_payload.push_back((PGN >> 16) & 0xFF);
1075 o_payload.push_back(0xFF);
1076 o_payload.push_back((uint8_t)source);
1077 o_payload.push_back(timestamp & 0xFF);
1078 o_payload.push_back((timestamp >> 8) & 0xFF);
1079 o_payload.push_back((timestamp >> 16) & 0xFF);
1080 o_payload.push_back((timestamp >> 24) & 0xFF);
1081 o_payload.push_back((uint8_t)data.Length() / 2);
1082 for (
size_t i = 0; i < data.Length(); i += 2) {
1084 wxString sbyte = data.Mid(i, 2);
1085 sbyte.ToULong(&dv, 16);
1086 o_payload.push_back((uint8_t)dv);
1088 o_payload.push_back(0x55);
1090 if (HandleMgntMsg(PGN, o_payload))
return false;
1094 auto n2k_payload = std::make_shared<std::vector<uint8_t>>(o_payload);
1095 Nevent.SetPayload(n2k_payload);
1096 AddPendingEvent(Nevent);
1102bool CommDriverN2KNet::ProcessMiniPlex(std::vector<unsigned char> packet) {
1197 while (!m_circle->empty()) {
1198 char b = m_circle->get();
1199 if ((b != 0x0a) && (b != 0x0d)) {
1207 wxString ss(m_sentence.c_str());
1209 wxStringTokenizer tkz(ss,
",");
1212 wxString token = tkz.GetNextToken();
1215 token = tkz.GetNextToken();
1217 token.ToULong(&PGN, 16);
1219 token = tkz.GetNextToken();
1221 token.ToULong(&attr, 16);
1223 bool send_bit = (attr >> 15) != 0;
1225 uint8_t priority = (attr >> 12) & 0x07;
1228 uint8_t dlc = (attr >> 8) & 0x0F;
1231 uint8_t address = attr & 0xFF;
1233 token = tkz.GetNextToken();
1235 wxStringTokenizer datatkz(token,
"*");
1236 wxString data = datatkz.GetNextToken();
1244 memset(&frame.data, 0, 8);
1245 for (
size_t i = 0; i < data.Length(); i += 2) {
1247 wxString sbyte = data.Mid(data.Length() - i - 2, 2);
1248 sbyte.ToULong(&dv, 16);
1249 frame.data[i / 2] = ((uint8_t)dv);
1251 frame.can_id = (uint32_t)BuildCanID(priority, address, 0xFF, PGN);
1252 HandleCanFrameInput(frame);
1259#define RD_BUF_SIZE 4096
1262 switch (event.GetSocketEvent()) {
1263 case wxSOCKET_INPUT: {
1277 std::vector<unsigned char> data(RD_BUF_SIZE + 1);
1279 uint8_t next_byte = 0;
1281 event.GetSocket()->Read(&data.front(), RD_BUF_SIZE);
1282 if (!event.GetSocket()->Error()) {
1283 size_t count =
event.GetSocket()->LastCount();
1296 for (
int i = 0; i < newdata; i++) {
1297 m_circle->put(data[i]);
1302 m_n2k_format = DetectFormat(data);
1304 switch (m_n2k_format) {
1305 case N2KFormat_Actisense_RAW_ASCII:
1306 ProcessActisense_ASCII_RAW(data);
1308 case N2KFormat_YD_RAW:
1309 ProcessActisense_ASCII_RAW(data);
1311 case N2KFormat_Actisense_N2K_ASCII:
1312 ProcessActisense_ASCII_N2K(data);
1314 case N2KFormat_Actisense_N2K:
1315 ProcessActisense_N2K(data);
1317 case N2KFormat_Actisense_RAW:
1318 ProcessActisense_RAW(data);
1320 case N2KFormat_Actisense_NGT:
1321 ProcessActisense_NGT(data);
1323 case N2KFormat_SeaSmart:
1324 ProcessSeaSmart(data);
1326 case N2KFormat_MiniPlex:
1327 ProcessMiniPlex(data);
1329 case N2KFormat_Undefined:
1336 m_dog_value = N_DOG_TIMEOUT;
1340 case wxSOCKET_LOST: {
1341 if (GetProtocol() == TCP || GetProtocol() == GPSD) {
1342 if (GetBrxConnectEvent())
1343 wxLogMessage(wxString::Format(
1344 _T(
"NetworkDataStream connection lost: %s"), GetPort().c_str()));
1345 if (GetSockServer()) {
1346 GetSock()->Destroy();
1350 wxDateTime now = wxDateTime::Now();
1351 wxTimeSpan since_connect(
1353 if (GetConnectTime().IsValid()) since_connect = now - GetConnectTime();
1355 int retry_time = 5000;
1361 if (!GetBrxConnectEvent() && (since_connect.GetSeconds() < 5))
1364 GetSocketThreadWatchdogTimer()->Stop();
1365 GetSocketTimer()->Start(
1366 retry_time, wxTIMER_ONE_SHOT);
1371 case wxSOCKET_CONNECTION: {
1372 if (GetProtocol() == GPSD) {
1376 char cmd[] =
"?WATCH={\"class\":\"WATCH\", \"nmea\":true}";
1377 GetSock()->Write(cmd, strlen(cmd));
1378 }
else if (GetProtocol() == TCP) {
1379 wxLogMessage(wxString::Format(
1380 _T(
"TCP NetworkDataStream connection established: %s"),
1381 GetPort().c_str()));
1382 m_dog_value = N_DOG_TIMEOUT;
1383 if (GetPortType() != DS_TYPE_OUTPUT) {
1385 if (GetParams().NoDataReconnect)
1386 GetSocketThreadWatchdogTimer()->Start(1000);
1388 if (GetPortType() != DS_TYPE_INPUT && GetSock()->IsOk())
1389 (void)SetOutputSocketOptions(GetSock());
1390 GetSocketTimer()->Stop();
1391 SetBrxConnectEvent(
true);
1394 SetConnectTime(wxDateTime::Now());
1403void CommDriverN2KNet::OnServerSocketEvent(wxSocketEvent& event) {
1404 switch (event.GetSocketEvent()) {
1405 case wxSOCKET_CONNECTION: {
1406 SetSock(GetSockServer()->Accept(
false));
1409 GetSock()->SetTimeout(2);
1411 GetSock()->SetEventHandler(*
this, DS_SOCKET_ID);
1412 int notify_flags = (wxSOCKET_CONNECTION_FLAG | wxSOCKET_LOST_FLAG);
1413 if (GetPortType() != DS_TYPE_INPUT) {
1414 notify_flags |= wxSOCKET_OUTPUT_FLAG;
1415 (void)SetOutputSocketOptions(GetSock());
1417 if (GetPortType() != DS_TYPE_OUTPUT)
1418 notify_flags |= wxSOCKET_INPUT_FLAG;
1419 GetSock()->SetNotify(notify_flags);
1420 GetSock()->Notify(
true);
1431std::vector<unsigned char> MakeSimpleOutMsg(
1432 int data_format,
int pgn, std::vector<unsigned char>& payload) {
1433 std::vector<unsigned char> out_vec;
1435 switch (data_format) {
1436 case N2KFormat_YD_RAW:
1437 case N2KFormat_Actisense_RAW_ASCII: {
1439 unsigned can_id = BuildCanID(6, 0xff, 0xff, pgn);
1441 scan_id.Printf(
"%08X", can_id);
1442 std::string sscan_id = scan_id.ToStdString();
1443 for (
unsigned char s : sscan_id) out_vec.push_back(s);
1444 out_vec.push_back(
' ');
1449 for (
unsigned char d : payload) {
1450 snprintf(tv, 4,
"%02X ", d);
1453 for (
unsigned char s : sspl) out_vec.push_back(s);
1456 out_vec.push_back(0x0d);
1457 out_vec.push_back(0x0a);
1460 case N2KFormat_Actisense_N2K_ASCII: {
1462 wxDateTime now = wxDateTime::Now();
1463 wxString stime = now.Format(
"%H%M%S");
1465 std::string sstime = stime.ToStdString();
1466 out_vec.push_back(
'A');
1467 for (
unsigned char s : sstime) out_vec.push_back(s);
1471 sdp.Printf(
"%02X%02X%1X ",
1473 (
unsigned char)0xFF, 0x6);
1474 std::string ssdp = sdp.ToStdString();
1475 for (
unsigned char s : ssdp) out_vec.push_back(s);
1479 spgn.Printf(
"%05X ", pgn);
1480 std::string sspgn = spgn.ToStdString();
1481 for (
unsigned char s : sspgn) out_vec.push_back(s);
1486 for (
unsigned char d : payload) {
1487 snprintf(tv, 3,
"%02X", d);
1490 for (
unsigned char s : sspl) out_vec.push_back(s);
1493 out_vec.push_back(0x0d);
1494 out_vec.push_back(0x0a);
1497 case N2KFormat_MiniPlex: {
1498 out_vec.push_back(
'$');
1499 out_vec.push_back(
'M');
1500 out_vec.push_back(
'X');
1501 out_vec.push_back(
'P');
1502 out_vec.push_back(
'G');
1503 out_vec.push_back(
'N');
1504 out_vec.push_back(
',');
1507 spgn.Printf(
"%06X,", pgn);
1508 std::string sspgn = spgn.ToStdString();
1509 for (
unsigned char c : sspgn) {
1510 out_vec.push_back(c);
1516 attr |= ((uint16_t)0x06) << 12;
1517 attr |= ((uint16_t)payload.size()) << 8;
1518 attr |= (uint16_t)0xFF;
1522 sattr.Printf(
"%04X,", attr);
1523 std::string ssattr = sattr.ToStdString();
1524 for (
unsigned char c : ssattr) {
1525 out_vec.push_back(c);
1529 for (
auto rit = payload.rbegin(); rit != payload.rend(); ++rit) {
1530 snprintf(tv, 3,
"%02X", *rit);
1531 out_vec.push_back(tv[0]);
1532 out_vec.push_back(tv[1]);
1536 for (
auto ci = ++out_vec.begin(); ci != out_vec.end(); ci++) {
1539 out_vec.push_back(
'*');
1540 snprintf(tv, 3,
"%02X", crc);
1541 out_vec.push_back(tv[0]);
1542 out_vec.push_back(tv[1]);
1545 out_vec.push_back(0x0d);
1546 out_vec.push_back(0x0a);
1557std::vector<std::vector<unsigned char>> CommDriverN2KNet::GetTxVector(
1558 const std::shared_ptr<const Nmea2000Msg>& msg,
1559 std::shared_ptr<const NavAddr2000> dest_addr) {
1560 std::vector<std::vector<unsigned char>> tx_vector;
1563 switch (m_n2k_format) {
1564 case N2KFormat_YD_RAW:
1566 case N2KFormat_Actisense_RAW_ASCII: {
1568 if (!IsFastMessagePGN(msg->PGN.pgn) && msg->payload.size() <= 8) {
1570 std::vector<unsigned char> header_vec;
1571 std::vector<unsigned char> out_vec;
1576 unsigned long can_id =
1577 BuildCanID(msg->priority, 0, dest_addr->address, msg->PGN.pgn);
1579 scan_id.Printf(
"%08X", can_id);
1580 std::string sscan_id = scan_id.ToStdString();
1581 for (
unsigned char s : sscan_id) header_vec.push_back(s);
1582 header_vec.push_back(
' ');
1585 for (
unsigned char s : header_vec) out_vec.push_back(s);
1589 for (
unsigned int k = 0; k < msg->payload.size(); k++) {
1591 snprintf(tb, 4,
"%02X ", msg->payload.data()[k]);
1594 for (
unsigned char s : ssdata) out_vec.push_back(s);
1597 out_vec.push_back(0x0d);
1598 out_vec.push_back(0x0a);
1600 tx_vector.push_back(out_vec);
1602 std::vector<unsigned char> header_vec;
1603 std::vector<unsigned char> out_vec;
1608 wxDateTime now = wxDateTime::Now();
1609 wxString stime = now.Format(
"%H:%M:%S");
1611 std::string sstime = stime.ToStdString();
1612 for (
unsigned char s : sstime) header_vec.push_back(s);
1615 header_vec.push_back(
'T');
1616 header_vec.push_back(
' ');
1622 unsigned long can_id =
1623 BuildCanID(msg->priority, 0, dest_addr->address, msg->PGN.pgn);
1625 scan_id.Printf(
"%08X", can_id);
1626 std::string sscan_id = scan_id.ToStdString();
1627 for (
unsigned char s : sscan_id) header_vec.push_back(s);
1628 header_vec.push_back(
' ');
1631 int payload_size = msg->payload.size();
1632 unsigned char temp[8];
1635 (payload_size > 6 ? (payload_size - 6 - 1) / 7 + 1 + 1 : 1);
1637 for (
int i = 0; i < nframes && result; i++) {
1638 temp[0] = i | m_order;
1640 temp[1] = msg->payload.size();
1642 for (
int j = 2; j < 8; j++) {
1643 temp[j] = msg->payload.data()[cur];
1649 for (; j < 8 && cur < payload_size; j++) {
1650 temp[j] = msg->payload.data()[cur];
1653 for (; j < 8; j++) {
1661 for (
unsigned char s : header_vec) out_vec.push_back(s);
1665 for (
unsigned int k = 0; k < 8; k++) {
1667 snprintf(tb, 4,
"%02X ", temp[k]);
1670 for (
unsigned char s : ssdata) out_vec.push_back(s);
1673 out_vec.push_back(0x0d);
1674 out_vec.push_back(0x0a);
1676 tx_vector.push_back(out_vec);
1680 case N2KFormat_Actisense_N2K_ASCII: {
1697 std::vector<unsigned char> ovec;
1700 wxDateTime now = wxDateTime::Now();
1701 wxString stime = now.Format(
"%H%M%S");
1703 std::string sstime = stime.ToStdString();
1704 ovec.push_back(
'A');
1705 for (
unsigned char s : sstime) ovec.push_back(s);
1709 sdp.Printf(
"%02X%02X%1X ",
1711 (
unsigned char)dest_addr->address,
1712 (
unsigned char)msg->priority);
1713 std::string ssdp = sdp.ToStdString();
1714 for (
unsigned char s : ssdp) ovec.push_back(s);
1718 spgn.Printf(
"%05X ", (
int)msg->PGN.pgn);
1719 std::string sspgn = spgn.ToStdString();
1720 for (
unsigned char s : sspgn) ovec.push_back(s);
1725 for (
unsigned char d : msg->payload) {
1726 snprintf(tv, 3,
"%02X", d);
1729 for (
unsigned char s : sspl) ovec.push_back(s);
1732 ovec.push_back(0x0d);
1733 ovec.push_back(0x0a);
1736 tx_vector.push_back(ovec);
1740 case N2KFormat_MiniPlex: {
1741 std::vector<unsigned char> ovec;
1742 if (!IsFastMessagePGN(msg->PGN.pgn) && msg->payload.size() < 8) {
1747 (msg->payload.size() > 6 ? (msg->payload.size() - 6 - 1) / 7 + 1 + 1
1749 for (
size_t i = 0; i < nframes; i++) {
1750 ovec.push_back(
'$');
1751 ovec.push_back(
'M');
1752 ovec.push_back(
'X');
1753 ovec.push_back(
'P');
1754 ovec.push_back(
'G');
1755 ovec.push_back(
'N');
1756 ovec.push_back(
',');
1759 spgn.Printf(
"%06X,", (
int)msg->PGN.pgn);
1760 std::string sspgn = spgn.ToStdString();
1761 for (
unsigned char c : sspgn) {
1767 if (i == nframes - 1) {
1768 len = msg->payload.size() + 1 - 6 - (nframes - 2) * 7;
1770 attr |= ((uint16_t)((uint8_t)msg->priority & 0x07)) << 12;
1771 attr |= ((uint16_t)len) << 8;
1772 attr |= (uint16_t)dest_addr->address;
1776 sattr.Printf(
"%04X,", attr);
1777 std::string ssattr = sattr.ToStdString();
1778 for (
unsigned char c : ssattr) {
1783 uint8_t databytes = i == 0 ? len - 2 : len - 1;
1784 std::vector<unsigned char> payload;
1785 for (uint8_t j = 0; j < databytes; j++) {
1786 payload.push_back(msg->payload[cur]);
1789 for (
auto rit = payload.rbegin(); rit != payload.rend(); ++rit) {
1790 snprintf(tv, 3,
"%02X", *rit);
1791 ovec.push_back(tv[0]);
1792 ovec.push_back(tv[1]);
1795 snprintf(tv, 3,
"%02X", (uint8_t)msg->payload.size());
1796 ovec.push_back(tv[0]);
1797 ovec.push_back(tv[1]);
1800 snprintf(tv, 3,
"%02X", (uint8_t)i | m_order);
1801 ovec.push_back(tv[0]);
1802 ovec.push_back(tv[1]);
1806 for (
auto ci = ++ovec.begin(); ci != ovec.end(); ci++) {
1809 ovec.push_back(
'*');
1810 snprintf(tv, 3,
"%02X", crc);
1811 ovec.push_back(tv[0]);
1812 ovec.push_back(tv[1]);
1815 ovec.push_back(0x0d);
1816 ovec.push_back(0x0a);
1822 tx_vector.push_back(ovec);
1829 case N2KFormat_Actisense_N2K:
1831 case N2KFormat_Actisense_RAW:
1833 case N2KFormat_Actisense_NGT:
1835 case N2KFormat_SeaSmart:
1846bool CommDriverN2KNet::PrepareForTX() {
1856 bool b_found =
false;
1863 if (m_n2k_format == N2KFormat_Actisense_N2K_ASCII)
return true;
1870 if (m_n2k_format == N2KFormat_MiniPlex)
return true;
1875 if (m_n2k_format == N2KFormat_SeaSmart)
return false;
1888 std::vector<unsigned char> payload;
1889 payload.push_back(0x14);
1890 payload.push_back(0xF0);
1891 payload.push_back(0x01);
1893 std::vector<std::vector<unsigned char>> out_data;
1894 std::vector<unsigned char> msg_vec =
1895 MakeSimpleOutMsg(N2KFormat_YD_RAW, 59904, payload);
1896 out_data.push_back(msg_vec);
1897 SendSentenceNetwork(out_data);
1900 m_prodinfo_timer.Start(200,
true);
1907bool CommDriverN2KNet::SendN2KNetwork(std::shared_ptr<const Nmea2000Msg>& msg,
1908 std::shared_ptr<const NavAddr2000> addr) {
1911 std::vector<std::vector<unsigned char>> out_data = GetTxVector(msg, addr);
1912 SendSentenceNetwork(out_data);
1915 std::vector<unsigned char> msg_payload = PrepareLogPayload(msg, addr);
1916 auto msg_all = std::make_shared<const Nmea2000Msg>(1, msg_payload, addr);
1919 m_listener.
Notify(std::move(msg));
1920 m_listener.
Notify(std::move(msg_all));
1925bool CommDriverN2KNet::SendSentenceNetwork(
1926 std::vector<std::vector<unsigned char>> payload) {
1933 wxDatagramSocket* udp_socket;
1934 switch (GetProtocol()) {
1936 for (std::vector<unsigned char>& v : payload) {
1937 if (GetSock() && GetSock()->IsOk()) {
1939 GetSock()->Write(v.data(), v.size());
1940 if (GetSock()->Error()) {
1941 if (GetSockServer()) {
1942 GetSock()->Destroy();
1945 wxSocketClient* tcp_socket =
1946 dynamic_cast<wxSocketClient*
>(GetSock());
1947 if (tcp_socket) tcp_socket->Close();
1948 if (!GetSocketTimer()->IsRunning())
1949 GetSocketTimer()->Start(
1950 5000, wxTIMER_ONE_SHOT);
1951 GetSocketThreadWatchdogTimer()->Stop();
1962 udp_socket =
dynamic_cast<wxDatagramSocket*
>(GetTSock());
1963 if (udp_socket && udp_socket->IsOk()) {
1964 udp_socket->SendTo(GetAddr(), payload.mb_str(), payload.size());
1965 if (udp_socket->Error()) ret =
false;
1980void CommDriverN2KNet::Close() {
1981 wxLogMessage(wxString::Format(_T(
"Closing NMEA NetworkDataStream %s"),
1982 GetNetPort().c_str()));
1986 m_sock->SetOption(IPPROTO_IP, IP_DROP_MEMBERSHIP, &m_mrq_container->m_mrq,
1987 sizeof(m_mrq_container->m_mrq));
1988 m_sock->Notify(FALSE);
1993 m_tsock->Notify(FALSE);
1997 if (m_socket_server) {
1998 m_socket_server->Notify(FALSE);
1999 m_socket_server->Destroy();
2002 m_socket_timer.Stop();
2003 m_socketread_watchdog_timer.Stop();
2006bool CommDriverN2KNet::SetOutputSocketOptions(wxSocketBase* tsock) {
2015 int nagleDisable = 1;
2016 ret = tsock->SetOption(IPPROTO_TCP, TCP_NODELAY, &nagleDisable,
2017 sizeof(nagleDisable));
2023 unsigned long outbuf_size = 1024;
2024 return (tsock->SetOption(SOL_SOCKET, SO_SNDBUF, &outbuf_size,
2025 sizeof(outbuf_size)) &&
void OnSocketEvent(wxSocketEvent &event)
Interface implemented by transport layer and possible other parties like test code which should handl...
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.
Driver registration container, a singleton.
Raw messages layer, supports sending and recieving navmsg messages.
Suspend/resume and new devices events exchange point.