OpenCPN Partial API docs
Loading...
Searching...
No Matches
comm_drv_n2k_serial.cpp
1/***************************************************************************
2 *
3 * Project: OpenCPN
4 * Purpose: Implement comm_drv_n2k.h -- Nmea2000 serial driver.
5 * Author: David Register, Alec Leamas
6 *
7 ***************************************************************************
8 * Copyright (C) 2022 by David Register, Alec Leamas *
9 * *
10 * This program is free software; you can redistribute it and/or modify *
11 * it under the terms of the GNU General Public License as published by *
12 * the Free Software Foundation; either version 2 of the License, or *
13 * (at your option) any later version. *
14 * *
15 * This program is distributed in the hope that it will be useful, *
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
18 * GNU General Public License for more details. *
19 * *
20 * You should have received a copy of the GNU General Public License *
21 * along with this program; if not, write to the *
22 * Free Software Foundation, Inc., *
23 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. *
24 **************************************************************************/
25
26// For compilers that support precompilation, includes "wx.h".
27#include <wx/wxprec.h>
28
29#ifndef WX_PRECOMP
30#include <wx/wx.h>
31#endif // precompiled headers
32
33#include <vector>
34#include <mutex> // std::mutex
35#include <queue> // std::queue
36
37#include <wx/log.h>
38
39#include "model/comm_drv_n2k_serial.h"
42#include "model/logger.h"
44
45#include <N2kMsg.h>
46
47/* Copied from canboat Project
48 * https://github.com/canboat/canboat
49 *
50 * The following startup command reverse engineered from Actisense NMEAreader.
51 * It instructs the NGT1 to clear its PGN message TX list, thus it starts
52 * sending all PGNs that it knows about.
53 */
54static unsigned char NGT_STARTUP_SEQ[] = {
55 0x11, /* msg byte 1, meaning ? */
56 0x02, /* msg byte 2, meaning ? */
57 0x00 /* msg byte 3, meaning ? */
58};
59
60std::vector<unsigned char> BufferToActisenseFormat(tN2kMsg& msg);
61
62template <typename T>
64public:
65 size_t size() {
66 std::lock_guard<std::mutex> lock(m_mutex);
67 return m_queque.size();
68 }
69
70 bool empty() {
71 std::lock_guard<std::mutex> lock(m_mutex);
72 return m_queque.empty();
73 }
74
75 const T& front() {
76 std::lock_guard<std::mutex> lock(m_mutex);
77 return m_queque.front();
78 }
79
80 void push(const T& value) {
81 std::lock_guard<std::mutex> lock(m_mutex);
82 m_queque.push(value);
83 }
84
85 void pop() {
86 std::lock_guard<std::mutex> lock(m_mutex);
87 m_queque.pop();
88 }
89
90private:
91 std::queue<T> m_queque;
92 mutable std::mutex m_mutex;
93};
94
95template <class T>
96class circular_buffer {
97public:
98 explicit circular_buffer(size_t size)
99 : buf_(std::unique_ptr<T[]>(new T[size])), max_size_(size) {}
100
101 void reset();
102 size_t capacity() const;
103 size_t size() const;
104
105 bool empty() const {
106 // if head and tail are equal, we are empty
107 return (!full_ && (head_ == tail_));
108 }
109
110 bool full() const {
111 // If tail is ahead the head by 1, we are full
112 return full_;
113 }
114
115 void put(T item) {
116 std::lock_guard<std::mutex> lock(mutex_);
117 buf_[head_] = item;
118 if (full_) tail_ = (tail_ + 1) % max_size_;
119
120 head_ = (head_ + 1) % max_size_;
121
122 full_ = head_ == tail_;
123 }
124
125 T get() {
126 std::lock_guard<std::mutex> lock(mutex_);
127
128 if (empty()) return T();
129
130 // Read data and advance the tail (we now have a free space)
131 auto val = buf_[tail_];
132 full_ = false;
133 tail_ = (tail_ + 1) % max_size_;
134
135 return val;
136 }
137
138private:
139 std::mutex mutex_;
140 std::unique_ptr<T[]> buf_;
141 size_t head_ = 0;
142 size_t tail_ = 0;
143 const size_t max_size_;
144 bool full_ = 0;
145};
146
147class CommDriverN2KSerialEvent; // fwd
148
149class CommDriverN2KSerialThread : public wxThread {
150public:
152 const wxString& PortName,
153 const wxString& strBaudRate);
154
156 void* Entry();
157 bool SetOutMsg(const std::vector<unsigned char>& load);
158 void OnExit(void);
159 DriverStats GetStats() const;
160
161private:
162#ifndef __ANDROID__
163 serial::Serial m_serial;
164#endif
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);
171
172 CommDriverN2KSerial* m_pParentDriver;
173 wxString m_PortName;
174 wxString m_FullPortName;
175
176 unsigned char* put_ptr;
177 unsigned char* tak_ptr;
178
179 unsigned char* rx_buffer;
180
181 int m_baud;
182 int m_n_timeout;
183
185 DriverStats m_driver_stats;
186 mutable std::mutex m_stats_mutex;
187#ifdef __WXMSW__
188 HANDLE m_hSerialComm;
189 bool m_nl_found;
190#endif
191};
192
194wxDECLARE_EVENT(wxEVT_COMMDRIVER_N2K_SERIAL, CommDriverN2KSerialEvent);
195
196class CommDriverN2KSerialEvent : public wxEvent {
197public:
198 CommDriverN2KSerialEvent(wxEventType commandType = wxEVT_NULL, int id = 0)
199 : wxEvent(id, commandType) {};
201
202 // accessors
203 void SetPayload(std::shared_ptr<std::vector<unsigned char>> data) {
204 m_payload = data;
205 }
206 std::shared_ptr<std::vector<unsigned char>> GetPayload() { return m_payload; }
207
208 // required for sending with wxPostEvent()
209 wxEvent* Clone() const {
211 newevent->m_payload = this->m_payload;
212 return newevent;
213 };
214
215private:
216 std::shared_ptr<std::vector<unsigned char>> m_payload;
217};
218
219//========================================================================
220/* commdriverN2KSerial implementation
221 * */
222
223wxDEFINE_EVENT(wxEVT_COMMDRIVER_N2K_SERIAL, CommDriverN2KSerialEvent);
224
225CommDriverN2KSerial::CommDriverN2KSerial(const ConnectionParams* params,
226 DriverListener& listener)
227 : CommDriverN2K(params->GetStrippedDSPort()),
228 m_Thread_run_flag(-1),
229 m_params(*params),
230 m_bok(false),
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");
241
242 // Prepare the wxEventHandler to accept events from the actual hardware thread
243 Bind(wxEVT_COMMDRIVER_N2K_SERIAL, &CommDriverN2KSerial::handle_N2K_SERIAL_RAW,
244 this);
245
246 // Dummy Driver Stats, may be polled before worker thread is active
247 m_driver_stats.driver_bus = NavAddr::Bus::N2000;
248 m_driver_stats.driver_iface = m_params.GetStrippedDSPort();
249 m_driver_stats.available = false;
250
251 Open();
252
253 wxMilliSleep(100);
254 GetMfgCode();
255
256 // Initialize the device clearing all rx/tx filterx
257 SendMgmtMsg(NGT_STARTUP_SEQ, sizeof(NGT_STARTUP_SEQ), 0x11, 0, NULL);
258
259#if 0
260 // Testing TX of Heartbeat
261 wxSleep(1);
262
263 tN2kMsg N2kMsg; // automatically sets destination 255
264 //SetHeartbeat(N2kMsg,2000,0);
265 //SetN2kPGN126993(N2kMsg, 2000, 0);
266 N2kMsg.SetPGN(126993L);
267 //N2kMsg.Priority=7;
268 N2kMsg.Source = 2;
269 N2kMsg.Destination = 133;
270 N2kMsg.Add2ByteUInt((uint16_t)(2000)); // Rate, msec
271
272 N2kMsg.AddByte(0); //Status
273 N2kMsg.AddByte(0xff); // Reserved
274 N2kMsg.Add4ByteUInt(0xffffffff); // Reserved
275
276 const std::vector<unsigned char> mv = BufferToActisenseFormat(N2kMsg);
277
278 size_t len = mv.size();
279
280 wxString comx = m_params.GetDSPort().AfterFirst(':');
281 std::string interface = comx.ToStdString();
282
283 N2kName source_name(1234);
284 auto source_address = std::make_shared<NavAddr2000>(interface, source_name);
285 auto dest_address = std::make_shared<NavAddr2000>(interface, N2kMsg.Destination);
286
287 auto message_to_send = std::make_shared<Nmea2000Msg>(126993L,
288 mv, source_address, 3);
289
290 for(size_t i=0; i< mv.size(); i++){
291 printf("%02X ", mv.at(i));
292 }
293 printf("\n\n");
294
295 SetTXPGN(126993);
296 wxSleep(1);
297
298 SendMessage(message_to_send, dest_address);
299
300 int yyp = 4;
301#endif
302}
303
304CommDriverN2KSerial::~CommDriverN2KSerial() { Close(); }
305
306DriverStats CommDriverN2KSerial::GetDriverStats() const {
307 if (m_pSecondary_Thread)
308 return m_pSecondary_Thread->GetStats();
309 else
310 return m_driver_stats;
311}
312
313bool CommDriverN2KSerial::Open() {
314 wxString comx;
315 comx = m_params.GetDSPort().AfterFirst(':'); // strip "Serial:"
316
317 comx =
318 comx.BeforeFirst(' '); // strip off any description provided by Windows
319
320#ifndef ANDROID
321 // Kick off the RX thread
322 SetSecondaryThread(new CommDriverN2KSerialThread(this, comx, m_BaudRate));
323 SetThreadRunFlag(1);
324 GetSecondaryThread()->Run();
325#endif
326
327 return true;
328}
329
330void CommDriverN2KSerial::Close() {
331 wxLogMessage(
332 wxString::Format(_T("Closing N2K Driver %s"), m_portstring.c_str()));
333
334 // Kill off the Secondary RX Thread if alive
335 if (m_pSecondary_Thread) {
336 if (m_bsec_thread_active) // Try to be sure thread object is still alive
337 {
338 wxLogMessage(_T("Stopping Secondary Thread"));
339
340 m_Thread_run_flag = 0;
341 int tsec = 10;
342 while ((m_Thread_run_flag >= 0) && (tsec--)) wxSleep(1);
343
344 wxString msg;
345 if (m_Thread_run_flag < 0)
346 msg.Printf(_T("Stopped in %d sec."), 10 - tsec);
347 else
348 msg.Printf(_T("Not Stopped after 10 sec."));
349 wxLogMessage(msg);
350 }
351
352 m_pSecondary_Thread = NULL;
353 m_bsec_thread_active = false;
354 }
355}
356static uint64_t PayloadToName(const std::vector<unsigned char> payload) {
357 uint64_t name;
358 memcpy(&name, reinterpret_cast<const void*>(payload.data()), sizeof(name));
359 return name;
360}
361
362bool CommDriverN2KSerial::SendMessage(std::shared_ptr<const NavMsg> msg,
363 std::shared_ptr<const NavAddr> addr) {
364#ifndef ANDROID
365
366 auto msg_n2k = std::dynamic_pointer_cast<const Nmea2000Msg>(msg);
367 std::vector<uint8_t> load = msg_n2k->payload;
368
369 uint64_t _pgn = msg_n2k->PGN.pgn;
370 auto destination_address = std::static_pointer_cast<const NavAddr2000>(addr);
371
372 tN2kMsg N2kMsg; // automatically sets destination 255
373 N2kMsg.SetPGN(_pgn);
374 N2kMsg.Priority = msg_n2k->priority;
375 if (destination_address) N2kMsg.Destination = destination_address->address;
376
377 for (size_t i = 0; i < load.size(); i++) N2kMsg.AddByte(load.at(i)); // data
378
379 const std::vector<uint8_t> acti_pkg = BufferToActisenseFormat(N2kMsg);
380
381 // Create the internal message for all N2K listeners
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);
386 auto msg_all =
387 std::make_shared<const Nmea2000Msg>(1, msg_payload, GetAddress(name));
388
389 // Notify listeners
390 m_listener.Notify(std::move(msg));
391 m_listener.Notify(std::move(msg_all));
392
393 if (GetSecondaryThread()) {
394 if (IsSecThreadActive()) {
395 int retry = 10;
396 while (retry) {
397 if (GetSecondaryThread()->SetOutMsg(acti_pkg))
398 return true;
399 else
400 retry--;
401 }
402 return false; // could not send after several tries....
403 } else
404 return false;
405 }
406#endif
407 return true;
408}
409
410void CommDriverN2KSerial::ProcessManagementPacket(
411 std::vector<unsigned char>* payload) {
412 if (payload->at(2) != 0xF2) { // hearbeat
413 // printf(" pl ");
414 // for (unsigned int i = 0; i < payload->size(); i++)
415 // printf("%02X ", payload->at(i));
416 // printf("\n");
417 }
418
419 switch (payload->at(2)) {
420 case 0x47:
421 m_bmg47_resp = true;
422 break;
423 case 0x01:
424 m_bmg01_resp = true;
425 break;
426 case 0x4B:
427 m_bmg4B_resp = true;
428 break;
429 case 0x041: {
430 m_bmg41_resp = true;
431 if (payload->at(3) == 0x02) { // ASCII device_common_name
432 std::string device_common_name;
433 for (unsigned int i = 0; i < 32; i++) {
434 device_common_name += payload->at(i + 14);
435 }
436 device_common_name += '\0';
437 m_device_common_name = device_common_name;
438 }
439 break;
440 }
441 case 0x042: {
442 m_bmg42_resp = true;
443 unsigned char name[8];
444 for (unsigned int i = 0; i < 8; i++) name[i] = payload->at(i + 15);
445
446 memcpy((void*)&NAME, name, 8);
447 // Extract the manufacturers code
448 int* f1 = (int*)&NAME;
449 int f1d = *f1;
450 m_manufacturers_code = f1d >> 21;
451 break;
452 }
453
454 default:
455 break;
456 }
457}
458
459void CommDriverN2KSerial::handle_N2K_SERIAL_RAW(
461 auto p = event.GetPayload();
462
463 std::vector<unsigned char>* payload = p.get();
464
465 if (payload->at(0) == 0xA0) {
466 ProcessManagementPacket(payload);
467 return;
468 }
469
470 // extract PGN
471 uint64_t pgn = 0;
472 unsigned char* c = (unsigned char*)&pgn;
473 *c++ = payload->at(3);
474 *c++ = payload->at(4);
475 *c++ = payload->at(5);
476 // memcpy(&v, &data[3], 1);
477 // printf(" %ld\n", pgn);
478
479 auto name = PayloadToName(*payload);
480 auto msg =
481 std::make_shared<const Nmea2000Msg>(pgn, *payload, GetAddress(name));
482 auto msg_all =
483 std::make_shared<const Nmea2000Msg>(1, *payload, GetAddress(name));
484
485 m_listener.Notify(std::move(msg));
486 m_listener.Notify(std::move(msg_all));
487
488#if 0 // Debug output
489 size_t packetLength = (size_t)payload->at(1);
490 size_t vector_length = payload->size();
491
492
493 //if(pgn > 120000)
494 {
495 printf("Payload Length: %ld\n", vector_length);
496
497 printf("PGN: %ld\n", pgn);
498
499 for(size_t i=0; i< vector_length ; i++){
500 printf("%02X ", payload->at(i));
501 }
502 printf("\n\n");
503 }
504#endif
505}
506
507int CommDriverN2KSerial::GetMfgCode() {
508 unsigned char request_name[] = {0x42};
509 int ni = SendMgmtMsg(request_name, sizeof(request_name), 0x41, 2000,
510 &m_bmg42_resp);
511 if (ni) return ni; // Not responding, return error so upper retries.
512 m_got_mfg_code = true;
513 return 0;
514}
515
516int CommDriverN2KSerial::SendMgmtMsg(unsigned char* string, size_t string_size,
517 unsigned char cmd_code, int timeout_msec,
518 bool* response_flag) {
519#ifndef ANDROID
520 // Make a message
521 int byteSum = 0;
522 uint8_t CheckSum;
523 std::vector<unsigned char> msg;
524
525 msg.push_back(ESCAPE);
526 msg.push_back(STARTOFTEXT);
527 msg.push_back(0xA1);
528 byteSum += 0xA1;
529 msg.push_back(string_size); // payload length
530 byteSum += string_size;
531
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];
536 }
537
538 // checksum
539 byteSum %= 256;
540 CheckSum = (uint8_t)((byteSum == 0) ? 0 : (256 - byteSum));
541 msg.push_back(CheckSum);
542
543 msg.push_back(ESCAPE);
544 msg.push_back(ENDOFTEXT);
545
546 // send it out
547
548 if (response_flag) *response_flag = false; // prime the response detector
549
550 // Send the msg
551 bool bsent = false;
552 bool not_done = true;
553 int ntry_outer = 10;
554 while (not_done) {
555 if (GetSecondaryThread() && IsSecThreadActive()) {
556 int retry = 10;
557 while (retry) {
558 if (GetSecondaryThread()->SetOutMsg(msg)) {
559 bsent = true;
560 not_done = false;
561 break;
562 } else
563 retry--;
564 }
565 } else {
566 wxMilliSleep(100);
567 if (ntry_outer-- <= 0) not_done = false;
568 }
569 }
570
571 if (!bsent) return 1;
572
573 bool bOK = false;
574 if (timeout_msec) {
575 int timeout = timeout_msec;
576 while (timeout > 0) {
577 wxYieldIfNeeded();
578 wxMilliSleep(100);
579 if (response_flag) {
580 if (*response_flag) {
581 bOK = true;
582 break;
583 }
584 }
585 timeout -= 100;
586 }
587 } else
588 bOK = true;
589
590 if (!bOK) {
591 // printf( "***Err-1\n");
592 return 1;
593 }
594 // else
595 // printf("***OK-1 %d\n", timeout);
596#endif
597 return 0;
598}
599
600int CommDriverN2KSerial::SetTXPGN(int pgn) {
601 // Enable PGN message
602 unsigned char request_enable[] = {0x47, 0x00, 0x00, 0x00, // pgn
603 0x00, 0x01, 0xFF, 0xFF, 0xFF, 0xFF};
604
605 int PGN = 0;
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];
610
611 int aa = SendMgmtMsg(request_enable, sizeof(request_enable), 0x47, 2000,
612 &m_bmg47_resp);
613 if (aa) return 0; // Probably YDNU-02 device
614
615 // Commit message
616 unsigned char request_commit[] = {0x01};
617 int bb = SendMgmtMsg(request_commit, sizeof(request_commit), 0x01, 2000,
618 &m_bmg01_resp);
619
620 // Activate message
621 unsigned char request_activate[] = {0x4B};
622 int cc = SendMgmtMsg(request_activate, sizeof(request_activate), 0x4B, 2000,
623 &m_bmg4B_resp);
624
625 return 0;
626}
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())
630 return;
631 else {
632 SetTXPGN(pgn);
633 pgn_tx_list.push_back(pgn);
634 }
635}
636
637#ifndef __ANDROID__
638
645// Commonly used raw format is actually inherited from an old paketizing format:
646// <10><02><application data><CRC (1)><10><03>
647
648// Actisense application data, from NGT-1 to PC
649// <data code=93><length (1)><priority (1)><PGN (3)><destination(1)><source
650// (1)><time (4)><len (1)><data (len)>
651
652// As applied to a real application data element, after extraction from packet
653// format: 93 13 02 01 F8 01 FF 01 76 C2 52 00 08 08 70 EB 14 E8 8E 52 D2 BB 10
654
655// length (1): 0x13
656// priority (1); 0x02
657// PGN (3): 0x01 0xF8 0x01
658// destination(1): 0xFF
659// source (1): 0x01
660// time (4): 0x76 0xC2 0x52 0x00
661// len (1): 0x08
662// data (len): 08 70 EB 14 E8 8E 52 D2
663// packet CRC: 0xBB
664
665#define DS_RX_BUFFER_SIZE 4096
666
667CommDriverN2KSerialThread::CommDriverN2KSerialThread(
668 CommDriverN2KSerial* Launcher, const wxString& PortName,
669 const wxString& strBaudRate) {
670 m_pParentDriver = Launcher; // This thread's immediate "parent"
671
672 m_PortName = PortName;
673 m_FullPortName = _T("Serial:") + PortName;
674
675 rx_buffer = new unsigned char[DS_RX_BUFFER_SIZE + 1];
676
677 put_ptr = rx_buffer; // local circular queue
678 tak_ptr = rx_buffer;
679
680 m_baud = 9600; // default
681 long lbaud;
682 if (strBaudRate.ToLong(&lbaud)) m_baud = (int)lbaud;
683 {
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;
688 }
689
690 Create();
691}
692
693CommDriverN2KSerialThread::~CommDriverN2KSerialThread(void) {
694 delete[] rx_buffer;
695}
696
697void CommDriverN2KSerialThread::OnExit(void) {}
698
699DriverStats CommDriverN2KSerialThread::GetStats() const {
700 std::lock_guard lock(m_stats_mutex);
701 return m_driver_stats;
702}
703
704bool CommDriverN2KSerialThread::OpenComPortPhysical(const wxString& com_name,
705 int baud_rate) {
706 try {
707 m_serial.setPort(com_name.ToStdString());
708 m_serial.setBaudrate(baud_rate);
709 m_serial.open();
710 m_serial.setTimeout(250, 250, 0, 250, 0);
711 } catch (std::exception&) {
712 // std::cerr << "Unhandled Exception while opening serial port: " <<
713 // e.what() << std::endl;
714 }
715 return m_serial.isOpen();
716}
717
718void CommDriverN2KSerialThread::CloseComPortPhysical() {
719 try {
720 m_serial.close();
721 } catch (std::exception&) {
722 // std::cerr << "Unhandled Exception while closing serial port: " <<
723 // e.what() << std::endl;
724 }
725 std::lock_guard lock(m_stats_mutex);
726 m_driver_stats.available = false;
727}
728
729void CommDriverN2KSerialThread::SetGatewayOperationMode(void) {
730 // For YDNU-02 device
731 // From Device User Manual
732 // Set the mode to "N2K"
733 unsigned char config_string[] = {0x10, 0x02, 0xA1, 0x03, 0x11,
734 0x02, 0x00, 0x49, 0x10, 0x03};
735 // std::vector<byte>writeBuffer {DLE,STX,NGT_TX_CMD,0x03,0x11,0x02,0x00,0x49,
736 // DLE,ETX};
737
738 WriteComPortPhysical(config_string, 10);
739}
740
741void CommDriverN2KSerialThread::ThreadMessage(const wxString& msg) {
742 // Signal the main program thread
743 // OCPN_ThreadMessageEvent event(wxEVT_OCPN_THREADMSG, 0);
744 // event.SetSString(std::string(msg.mb_str()));
745 // if (gFrame) gFrame->GetEventHandler()->AddPendingEvent(event);
746}
747
748size_t CommDriverN2KSerialThread::WriteComPortPhysical(
749 std::vector<unsigned char> msg) {
750 return WriteComPortPhysical(msg.data(), msg.size());
751}
752
753size_t CommDriverN2KSerialThread::WriteComPortPhysical(unsigned char* msg,
754 size_t length) {
755 if (!m_serial.isOpen()) return 0;
756 try {
757 size_t status = m_serial.write((uint8_t*)msg, length);
758 if (status) m_serial.flushOutput();
759 return status;
760 } catch (std::exception& e) {
761 DEBUG_LOG << "Unhandled Exception while writing to serial port: "
762 << e.what();
763 }
764 return 0;
765}
766
767bool CommDriverN2KSerialThread::SetOutMsg(
768 const std::vector<unsigned char>& msg) {
769 if (out_que.size() < OUT_QUEUE_LENGTH) {
770 out_que.push(msg);
771 return true;
772 }
773 return false;
774}
775
776#ifndef __WXMSW__
777void* CommDriverN2KSerialThread::Entry() {
778 bool not_done = true;
779 bool nl_found = false;
780 wxString msg;
781 uint8_t rdata[2000];
782 circular_buffer<uint8_t> circle(DS_RX_BUFFER_SIZE);
783 int ib = 0;
784
785 // Request the com port from the comm manager
786 if (!OpenComPortPhysical(m_PortName, m_baud)) {
787 wxString msg(_T("NMEA input device open failed: "));
788 msg.Append(m_PortName);
789 ThreadMessage(msg);
790 std::lock_guard lock(m_stats_mutex);
791 m_driver_stats.available = false;
792
793 // goto thread_exit; // This means we will not be trying to connect = The
794 // device must be connected when the thread is created. Does not seem to be
795 // needed/what we want as the reconnection logic is able to pick it up
796 // whenever it actually appears (Of course given it appears with the
797 // expected device name).
798 } else {
799 wxMilliSleep(100);
800 std::lock_guard lock(m_stats_mutex);
801 m_driver_stats.available = true;
802 SetGatewayOperationMode();
803 }
804
805 m_pParentDriver->SetSecThreadActive(); // I am alive
806
807 // The main loop
808 static size_t retries = 0;
809
810 bool bInMsg = false;
811 bool bGotESC = false;
812 bool bGotSOT = false;
813
814 while ((not_done) && (m_pParentDriver->m_Thread_run_flag > 0)) {
815 if (TestDestroy()) not_done = false; // smooth exit
816
817 uint8_t next_byte = 0;
818 int newdata = 0;
819 if (m_serial.isOpen()) {
820 try {
821 newdata = m_serial.read(rdata, 1000);
822 } catch (std::exception& e) {
823 // std::cerr << "Serial read exception: " << e.what() << std::endl;
824 std::lock_guard lock(m_stats_mutex);
825 m_driver_stats.error_count++;
826
827 if (10 < retries++) {
828 // We timed out waiting for the next character 10 times, let's close
829 // the port so that the reconnection logic kicks in and tries to fix
830 // our connection.
831 CloseComPortPhysical();
832 retries = 0;
833 }
834 }
835 } else {
836 // Reconnection logic. Let's try to reopen the port while waiting longer
837 // every time (until we simply keep trying every 2.5 seconds)
838 // std::cerr << "Serial port seems closed." << std::endl;
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;
845 retries = 0;
846 } else if (retries < 10) {
847 std::lock_guard lock(m_stats_mutex);
848 m_driver_stats.available = false;
849 retries++;
850 }
851 }
852
853 if (newdata > 0) {
854 std::lock_guard lock(m_stats_mutex);
855 m_driver_stats.rx_count += newdata;
856
857 for (int i = 0; i < newdata; i++) {
858 circle.put(rdata[i]);
859 }
860 }
861
862 while (!circle.empty()) {
863 if (ib >= DS_RX_BUFFER_SIZE) ib = 0;
864 uint8_t next_byte = circle.get();
865
866 if (bInMsg) {
867 if (bGotESC) {
868 if (ESCAPE == next_byte) {
869 rx_buffer[ib++] = next_byte;
870 bGotESC = false;
871 }
872 }
873
874 if (bGotESC && (ENDOFTEXT == next_byte)) {
875 // Process packet
876 // Copy the message into a std::vector
877
878 auto buffer = std::make_shared<std::vector<unsigned char>>(
879 rx_buffer, rx_buffer + ib);
880 std::vector<unsigned char>* vec = buffer.get();
881
882 ib = 0;
883 // tak_ptr = tptr;
884 bInMsg = false;
885 bGotESC = false;
886
887 // printf("raw ");
888 // for (unsigned int i = 0; i < vec->size(); i++)
889 // printf("%02X ", vec->at(i));
890 // printf("\n");
891
892 // Message is finished
893 // Send the captured raw data vector pointer to the thread's "parent"
894 // thereby releasing the thread for further data capture
895 CommDriverN2KSerialEvent Nevent(wxEVT_COMMDRIVER_N2K_SERIAL, 0);
896 Nevent.SetPayload(buffer);
897 m_pParentDriver->AddPendingEvent(Nevent);
898
899 } else {
900 bGotESC = (next_byte == ESCAPE);
901
902 if (!bGotESC) {
903 rx_buffer[ib++] = next_byte;
904 }
905 }
906 }
907
908 else {
909 if (STARTOFTEXT == next_byte) {
910 bGotSOT = false;
911 if (bGotESC) {
912 bGotSOT = true;
913 }
914 } else {
915 bGotESC = (next_byte == ESCAPE);
916 if (bGotSOT) {
917 bGotSOT = false;
918 bInMsg = true;
919
920 rx_buffer[ib++] = next_byte;
921 }
922 }
923 }
924 } // if newdata > 0
925
926 // Check for any pending output message
927#if 1
928 bool b_qdata = !out_que.empty();
929
930 while (b_qdata) {
931 // Take a copy of message
932 std::vector<unsigned char> qmsg = out_que.front();
933 out_que.pop();
934
935 if (static_cast<size_t>(-1) == WriteComPortPhysical(qmsg) &&
936 10 < retries++) {
937 // We failed to write the port 10 times, let's close the port so that
938 // the reconnection logic kicks in and tries to fix our connection.
939 retries = 0;
940 CloseComPortPhysical();
941 }
942
943 b_qdata = !out_que.empty();
944 } // while b_qdata
945
946#endif
947 } // while ((not_done)
948
949 // thread_exit:
950 CloseComPortPhysical();
951 m_pParentDriver->SetSecThreadInActive(); // I am dead
952 m_pParentDriver->m_Thread_run_flag = -1;
953
954 std::lock_guard lock(m_stats_mutex);
955 m_driver_stats.available = false;
956
957 return 0;
958}
959
960#else
961void* CommDriverN2KSerialThread::Entry() {
962 bool not_done = true;
963 bool nl_found = false;
964 wxString msg;
965 circular_buffer<uint8_t> circle(DS_RX_BUFFER_SIZE);
966
967 // Request the com port from the comm manager
968 if (!OpenComPortPhysical(m_PortName, m_baud)) {
969 wxString msg(_T("NMEA input device open failed: "));
970 msg.Append(m_PortName);
971 ThreadMessage(msg);
972 // goto thread_exit; // This means we will not be trying to connect = The
973 // device must be connected when the thread is created. Does not seem to be
974 // needed/what we want as the reconnection logic is able to pick it up
975 // whenever it actually appears (Of course given it appears with the
976 // expected device name).
977 } else {
978 SetGatewayOperationMode();
979 }
980
981 m_pParentDriver->SetSecThreadActive(); // I am alive
982
983 // The main loop
984 static size_t retries = 0;
985
986 bool bInMsg = false;
987 bool bGotESC = false;
988 bool bGotSOT = false;
989
990 while ((not_done) && (m_pParentDriver->m_Thread_run_flag > 0)) {
991 if (TestDestroy()) not_done = false; // smooth exit
992
993 uint8_t next_byte = 0;
994 int newdata = -1;
995 uint8_t rdata[2000];
996
997 if (m_serial.isOpen()) {
998 try {
999 newdata = m_serial.read(rdata, 200);
1000 } catch (std::exception& e) {
1001 // std::cerr << "Serial read exception: " << e.what() << std::endl;
1002 if (10 < retries++) {
1003 // We timed out waiting for the next character 10 times, let's close
1004 // the port so that the reconnection logic kicks in and tries to fix
1005 // our connection.
1006 CloseComPortPhysical();
1007 retries = 0;
1008 }
1009 }
1010 } else {
1011 // Reconnection logic. Let's try to reopen the port while waiting longer
1012 // every time (until we simply keep trying every 2.5 seconds)
1013 // std::cerr << "Serial port seems closed." << std::endl;
1014 wxMilliSleep(250 * retries);
1015 CloseComPortPhysical();
1016 if (OpenComPortPhysical(m_PortName, m_baud)) {
1017 SetGatewayOperationMode();
1018 retries = 0;
1019 } else if (retries < 10)
1020 retries++;
1021 }
1022
1023 if (newdata > 0) {
1024 for (int i = 0; i < newdata; i++) {
1025 circle.put(rdata[i]);
1026 }
1027 }
1028
1029 while (!circle.empty()) {
1030 uint8_t next_byte = circle.get();
1031
1032 if (1) {
1033 if (bInMsg) {
1034 if (bGotESC) {
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;
1039 bGotESC = false;
1040 } else if (ENDOFTEXT == next_byte) {
1041 // Process packet
1042 // Copy the message into a std::vector
1043
1044 auto buffer = std::make_shared<std::vector<unsigned char>>();
1045 std::vector<unsigned char>* vec = buffer.get();
1046
1047 unsigned char* tptr;
1048 tptr = tak_ptr;
1049
1050 while ((tptr != put_ptr)) {
1051 vec->push_back(*tptr++);
1052 if ((tptr - rx_buffer) > DS_RX_BUFFER_SIZE) tptr = rx_buffer;
1053 }
1054
1055 tak_ptr = tptr;
1056 bInMsg = false;
1057 bGotESC = false;
1058
1059 // Message is finished
1060 // Send the captured raw data vector pointer to the thread's
1061 // "parent"
1062 // thereby releasing the thread for further data capture
1063 CommDriverN2KSerialEvent Nevent(wxEVT_COMMDRIVER_N2K_SERIAL, 0);
1064 Nevent.SetPayload(buffer);
1065 m_pParentDriver->AddPendingEvent(Nevent);
1066 } else if (next_byte == STARTOFTEXT) {
1067 put_ptr = rx_buffer;
1068 bGotESC = false;
1069 } else {
1070 put_ptr = rx_buffer;
1071 bInMsg = false;
1072 bGotESC = false;
1073 }
1074
1075 } else {
1076 bGotESC = (next_byte == ESCAPE);
1077
1078 if (!bGotESC) {
1079 *put_ptr++ = next_byte;
1080 if ((put_ptr - rx_buffer) > DS_RX_BUFFER_SIZE)
1081 put_ptr = rx_buffer;
1082 }
1083 }
1084 }
1085
1086 else {
1087 if (STARTOFTEXT == next_byte) {
1088 bGotSOT = false;
1089 if (bGotESC) {
1090 bGotSOT = true;
1091 }
1092 } else {
1093 bGotESC = (next_byte == ESCAPE);
1094 if (bGotSOT) {
1095 bGotSOT = false;
1096 bInMsg = true;
1097
1098 *put_ptr++ = next_byte;
1099 if ((put_ptr - rx_buffer) > DS_RX_BUFFER_SIZE)
1100 put_ptr = rx_buffer;
1101 }
1102 }
1103 }
1104 } // if newdata > 0
1105 } // while
1106
1107 // Check for any pending output message
1108 bool b_qdata = !out_que.empty();
1109
1110 while (b_qdata) {
1111 // Take a copy of message
1112 std::vector<unsigned char> qmsg = out_que.front();
1113 out_que.pop();
1114
1115 if (static_cast<size_t>(-1) == WriteComPortPhysical(qmsg) &&
1116 10 < retries++) {
1117 // We failed to write the port 10 times, let's close the port so that
1118 // the reconnection logic kicks in and tries to fix our connection.
1119 retries = 0;
1120 CloseComPortPhysical();
1121 }
1122
1123 b_qdata = !out_que.empty();
1124 } // while b_qdata
1125 } // while ((not_done)
1126
1127 // thread_exit:
1128 CloseComPortPhysical();
1129 m_pParentDriver->SetSecThreadInActive(); // I am dead
1130 m_pParentDriver->m_Thread_run_flag = -1;
1131
1132 return 0;
1133}
1134
1135#endif // wxmsw Entry()
1136
1137#endif // Android
1138
1139//*****************************************************************************
1140// Actisense Format:
1141// <10><02><93><length (1)><priority (1)><PGN (3)><destination (1)><source
1142// (1)><time (4)><len (1)><data (len)><CRC (1)><10><03>
1143#define MaxActisenseMsgBuf 400
1144#define MsgTypeN2kTX 0x94
1145
1146void AddByteEscapedToBuf(unsigned char byteToAdd, uint8_t& idx,
1147 unsigned char* buf, int& byteSum);
1148
1149std::vector<unsigned char> BufferToActisenseFormat(tN2kMsg& msg) {
1150 unsigned long _PGN = msg.PGN;
1151 uint8_t msgIdx = 0;
1152 int byteSum = 0;
1153 uint8_t CheckSum;
1154 unsigned char ActisenseMsgBuf[MaxActisenseMsgBuf];
1155
1156 ActisenseMsgBuf[msgIdx++] = ESCAPE;
1157 ActisenseMsgBuf[msgIdx++] = STARTOFTEXT;
1158 AddByteEscapedToBuf(MsgTypeN2kTX, msgIdx, ActisenseMsgBuf, byteSum);
1159 AddByteEscapedToBuf(msg.DataLen + 6, msgIdx, ActisenseMsgBuf,
1160 byteSum); // length does not include escaped chars
1161
1162 AddByteEscapedToBuf(msg.Priority, msgIdx, ActisenseMsgBuf, byteSum);
1163 AddByteEscapedToBuf(_PGN & 0xff, msgIdx, ActisenseMsgBuf, byteSum);
1164 _PGN >>= 8;
1165 AddByteEscapedToBuf(_PGN & 0xff, msgIdx, ActisenseMsgBuf, byteSum);
1166 _PGN >>= 8;
1167 AddByteEscapedToBuf(_PGN & 0xff, msgIdx, ActisenseMsgBuf, byteSum);
1168 AddByteEscapedToBuf(msg.Destination, msgIdx, ActisenseMsgBuf, byteSum);
1169
1170#if 0
1171 // For TX through Actisense compatible gateway, we skip "source" byte and msg time fields
1172 // Source
1173 AddByteEscapedToBuf(msg.Source,msgIdx,ActisenseMsgBuf,byteSum);
1174 // Time
1175 int _MsgTime = 0;
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);
1180
1181#endif
1182
1183 AddByteEscapedToBuf(msg.DataLen, msgIdx, ActisenseMsgBuf, byteSum);
1184
1185 for (int i = 0; i < msg.DataLen; i++)
1186 AddByteEscapedToBuf(msg.Data[i], msgIdx, ActisenseMsgBuf, byteSum);
1187 byteSum %= 256;
1188
1189 CheckSum = (uint8_t)((byteSum == 0) ? 0 : (256 - byteSum));
1190 ActisenseMsgBuf[msgIdx++] = CheckSum;
1191 if (CheckSum == ESCAPE) ActisenseMsgBuf[msgIdx++] = CheckSum;
1192
1193 ActisenseMsgBuf[msgIdx++] = ESCAPE;
1194 ActisenseMsgBuf[msgIdx++] = ENDOFTEXT;
1195
1196 std::vector<unsigned char> rv;
1197 for (unsigned int i = 0; i < msgIdx; i++) rv.push_back(ActisenseMsgBuf[i]);
1198
1199 return rv;
1200}
Interface implemented by transport layer and possible other parties like test code which should handl...
Definition comm_driver.h:48
virtual void Notify(std::shared_ptr< const NavMsg > message)=0
Handle a received message.
Class that provides a portable serial port interface.
Definition serial.h:147
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.
Definition comm_navmsg.h:69