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