39#include "model/comm_drv_n2k_serial.h"
54static unsigned char NGT_STARTUP_SEQ[] = {
60std::vector<unsigned char> BufferToActisenseFormat(tN2kMsg& msg);
66 std::lock_guard<std::mutex> lock(m_mutex);
67 return m_queque.size();
71 std::lock_guard<std::mutex> lock(m_mutex);
72 return m_queque.empty();
76 std::lock_guard<std::mutex> lock(m_mutex);
77 return m_queque.front();
80 void push(
const T& value) {
81 std::lock_guard<std::mutex> lock(m_mutex);
86 std::lock_guard<std::mutex> lock(m_mutex);
91 std::queue<T> m_queque;
92 mutable std::mutex m_mutex;
99 : buf_(std::unique_ptr<T[]>(new T[size])), max_size_(size) {}
102 size_t capacity()
const;
107 return (!full_ && (head_ == tail_));
116 std::lock_guard<std::mutex> lock(mutex_);
118 if (full_) tail_ = (tail_ + 1) % max_size_;
120 head_ = (head_ + 1) % max_size_;
122 full_ = head_ == tail_;
126 std::lock_guard<std::mutex> lock(mutex_);
128 if (empty())
return T();
131 auto val = buf_[tail_];
133 tail_ = (tail_ + 1) % max_size_;
140 std::unique_ptr<T[]> buf_;
143 const size_t max_size_;
152 const wxString& PortName,
153 const wxString& strBaudRate);
157 bool SetOutMsg(
const std::vector<unsigned char>& load);
165 void ThreadMessage(
const wxString& msg);
166 bool OpenComPortPhysical(
const wxString& com_name,
int baud_rate);
167 void CloseComPortPhysical();
168 size_t WriteComPortPhysical(std::vector<unsigned char> msg);
169 size_t WriteComPortPhysical(
unsigned char* msg,
size_t length);
170 void SetGatewayOperationMode(
void);
174 wxString m_FullPortName;
176 unsigned char* put_ptr;
177 unsigned char* tak_ptr;
179 unsigned char* rx_buffer;
186 mutable std::mutex m_stats_mutex;
188 HANDLE m_hSerialComm;
199 : wxEvent(
id, commandType) {};
203 void SetPayload(std::shared_ptr<std::vector<unsigned char>> data) {
206 std::shared_ptr<std::vector<unsigned char>> GetPayload() {
return m_payload; }
209 wxEvent* Clone()
const {
211 newevent->m_payload = this->m_payload;
216 std::shared_ptr<std::vector<unsigned char>> m_payload;
228 m_Thread_run_flag(-1),
231 m_portstring(params->GetDSPort()),
232 m_pSecondary_Thread(NULL),
233 m_listener(listener),
234 m_stats_timer(*this, 2s) {
235 m_BaudRate = wxString::Format(
"%i", params->Baudrate), SetSecThreadInActive();
236 m_manufacturers_code = 0;
237 m_got_mfg_code =
false;
238 this->attributes[
"canAddress"] = std::string(
"-1");
239 this->attributes[
"userComment"] = params->UserComment.ToStdString();
240 this->attributes[
"ioDirection"] = std::string(
"IN/OUT");
243 Bind(wxEVT_COMMDRIVER_N2K_SERIAL, &CommDriverN2KSerial::handle_N2K_SERIAL_RAW,
247 m_driver_stats.driver_bus = NavAddr::Bus::N2000;
248 m_driver_stats.driver_iface = m_params.GetStrippedDSPort();
249 m_driver_stats.available =
false;
257 SendMgmtMsg(NGT_STARTUP_SEQ,
sizeof(NGT_STARTUP_SEQ), 0x11, 0, NULL);
266 N2kMsg.SetPGN(126993L);
269 N2kMsg.Destination = 133;
270 N2kMsg.Add2ByteUInt((uint16_t)(2000));
273 N2kMsg.AddByte(0xff);
274 N2kMsg.Add4ByteUInt(0xffffffff);
276 const std::vector<unsigned char> mv = BufferToActisenseFormat(N2kMsg);
278 size_t len = mv.size();
280 wxString comx = m_params.GetDSPort().AfterFirst(
':');
281 std::string
interface = comx.ToStdString();
284 auto source_address = std::make_shared<NavAddr2000>(interface, source_name);
285 auto dest_address = std::make_shared<NavAddr2000>(interface, N2kMsg.Destination);
287 auto message_to_send = std::make_shared<Nmea2000Msg>(126993L,
288 mv, source_address, 3);
290 for(
size_t i=0; i< mv.size(); i++){
291 printf(
"%02X ", mv.at(i));
298 SendMessage(message_to_send, dest_address);
304CommDriverN2KSerial::~CommDriverN2KSerial() { Close(); }
306DriverStats CommDriverN2KSerial::GetDriverStats()
const {
307 if (m_pSecondary_Thread)
308 return m_pSecondary_Thread->GetStats();
310 return m_driver_stats;
313bool CommDriverN2KSerial::Open() {
315 comx = m_params.GetDSPort().AfterFirst(
':');
318 comx.BeforeFirst(
' ');
324 GetSecondaryThread()->Run();
330void CommDriverN2KSerial::Close() {
332 wxString::Format(_T(
"Closing N2K Driver %s"), m_portstring.c_str()));
335 if (m_pSecondary_Thread) {
336 if (m_bsec_thread_active)
338 wxLogMessage(_T(
"Stopping Secondary Thread"));
340 m_Thread_run_flag = 0;
342 while ((m_Thread_run_flag >= 0) && (tsec--)) wxSleep(1);
345 if (m_Thread_run_flag < 0)
346 msg.Printf(_T(
"Stopped in %d sec."), 10 - tsec);
348 msg.Printf(_T(
"Not Stopped after 10 sec."));
352 m_pSecondary_Thread = NULL;
353 m_bsec_thread_active =
false;
356static uint64_t PayloadToName(
const std::vector<unsigned char> payload) {
358 memcpy(&name,
reinterpret_cast<const void*
>(payload.data()),
sizeof(name));
362bool CommDriverN2KSerial::SendMessage(std::shared_ptr<const NavMsg> msg,
363 std::shared_ptr<const NavAddr> addr) {
366 auto msg_n2k = std::dynamic_pointer_cast<const Nmea2000Msg>(msg);
367 std::vector<uint8_t> load = msg_n2k->payload;
369 uint64_t _pgn = msg_n2k->PGN.pgn;
370 auto destination_address = std::static_pointer_cast<const NavAddr2000>(addr);
374 N2kMsg.Priority = msg_n2k->priority;
375 if (destination_address) N2kMsg.Destination = destination_address->address;
377 for (
size_t i = 0; i < load.size(); i++) N2kMsg.AddByte(load.at(i));
379 const std::vector<uint8_t> acti_pkg = BufferToActisenseFormat(N2kMsg);
382 std::vector<unsigned char> msg_payload;
383 for (
size_t i = 2; i < acti_pkg.size() - 2; i++)
384 msg_payload.push_back(acti_pkg[i]);
385 auto name = PayloadToName(load);
387 std::make_shared<const Nmea2000Msg>(1, msg_payload, GetAddress(name));
390 m_listener.
Notify(std::move(msg));
391 m_listener.
Notify(std::move(msg_all));
393 if (GetSecondaryThread()) {
394 if (IsSecThreadActive()) {
397 if (GetSecondaryThread()->SetOutMsg(acti_pkg))
410void CommDriverN2KSerial::ProcessManagementPacket(
411 std::vector<unsigned char>* payload) {
412 if (payload->at(2) != 0xF2) {
419 switch (payload->at(2)) {
431 if (payload->at(3) == 0x02) {
432 std::string device_common_name;
433 for (
unsigned int i = 0; i < 32; i++) {
434 device_common_name += payload->at(i + 14);
436 device_common_name +=
'\0';
437 m_device_common_name = device_common_name;
443 unsigned char name[8];
444 for (
unsigned int i = 0; i < 8; i++) name[i] = payload->at(i + 15);
446 memcpy((
void*)&NAME, name, 8);
448 int* f1 = (
int*)&NAME;
450 m_manufacturers_code = f1d >> 21;
459void CommDriverN2KSerial::handle_N2K_SERIAL_RAW(
461 auto p =
event.GetPayload();
463 std::vector<unsigned char>* payload = p.get();
465 if (payload->at(0) == 0xA0) {
466 ProcessManagementPacket(payload);
472 unsigned char* c = (
unsigned char*)&pgn;
473 *c++ = payload->at(3);
474 *c++ = payload->at(4);
475 *c++ = payload->at(5);
479 auto name = PayloadToName(*payload);
481 std::make_shared<const Nmea2000Msg>(pgn, *payload, GetAddress(name));
483 std::make_shared<const Nmea2000Msg>(1, *payload, GetAddress(name));
485 m_listener.
Notify(std::move(msg));
486 m_listener.
Notify(std::move(msg_all));
489 size_t packetLength = (size_t)payload->at(1);
490 size_t vector_length = payload->size();
495 printf(
"Payload Length: %ld\n", vector_length);
497 printf(
"PGN: %ld\n", pgn);
499 for(
size_t i=0; i< vector_length ; i++){
500 printf(
"%02X ", payload->at(i));
507int CommDriverN2KSerial::GetMfgCode() {
508 unsigned char request_name[] = {0x42};
509 int ni = SendMgmtMsg(request_name,
sizeof(request_name), 0x41, 2000,
512 m_got_mfg_code =
true;
516int CommDriverN2KSerial::SendMgmtMsg(
unsigned char*
string,
size_t string_size,
517 unsigned char cmd_code,
int timeout_msec,
518 bool* response_flag) {
523 std::vector<unsigned char> msg;
525 msg.push_back(ESCAPE);
526 msg.push_back(STARTOFTEXT);
529 msg.push_back(string_size);
530 byteSum += string_size;
532 for (
unsigned int i = 0; i < string_size; i++) {
533 if (
string[i] == ESCAPE) msg.push_back(
string[i]);
534 msg.push_back(
string[i]);
535 byteSum +=
string[i];
540 CheckSum = (uint8_t)((byteSum == 0) ? 0 : (256 - byteSum));
541 msg.push_back(CheckSum);
543 msg.push_back(ESCAPE);
544 msg.push_back(ENDOFTEXT);
548 if (response_flag) *response_flag =
false;
552 bool not_done =
true;
555 if (GetSecondaryThread() && IsSecThreadActive()) {
558 if (GetSecondaryThread()->SetOutMsg(msg)) {
567 if (ntry_outer-- <= 0) not_done =
false;
571 if (!bsent)
return 1;
575 int timeout = timeout_msec;
576 while (timeout > 0) {
580 if (*response_flag) {
600int CommDriverN2KSerial::SetTXPGN(
int pgn) {
602 unsigned char request_enable[] = {0x47, 0x00, 0x00, 0x00,
603 0x00, 0x01, 0xFF, 0xFF, 0xFF, 0xFF};
606 unsigned char* c = (
unsigned char*)&pgn;
607 request_enable[1] = c[0];
608 request_enable[2] = c[1];
609 request_enable[3] = c[2];
611 int aa = SendMgmtMsg(request_enable,
sizeof(request_enable), 0x47, 2000,
616 unsigned char request_commit[] = {0x01};
617 int bb = SendMgmtMsg(request_commit,
sizeof(request_commit), 0x01, 2000,
621 unsigned char request_activate[] = {0x4B};
622 int cc = SendMgmtMsg(request_activate,
sizeof(request_activate), 0x4B, 2000,
627void CommDriverN2KSerial::AddTxPGN(
int pgn) {
628 auto it = std::find(pgn_tx_list.begin(), pgn_tx_list.end(), pgn);
629 if (it != pgn_tx_list.end())
633 pgn_tx_list.push_back(pgn);
665#define DS_RX_BUFFER_SIZE 4096
667CommDriverN2KSerialThread::CommDriverN2KSerialThread(
669 const wxString& strBaudRate) {
670 m_pParentDriver = Launcher;
672 m_PortName = PortName;
673 m_FullPortName = _T(
"Serial:") + PortName;
675 rx_buffer =
new unsigned char[DS_RX_BUFFER_SIZE + 1];
682 if (strBaudRate.ToLong(&lbaud)) m_baud = (int)lbaud;
684 std::lock_guard lock(m_stats_mutex);
685 m_driver_stats.driver_bus = NavAddr::Bus::N2000;
686 m_driver_stats.driver_iface = m_pParentDriver->m_params.GetStrippedDSPort();
687 m_driver_stats.available =
false;
693CommDriverN2KSerialThread::~CommDriverN2KSerialThread(
void) {
697void CommDriverN2KSerialThread::OnExit(
void) {}
699DriverStats CommDriverN2KSerialThread::GetStats()
const {
700 std::lock_guard lock(m_stats_mutex);
701 return m_driver_stats;
704bool CommDriverN2KSerialThread::OpenComPortPhysical(
const wxString& com_name,
707 m_serial.
setPort(com_name.ToStdString());
711 }
catch (std::exception&) {
718void CommDriverN2KSerialThread::CloseComPortPhysical() {
721 }
catch (std::exception&) {
725 std::lock_guard lock(m_stats_mutex);
726 m_driver_stats.available =
false;
729void CommDriverN2KSerialThread::SetGatewayOperationMode(
void) {
733 unsigned char config_string[] = {0x10, 0x02, 0xA1, 0x03, 0x11,
734 0x02, 0x00, 0x49, 0x10, 0x03};
738 WriteComPortPhysical(config_string, 10);
741void CommDriverN2KSerialThread::ThreadMessage(
const wxString& msg) {
748size_t CommDriverN2KSerialThread::WriteComPortPhysical(
749 std::vector<unsigned char> msg) {
750 return WriteComPortPhysical(msg.data(), msg.size());
753size_t CommDriverN2KSerialThread::WriteComPortPhysical(
unsigned char* msg,
755 if (!m_serial.
isOpen())
return 0;
757 size_t status = m_serial.
write((uint8_t*)msg, length);
760 }
catch (std::exception& e) {
761 DEBUG_LOG <<
"Unhandled Exception while writing to serial port: "
767bool CommDriverN2KSerialThread::SetOutMsg(
768 const std::vector<unsigned char>& msg) {
769 if (out_que.size() < OUT_QUEUE_LENGTH) {
777void* CommDriverN2KSerialThread::Entry() {
778 bool not_done =
true;
779 bool nl_found =
false;
786 if (!OpenComPortPhysical(m_PortName, m_baud)) {
787 wxString msg(_T(
"NMEA input device open failed: "));
788 msg.Append(m_PortName);
790 std::lock_guard lock(m_stats_mutex);
791 m_driver_stats.available =
false;
800 std::lock_guard lock(m_stats_mutex);
801 m_driver_stats.available =
true;
802 SetGatewayOperationMode();
805 m_pParentDriver->SetSecThreadActive();
808 static size_t retries = 0;
811 bool bGotESC =
false;
812 bool bGotSOT =
false;
814 while ((not_done) && (m_pParentDriver->m_Thread_run_flag > 0)) {
815 if (TestDestroy()) not_done =
false;
817 uint8_t next_byte = 0;
821 newdata = m_serial.
read(rdata, 1000);
822 }
catch (std::exception& e) {
824 std::lock_guard lock(m_stats_mutex);
827 if (10 < retries++) {
831 CloseComPortPhysical();
839 wxMilliSleep(250 * retries);
840 CloseComPortPhysical();
841 if (OpenComPortPhysical(m_PortName, m_baud)) {
842 SetGatewayOperationMode();
843 std::lock_guard lock(m_stats_mutex);
844 m_driver_stats.available =
true;
846 }
else if (retries < 10) {
847 std::lock_guard lock(m_stats_mutex);
848 m_driver_stats.available =
false;
854 std::lock_guard lock(m_stats_mutex);
857 for (
int i = 0; i < newdata; i++) {
858 circle.put(rdata[i]);
862 while (!circle.empty()) {
863 if (ib >= DS_RX_BUFFER_SIZE) ib = 0;
864 uint8_t next_byte = circle.get();
868 if (ESCAPE == next_byte) {
869 rx_buffer[ib++] = next_byte;
874 if (bGotESC && (ENDOFTEXT == next_byte)) {
878 auto buffer = std::make_shared<std::vector<unsigned char>>(
879 rx_buffer, rx_buffer + ib);
880 std::vector<unsigned char>* vec = buffer.get();
896 Nevent.SetPayload(buffer);
897 m_pParentDriver->AddPendingEvent(Nevent);
900 bGotESC = (next_byte == ESCAPE);
903 rx_buffer[ib++] = next_byte;
909 if (STARTOFTEXT == next_byte) {
915 bGotESC = (next_byte == ESCAPE);
920 rx_buffer[ib++] = next_byte;
928 bool b_qdata = !out_que.empty();
932 std::vector<unsigned char> qmsg = out_que.front();
935 if (
static_cast<size_t>(-1) == WriteComPortPhysical(qmsg) &&
940 CloseComPortPhysical();
943 b_qdata = !out_que.empty();
950 CloseComPortPhysical();
951 m_pParentDriver->SetSecThreadInActive();
952 m_pParentDriver->m_Thread_run_flag = -1;
954 std::lock_guard lock(m_stats_mutex);
955 m_driver_stats.available =
false;
961void* CommDriverN2KSerialThread::Entry() {
962 bool not_done =
true;
963 bool nl_found =
false;
968 if (!OpenComPortPhysical(m_PortName, m_baud)) {
969 wxString msg(_T(
"NMEA input device open failed: "));
970 msg.Append(m_PortName);
978 SetGatewayOperationMode();
981 m_pParentDriver->SetSecThreadActive();
984 static size_t retries = 0;
987 bool bGotESC =
false;
988 bool bGotSOT =
false;
990 while ((not_done) && (m_pParentDriver->m_Thread_run_flag > 0)) {
991 if (TestDestroy()) not_done =
false;
993 uint8_t next_byte = 0;
999 newdata = m_serial.
read(rdata, 200);
1000 }
catch (std::exception& e) {
1002 if (10 < retries++) {
1006 CloseComPortPhysical();
1014 wxMilliSleep(250 * retries);
1015 CloseComPortPhysical();
1016 if (OpenComPortPhysical(m_PortName, m_baud)) {
1017 SetGatewayOperationMode();
1019 }
else if (retries < 10)
1024 for (
int i = 0; i < newdata; i++) {
1025 circle.put(rdata[i]);
1029 while (!circle.empty()) {
1030 uint8_t next_byte = circle.get();
1035 if (ESCAPE == next_byte) {
1036 *put_ptr++ = next_byte;
1037 if ((put_ptr - rx_buffer) > DS_RX_BUFFER_SIZE)
1038 put_ptr = rx_buffer;
1040 }
else if (ENDOFTEXT == next_byte) {
1044 auto buffer = std::make_shared<std::vector<unsigned char>>();
1045 std::vector<unsigned char>* vec = buffer.get();
1047 unsigned char* tptr;
1050 while ((tptr != put_ptr)) {
1051 vec->push_back(*tptr++);
1052 if ((tptr - rx_buffer) > DS_RX_BUFFER_SIZE) tptr = rx_buffer;
1064 Nevent.SetPayload(buffer);
1065 m_pParentDriver->AddPendingEvent(Nevent);
1066 }
else if (next_byte == STARTOFTEXT) {
1067 put_ptr = rx_buffer;
1070 put_ptr = rx_buffer;
1076 bGotESC = (next_byte == ESCAPE);
1079 *put_ptr++ = next_byte;
1080 if ((put_ptr - rx_buffer) > DS_RX_BUFFER_SIZE)
1081 put_ptr = rx_buffer;
1087 if (STARTOFTEXT == next_byte) {
1093 bGotESC = (next_byte == ESCAPE);
1098 *put_ptr++ = next_byte;
1099 if ((put_ptr - rx_buffer) > DS_RX_BUFFER_SIZE)
1100 put_ptr = rx_buffer;
1108 bool b_qdata = !out_que.empty();
1112 std::vector<unsigned char> qmsg = out_que.front();
1115 if (
static_cast<size_t>(-1) == WriteComPortPhysical(qmsg) &&
1120 CloseComPortPhysical();
1123 b_qdata = !out_que.empty();
1128 CloseComPortPhysical();
1129 m_pParentDriver->SetSecThreadInActive();
1130 m_pParentDriver->m_Thread_run_flag = -1;
1143#define MaxActisenseMsgBuf 400
1144#define MsgTypeN2kTX 0x94
1146void AddByteEscapedToBuf(
unsigned char byteToAdd, uint8_t& idx,
1147 unsigned char* buf,
int& byteSum);
1149std::vector<unsigned char> BufferToActisenseFormat(tN2kMsg& msg) {
1150 unsigned long _PGN = msg.PGN;
1154 unsigned char ActisenseMsgBuf[MaxActisenseMsgBuf];
1156 ActisenseMsgBuf[msgIdx++] = ESCAPE;
1157 ActisenseMsgBuf[msgIdx++] = STARTOFTEXT;
1158 AddByteEscapedToBuf(MsgTypeN2kTX, msgIdx, ActisenseMsgBuf, byteSum);
1159 AddByteEscapedToBuf(msg.DataLen + 6, msgIdx, ActisenseMsgBuf,
1162 AddByteEscapedToBuf(msg.Priority, msgIdx, ActisenseMsgBuf, byteSum);
1163 AddByteEscapedToBuf(_PGN & 0xff, msgIdx, ActisenseMsgBuf, byteSum);
1165 AddByteEscapedToBuf(_PGN & 0xff, msgIdx, ActisenseMsgBuf, byteSum);
1167 AddByteEscapedToBuf(_PGN & 0xff, msgIdx, ActisenseMsgBuf, byteSum);
1168 AddByteEscapedToBuf(msg.Destination, msgIdx, ActisenseMsgBuf, byteSum);
1173 AddByteEscapedToBuf(msg.Source,msgIdx,ActisenseMsgBuf,byteSum);
1176 AddByteEscapedToBuf(_MsgTime & 0xff,msgIdx,ActisenseMsgBuf,byteSum); _MsgTime>>=8;
1177 AddByteEscapedToBuf(_MsgTime & 0xff,msgIdx,ActisenseMsgBuf,byteSum); _MsgTime>>=8;
1178 AddByteEscapedToBuf(_MsgTime & 0xff,msgIdx,ActisenseMsgBuf,byteSum); _MsgTime>>=8;
1179 AddByteEscapedToBuf(_MsgTime & 0xff,msgIdx,ActisenseMsgBuf,byteSum);
1183 AddByteEscapedToBuf(msg.DataLen, msgIdx, ActisenseMsgBuf, byteSum);
1185 for (
int i = 0; i < msg.DataLen; i++)
1186 AddByteEscapedToBuf(msg.Data[i], msgIdx, ActisenseMsgBuf, byteSum);
1189 CheckSum = (uint8_t)((byteSum == 0) ? 0 : (256 - byteSum));
1190 ActisenseMsgBuf[msgIdx++] = CheckSum;
1191 if (CheckSum == ESCAPE) ActisenseMsgBuf[msgIdx++] = CheckSum;
1193 ActisenseMsgBuf[msgIdx++] = ESCAPE;
1194 ActisenseMsgBuf[msgIdx++] = ENDOFTEXT;
1196 std::vector<unsigned char> rv;
1197 for (
unsigned int i = 0; i < msgIdx; i++) rv.push_back(ActisenseMsgBuf[i]);
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.
Class that provides a portable serial port interface.
size_t read(uint8_t *buffer, size_t size)
Read a given amount of bytes from the serial port into a given buffer.
void setPort(const std::string &port)
Sets the serial port identifier.
size_t write(const uint8_t *data, size_t size)
Write a string to the serial port.
void setBaudrate(uint32_t baudrate)
Sets the baudrate for the serial port.
void close()
Closes the serial port.
void setTimeout(Timeout &timeout)
Sets the timeout for reads and writes using the Timeout struct.
void flushOutput()
Flush only the output buffer.
void open()
Opens the serial port as long as the port is set and the port isn't already open.
bool isOpen() const
Gets the open status of the serial port.
Driver registration container, a singleton.
Communication statistics infrastructure.
Raw messages layer, supports sending and recieving navmsg messages.
Enhanced logging interface on top of wx/log.h.
Driver statistics report.
unsigned rx_count
Number of bytes received since program start.
unsigned error_count
Number of detected errors since program start.
N2k uses CAN which defines the basic properties of messages.