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;
357bool CommDriverN2KSerial::SendMessage(std::shared_ptr<const NavMsg> msg,
358 std::shared_ptr<const NavAddr> addr) {
361 auto msg_n2k = std::dynamic_pointer_cast<const Nmea2000Msg>(msg);
362 std::vector<uint8_t> load = msg_n2k->payload;
364 uint64_t _pgn = msg_n2k->PGN.pgn;
365 auto destination_address = std::static_pointer_cast<const NavAddr2000>(addr);
369 N2kMsg.Priority = msg_n2k->priority;
370 if (destination_address) N2kMsg.Destination = destination_address->address;
372 for (
size_t i = 0; i < load.size(); i++) N2kMsg.AddByte(load.at(i));
374 const std::vector<uint8_t> mv = BufferToActisenseFormat(N2kMsg);
382 if (GetSecondaryThread()) {
383 if (IsSecThreadActive()) {
386 if (GetSecondaryThread()->SetOutMsg(mv))
399void CommDriverN2KSerial::ProcessManagementPacket(
400 std::vector<unsigned char>* payload) {
401 if (payload->at(2) != 0xF2) {
408 switch (payload->at(2)) {
420 if (payload->at(3) == 0x02) {
421 std::string device_common_name;
422 for (
unsigned int i = 0; i < 32; i++) {
423 device_common_name += payload->at(i + 14);
425 device_common_name +=
'\0';
426 m_device_common_name = device_common_name;
432 unsigned char name[8];
433 for (
unsigned int i = 0; i < 8; i++) name[i] = payload->at(i + 15);
435 memcpy((
void*)&NAME, name, 8);
437 int* f1 = (
int*)&NAME;
439 m_manufacturers_code = f1d >> 21;
448static uint64_t PayloadToName(
const std::vector<unsigned char> payload) {
450 memcpy(&name,
reinterpret_cast<const void*
>(payload.data()),
sizeof(name));
454void CommDriverN2KSerial::handle_N2K_SERIAL_RAW(
456 auto p =
event.GetPayload();
458 std::vector<unsigned char>* payload = p.get();
460 if (payload->at(0) == 0xA0) {
461 ProcessManagementPacket(payload);
467 unsigned char* c = (
unsigned char*)&pgn;
468 *c++ = payload->at(3);
469 *c++ = payload->at(4);
470 *c++ = payload->at(5);
474 auto name = PayloadToName(*payload);
476 std::make_shared<const Nmea2000Msg>(pgn, *payload, GetAddress(name));
478 std::make_shared<const Nmea2000Msg>(1, *payload, GetAddress(name));
480 m_listener.
Notify(std::move(msg));
481 m_listener.
Notify(std::move(msg_all));
484 size_t packetLength = (size_t)payload->at(1);
485 size_t vector_length = payload->size();
490 printf(
"Payload Length: %ld\n", vector_length);
492 printf(
"PGN: %ld\n", pgn);
494 for(
size_t i=0; i< vector_length ; i++){
495 printf(
"%02X ", payload->at(i));
502int CommDriverN2KSerial::GetMfgCode() {
503 unsigned char request_name[] = {0x42};
504 int ni = SendMgmtMsg(request_name,
sizeof(request_name), 0x41, 2000,
507 m_got_mfg_code =
true;
511int CommDriverN2KSerial::SendMgmtMsg(
unsigned char*
string,
size_t string_size,
512 unsigned char cmd_code,
int timeout_msec,
513 bool* response_flag) {
518 std::vector<unsigned char> msg;
520 msg.push_back(ESCAPE);
521 msg.push_back(STARTOFTEXT);
524 msg.push_back(string_size);
525 byteSum += string_size;
527 for (
unsigned int i = 0; i < string_size; i++) {
528 if (
string[i] == ESCAPE) msg.push_back(
string[i]);
529 msg.push_back(
string[i]);
530 byteSum +=
string[i];
535 CheckSum = (uint8_t)((byteSum == 0) ? 0 : (256 - byteSum));
536 msg.push_back(CheckSum);
538 msg.push_back(ESCAPE);
539 msg.push_back(ENDOFTEXT);
543 if (response_flag) *response_flag =
false;
547 bool not_done =
true;
550 if (GetSecondaryThread() && IsSecThreadActive()) {
553 if (GetSecondaryThread()->SetOutMsg(msg)) {
562 if (ntry_outer-- <= 0) not_done =
false;
566 if (!bsent)
return 1;
570 int timeout = timeout_msec;
571 while (timeout > 0) {
575 if (*response_flag) {
595int CommDriverN2KSerial::SetTXPGN(
int pgn) {
597 unsigned char request_enable[] = {0x47, 0x00, 0x00, 0x00,
598 0x00, 0x01, 0xFF, 0xFF, 0xFF, 0xFF};
601 unsigned char* c = (
unsigned char*)&pgn;
602 request_enable[1] = c[0];
603 request_enable[2] = c[1];
604 request_enable[3] = c[2];
606 int aa = SendMgmtMsg(request_enable,
sizeof(request_enable), 0x47, 2000,
611 unsigned char request_commit[] = {0x01};
612 int bb = SendMgmtMsg(request_commit,
sizeof(request_commit), 0x01, 2000,
616 unsigned char request_activate[] = {0x4B};
617 int cc = SendMgmtMsg(request_activate,
sizeof(request_activate), 0x4B, 2000,
622void CommDriverN2KSerial::AddTxPGN(
int pgn) {
623 auto it = std::find(pgn_tx_list.begin(), pgn_tx_list.end(), pgn);
624 if (it != pgn_tx_list.end())
628 pgn_tx_list.push_back(pgn);
660#define DS_RX_BUFFER_SIZE 4096
662CommDriverN2KSerialThread::CommDriverN2KSerialThread(
664 const wxString& strBaudRate) {
665 m_pParentDriver = Launcher;
667 m_PortName = PortName;
668 m_FullPortName = _T(
"Serial:") + PortName;
670 rx_buffer =
new unsigned char[DS_RX_BUFFER_SIZE + 1];
677 if (strBaudRate.ToLong(&lbaud)) m_baud = (int)lbaud;
679 std::lock_guard lock(m_stats_mutex);
680 m_driver_stats.driver_bus = NavAddr::Bus::N2000;
681 m_driver_stats.driver_iface = m_pParentDriver->m_params.GetStrippedDSPort();
682 m_driver_stats.available =
false;
688CommDriverN2KSerialThread::~CommDriverN2KSerialThread(
void) {
692void CommDriverN2KSerialThread::OnExit(
void) {}
694DriverStats CommDriverN2KSerialThread::GetStats()
const {
695 std::lock_guard lock(m_stats_mutex);
696 return m_driver_stats;
699bool CommDriverN2KSerialThread::OpenComPortPhysical(
const wxString& com_name,
702 m_serial.
setPort(com_name.ToStdString());
706 }
catch (std::exception&) {
713void CommDriverN2KSerialThread::CloseComPortPhysical() {
716 }
catch (std::exception&) {
720 std::lock_guard lock(m_stats_mutex);
721 m_driver_stats.available =
false;
724void CommDriverN2KSerialThread::SetGatewayOperationMode(
void) {
728 unsigned char config_string[] = {0x10, 0x02, 0xA1, 0x03, 0x11,
729 0x02, 0x00, 0x49, 0x10, 0x03};
733 WriteComPortPhysical(config_string, 10);
736void CommDriverN2KSerialThread::ThreadMessage(
const wxString& msg) {
743size_t CommDriverN2KSerialThread::WriteComPortPhysical(
744 std::vector<unsigned char> msg) {
745 return WriteComPortPhysical(msg.data(), msg.size());
748size_t CommDriverN2KSerialThread::WriteComPortPhysical(
unsigned char* msg,
750 if (!m_serial.
isOpen())
return 0;
752 size_t status = m_serial.
write((uint8_t*)msg, length);
755 }
catch (std::exception& e) {
756 DEBUG_LOG <<
"Unhandled Exception while writing to serial port: "
762bool CommDriverN2KSerialThread::SetOutMsg(
763 const std::vector<unsigned char>& msg) {
764 if (out_que.size() < OUT_QUEUE_LENGTH) {
772void* CommDriverN2KSerialThread::Entry() {
773 bool not_done =
true;
774 bool nl_found =
false;
781 if (!OpenComPortPhysical(m_PortName, m_baud)) {
782 wxString msg(_T(
"NMEA input device open failed: "));
783 msg.Append(m_PortName);
785 std::lock_guard lock(m_stats_mutex);
786 m_driver_stats.available =
false;
795 std::lock_guard lock(m_stats_mutex);
796 m_driver_stats.available =
true;
797 SetGatewayOperationMode();
800 m_pParentDriver->SetSecThreadActive();
803 static size_t retries = 0;
806 bool bGotESC =
false;
807 bool bGotSOT =
false;
809 while ((not_done) && (m_pParentDriver->m_Thread_run_flag > 0)) {
810 if (TestDestroy()) not_done =
false;
812 uint8_t next_byte = 0;
816 newdata = m_serial.
read(rdata, 1000);
817 }
catch (std::exception& e) {
819 std::lock_guard lock(m_stats_mutex);
822 if (10 < retries++) {
826 CloseComPortPhysical();
834 wxMilliSleep(250 * retries);
835 CloseComPortPhysical();
836 if (OpenComPortPhysical(m_PortName, m_baud)) {
837 SetGatewayOperationMode();
838 std::lock_guard lock(m_stats_mutex);
839 m_driver_stats.available =
true;
841 }
else if (retries < 10) {
842 std::lock_guard lock(m_stats_mutex);
843 m_driver_stats.available =
false;
849 std::lock_guard lock(m_stats_mutex);
852 for (
int i = 0; i < newdata; i++) {
853 circle.put(rdata[i]);
857 while (!circle.empty()) {
858 if (ib >= DS_RX_BUFFER_SIZE) ib = 0;
859 uint8_t next_byte = circle.get();
863 if (ESCAPE == next_byte) {
864 rx_buffer[ib++] = next_byte;
869 if (bGotESC && (ENDOFTEXT == next_byte)) {
873 auto buffer = std::make_shared<std::vector<unsigned char>>(
874 rx_buffer, rx_buffer + ib);
875 std::vector<unsigned char>* vec = buffer.get();
891 Nevent.SetPayload(buffer);
892 m_pParentDriver->AddPendingEvent(Nevent);
895 bGotESC = (next_byte == ESCAPE);
898 rx_buffer[ib++] = next_byte;
904 if (STARTOFTEXT == next_byte) {
910 bGotESC = (next_byte == ESCAPE);
915 rx_buffer[ib++] = next_byte;
923 bool b_qdata = !out_que.empty();
927 std::vector<unsigned char> qmsg = out_que.front();
930 if (
static_cast<size_t>(-1) == WriteComPortPhysical(qmsg) &&
935 CloseComPortPhysical();
938 b_qdata = !out_que.empty();
945 CloseComPortPhysical();
946 m_pParentDriver->SetSecThreadInActive();
947 m_pParentDriver->m_Thread_run_flag = -1;
949 std::lock_guard lock(m_stats_mutex);
950 m_driver_stats.available =
false;
956void* CommDriverN2KSerialThread::Entry() {
957 bool not_done =
true;
958 bool nl_found =
false;
963 if (!OpenComPortPhysical(m_PortName, m_baud)) {
964 wxString msg(_T(
"NMEA input device open failed: "));
965 msg.Append(m_PortName);
973 SetGatewayOperationMode();
976 m_pParentDriver->SetSecThreadActive();
979 static size_t retries = 0;
982 bool bGotESC =
false;
983 bool bGotSOT =
false;
985 while ((not_done) && (m_pParentDriver->m_Thread_run_flag > 0)) {
986 if (TestDestroy()) not_done =
false;
988 uint8_t next_byte = 0;
994 newdata = m_serial.
read(rdata, 200);
995 }
catch (std::exception& e) {
997 if (10 < retries++) {
1001 CloseComPortPhysical();
1009 wxMilliSleep(250 * retries);
1010 CloseComPortPhysical();
1011 if (OpenComPortPhysical(m_PortName, m_baud)) {
1012 SetGatewayOperationMode();
1014 }
else if (retries < 10)
1019 for (
int i = 0; i < newdata; i++) {
1020 circle.put(rdata[i]);
1024 while (!circle.empty()) {
1025 uint8_t next_byte = circle.get();
1030 if (ESCAPE == next_byte) {
1031 *put_ptr++ = next_byte;
1032 if ((put_ptr - rx_buffer) > DS_RX_BUFFER_SIZE)
1033 put_ptr = rx_buffer;
1035 }
else if (ENDOFTEXT == next_byte) {
1039 auto buffer = std::make_shared<std::vector<unsigned char>>();
1040 std::vector<unsigned char>* vec = buffer.get();
1042 unsigned char* tptr;
1045 while ((tptr != put_ptr)) {
1046 vec->push_back(*tptr++);
1047 if ((tptr - rx_buffer) > DS_RX_BUFFER_SIZE) tptr = rx_buffer;
1059 Nevent.SetPayload(buffer);
1060 m_pParentDriver->AddPendingEvent(Nevent);
1061 }
else if (next_byte == STARTOFTEXT) {
1062 put_ptr = rx_buffer;
1065 put_ptr = rx_buffer;
1071 bGotESC = (next_byte == ESCAPE);
1074 *put_ptr++ = next_byte;
1075 if ((put_ptr - rx_buffer) > DS_RX_BUFFER_SIZE)
1076 put_ptr = rx_buffer;
1082 if (STARTOFTEXT == next_byte) {
1088 bGotESC = (next_byte == ESCAPE);
1093 *put_ptr++ = next_byte;
1094 if ((put_ptr - rx_buffer) > DS_RX_BUFFER_SIZE)
1095 put_ptr = rx_buffer;
1103 bool b_qdata = !out_que.empty();
1107 std::vector<unsigned char> qmsg = out_que.front();
1110 if (
static_cast<size_t>(-1) == WriteComPortPhysical(qmsg) &&
1115 CloseComPortPhysical();
1118 b_qdata = !out_que.empty();
1123 CloseComPortPhysical();
1124 m_pParentDriver->SetSecThreadInActive();
1125 m_pParentDriver->m_Thread_run_flag = -1;
1138#define MaxActisenseMsgBuf 400
1139#define MsgTypeN2kTX 0x94
1141void AddByteEscapedToBuf(
unsigned char byteToAdd, uint8_t& idx,
1142 unsigned char* buf,
int& byteSum);
1144std::vector<unsigned char> BufferToActisenseFormat(tN2kMsg& msg) {
1145 unsigned long _PGN = msg.PGN;
1149 unsigned char ActisenseMsgBuf[MaxActisenseMsgBuf];
1151 ActisenseMsgBuf[msgIdx++] = ESCAPE;
1152 ActisenseMsgBuf[msgIdx++] = STARTOFTEXT;
1153 AddByteEscapedToBuf(MsgTypeN2kTX, msgIdx, ActisenseMsgBuf, byteSum);
1154 AddByteEscapedToBuf(msg.DataLen + 6, msgIdx, ActisenseMsgBuf,
1157 AddByteEscapedToBuf(msg.Priority, msgIdx, ActisenseMsgBuf, byteSum);
1158 AddByteEscapedToBuf(_PGN & 0xff, msgIdx, ActisenseMsgBuf, byteSum);
1160 AddByteEscapedToBuf(_PGN & 0xff, msgIdx, ActisenseMsgBuf, byteSum);
1162 AddByteEscapedToBuf(_PGN & 0xff, msgIdx, ActisenseMsgBuf, byteSum);
1163 AddByteEscapedToBuf(msg.Destination, msgIdx, ActisenseMsgBuf, byteSum);
1168 AddByteEscapedToBuf(msg.Source,msgIdx,ActisenseMsgBuf,byteSum);
1171 AddByteEscapedToBuf(_MsgTime & 0xff,msgIdx,ActisenseMsgBuf,byteSum); _MsgTime>>=8;
1172 AddByteEscapedToBuf(_MsgTime & 0xff,msgIdx,ActisenseMsgBuf,byteSum); _MsgTime>>=8;
1173 AddByteEscapedToBuf(_MsgTime & 0xff,msgIdx,ActisenseMsgBuf,byteSum); _MsgTime>>=8;
1174 AddByteEscapedToBuf(_MsgTime & 0xff,msgIdx,ActisenseMsgBuf,byteSum);
1178 AddByteEscapedToBuf(msg.DataLen, msgIdx, ActisenseMsgBuf, byteSum);
1180 for (
int i = 0; i < msg.DataLen; i++)
1181 AddByteEscapedToBuf(msg.Data[i], msgIdx, ActisenseMsgBuf, byteSum);
1184 CheckSum = (uint8_t)((byteSum == 0) ? 0 : (256 - byteSum));
1185 ActisenseMsgBuf[msgIdx++] = CheckSum;
1186 if (CheckSum == ESCAPE) ActisenseMsgBuf[msgIdx++] = CheckSum;
1188 ActisenseMsgBuf[msgIdx++] = ESCAPE;
1189 ActisenseMsgBuf[msgIdx++] = ENDOFTEXT;
1191 std::vector<unsigned char> rv;
1192 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.