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_closing(false) {
236 m_BaudRate = wxString::Format("%i", params->Baudrate), SetSecThreadInActive();
237 m_manufacturers_code = 0;
238 m_got_mfg_code = false;
239 this->attributes["canAddress"] = std::string("-1");
240 this->attributes["userComment"] = params->UserComment.ToStdString();
241 this->attributes["ioDirection"] = DsPortTypeToString(params->IOSelect);
242
243 // Prepare the wxEventHandler to accept events from the actual hardware thread
244 Bind(wxEVT_COMMDRIVER_N2K_SERIAL, &CommDriverN2KSerial::handle_N2K_SERIAL_RAW,
245 this);
246
247 // Dummy Driver Stats, may be polled before worker thread is active
248 m_driver_stats.driver_bus = NavAddr::Bus::N2000;
249 m_driver_stats.driver_iface = m_params.GetStrippedDSPort();
250 m_driver_stats.available = false;
251
252 Open();
253
254 wxMilliSleep(100);
255 GetMfgCode();
256
257 // Initialize the device clearing all rx/tx filterx
258 SendMgmtMsg(NGT_STARTUP_SEQ, sizeof(NGT_STARTUP_SEQ), 0x11, 0, NULL);
259
260#if 0
261 // Testing TX of Heartbeat
262 wxSleep(1);
263
264 tN2kMsg N2kMsg; // automatically sets destination 255
265 //SetHeartbeat(N2kMsg,2000,0);
266 //SetN2kPGN126993(N2kMsg, 2000, 0);
267 N2kMsg.SetPGN(126993L);
268 //N2kMsg.Priority=7;
269 N2kMsg.Source = 2;
270 N2kMsg.Destination = 133;
271 N2kMsg.Add2ByteUInt((uint16_t)(2000)); // Rate, msec
272
273 N2kMsg.AddByte(0); //Status
274 N2kMsg.AddByte(0xff); // Reserved
275 N2kMsg.Add4ByteUInt(0xffffffff); // Reserved
276
277 const std::vector<unsigned char> mv = BufferToActisenseFormat(N2kMsg);
278
279 size_t len = mv.size();
280
281 wxString comx = m_params.GetDSPort().AfterFirst(':');
282 std::string interface = comx.ToStdString();
283
284 N2kName source_name(1234);
285 auto source_address = std::make_shared<NavAddr2000>(interface, source_name);
286 auto dest_address = std::make_shared<NavAddr2000>(interface, N2kMsg.Destination);
287
288 auto message_to_send = std::make_shared<Nmea2000Msg>(126993L,
289 mv, source_address, 3);
290
291 for(size_t i=0; i< mv.size(); i++){
292 printf("%02X ", mv.at(i));
293 }
294 printf("\n\n");
295
296 SetTXPGN(126993);
297 wxSleep(1);
298
299 SendMessage(message_to_send, dest_address);
300
301 int yyp = 4;
302#endif
303}
304
305CommDriverN2KSerial::~CommDriverN2KSerial() { Close(); }
306
307DriverStats CommDriverN2KSerial::GetDriverStats() const {
308 if (m_closing) return m_driver_stats;
309
310#ifndef ANDROID
311 if (m_pSecondary_Thread)
312 return m_pSecondary_Thread->GetStats();
313 else
314#endif
315 return m_driver_stats;
316}
317
318bool CommDriverN2KSerial::Open() {
319 wxString comx;
320 comx = m_params.GetDSPort().AfterFirst(':'); // strip "Serial:"
321
322 comx =
323 comx.BeforeFirst(' '); // strip off any description provided by Windows
324
325#ifndef ANDROID
326 // Kick off the RX thread
327 SetSecondaryThread(new CommDriverN2KSerialThread(this, comx, m_BaudRate));
328 SetThreadRunFlag(1);
329 GetSecondaryThread()->Run();
330#endif
331
332 return true;
333}
334
335void CommDriverN2KSerial::Close() {
336 wxLogMessage(
337 wxString::Format(_T("Closing N2K Driver %s"), m_portstring.c_str()));
338
339 m_stats_timer.Stop();
340 m_closing = true;
341
342 // Kill off the Secondary RX Thread if alive
343 if (m_pSecondary_Thread) {
344 if (m_bsec_thread_active) // Try to be sure thread object is still alive
345 {
346 wxLogMessage(_T("Stopping Secondary Thread"));
347
348 m_Thread_run_flag = 0;
349 int tsec = 10;
350 while ((m_Thread_run_flag >= 0) && (tsec--)) wxSleep(1);
351
352 wxString msg;
353 if (m_Thread_run_flag < 0)
354 msg.Printf(_T("Stopped in %d sec."), 10 - tsec);
355 else
356 msg.Printf(_T("Not Stopped after 10 sec."));
357 wxLogMessage(msg);
358 }
359
360 m_pSecondary_Thread = NULL;
361 m_bsec_thread_active = false;
362 }
363}
364static uint64_t PayloadToName(const std::vector<unsigned char> payload) {
365 uint64_t name;
366 memcpy(&name, reinterpret_cast<const void*>(payload.data()), sizeof(name));
367 return name;
368}
369
370bool CommDriverN2KSerial::SendMessage(std::shared_ptr<const NavMsg> msg,
371 std::shared_ptr<const NavAddr> addr) {
372 if (m_closing) return false;
373
374#ifndef ANDROID
375
376 auto msg_n2k = std::dynamic_pointer_cast<const Nmea2000Msg>(msg);
377 std::vector<uint8_t> load = msg_n2k->payload;
378
379 uint64_t _pgn = msg_n2k->PGN.pgn;
380 auto destination_address = std::static_pointer_cast<const NavAddr2000>(addr);
381
382 tN2kMsg N2kMsg; // automatically sets destination 255
383 N2kMsg.SetPGN(_pgn);
384 N2kMsg.Priority = msg_n2k->priority;
385 if (destination_address) N2kMsg.Destination = destination_address->address;
386
387 for (size_t i = 0; i < load.size(); i++) N2kMsg.AddByte(load.at(i)); // data
388
389 const std::vector<uint8_t> acti_pkg = BufferToActisenseFormat(N2kMsg);
390
391 // Create the internal message for all N2K listeners
392 std::vector<unsigned char> msg_payload;
393 for (size_t i = 2; i < acti_pkg.size() - 2; i++)
394 msg_payload.push_back(acti_pkg[i]);
395 auto name = PayloadToName(load);
396 auto msg_all =
397 std::make_shared<const Nmea2000Msg>(1, msg_payload, GetAddress(name));
398 auto msg_internal =
399 std::make_shared<const Nmea2000Msg>(_pgn, msg_payload, GetAddress(name));
400
401 // Notify listeners
402 m_listener.Notify(std::move(msg_internal));
403 m_listener.Notify(std::move(msg_all));
404
405 if (GetSecondaryThread()) {
406 if (IsSecThreadActive()) {
407 int retry = 10;
408 while (retry) {
409 if (GetSecondaryThread()->SetOutMsg(acti_pkg))
410 return true;
411 else
412 retry--;
413 }
414 return false; // could not send after several tries....
415 } else
416 return false;
417 }
418#endif
419 return true;
420}
421
422void CommDriverN2KSerial::ProcessManagementPacket(
423 std::vector<unsigned char>* payload) {
424 if (payload->at(2) != 0xF2) { // hearbeat
425 // printf(" pl ");
426 // for (unsigned int i = 0; i < payload->size(); i++)
427 // printf("%02X ", payload->at(i));
428 // printf("\n");
429 }
430
431 switch (payload->at(2)) {
432 case 0x47:
433 m_bmg47_resp = true;
434 break;
435 case 0x01:
436 m_bmg01_resp = true;
437 break;
438 case 0x4B:
439 m_bmg4B_resp = true;
440 break;
441 case 0x041: {
442 m_bmg41_resp = true;
443 if (payload->at(3) == 0x02) { // ASCII device_common_name
444 std::string device_common_name;
445 for (unsigned int i = 0; i < 32; i++) {
446 device_common_name += payload->at(i + 14);
447 }
448 device_common_name += '\0';
449 m_device_common_name = device_common_name;
450 }
451 break;
452 }
453 case 0x042: {
454 m_bmg42_resp = true;
455 unsigned char name[8];
456 for (unsigned int i = 0; i < 8; i++) name[i] = payload->at(i + 15);
457
458 memcpy((void*)&NAME, name, 8);
459 // Extract the manufacturers code
460 int* f1 = (int*)&NAME;
461 int f1d = *f1;
462 m_manufacturers_code = f1d >> 21;
463 break;
464 }
465
466 default:
467 break;
468 }
469}
470
471void CommDriverN2KSerial::handle_N2K_SERIAL_RAW(
473 auto p = event.GetPayload();
474
475 std::vector<unsigned char>* payload = p.get();
476
477 if (payload->at(0) == 0xA0) {
478 ProcessManagementPacket(payload);
479 return;
480 }
481
482 // If port INPUT is not set, filter the mesage here
483 if (m_params.IOSelect != DS_TYPE_OUTPUT) {
484 // extract PGN
485 uint64_t pgn = 0;
486 unsigned char* c = (unsigned char*)&pgn;
487 *c++ = payload->at(3);
488 *c++ = payload->at(4);
489 *c++ = payload->at(5);
490
491 auto name = PayloadToName(*payload);
492 auto msg =
493 std::make_shared<const Nmea2000Msg>(pgn, *payload, GetAddress(name));
494 auto msg_all =
495 std::make_shared<const Nmea2000Msg>(1, *payload, GetAddress(name));
496
497 m_listener.Notify(std::move(msg));
498 m_listener.Notify(std::move(msg_all));
499 }
500}
501
502int CommDriverN2KSerial::GetMfgCode() {
503 unsigned char request_name[] = {0x42};
504 int ni = SendMgmtMsg(request_name, sizeof(request_name), 0x41, 2000,
505 &m_bmg42_resp);
506 if (ni) return ni; // Not responding, return error so upper retries.
507 m_got_mfg_code = true;
508 return 0;
509}
510
511int CommDriverN2KSerial::SendMgmtMsg(unsigned char* string, size_t string_size,
512 unsigned char cmd_code, int timeout_msec,
513 bool* response_flag) {
514#ifndef ANDROID
515 // Make a message
516 int byteSum = 0;
517 uint8_t CheckSum;
518 std::vector<unsigned char> msg;
519
520 msg.push_back(ESCAPE);
521 msg.push_back(STARTOFTEXT);
522 msg.push_back(0xA1);
523 byteSum += 0xA1;
524 msg.push_back(string_size); // payload length
525 byteSum += string_size;
526
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];
531 }
532
533 // checksum
534 byteSum %= 256;
535 CheckSum = (uint8_t)((byteSum == 0) ? 0 : (256 - byteSum));
536 msg.push_back(CheckSum);
537
538 msg.push_back(ESCAPE);
539 msg.push_back(ENDOFTEXT);
540
541 // send it out
542
543 if (response_flag) *response_flag = false; // prime the response detector
544
545 // Send the msg
546 bool bsent = false;
547 bool not_done = true;
548 int ntry_outer = 10;
549 while (not_done) {
550 if (GetSecondaryThread() && IsSecThreadActive()) {
551 int retry = 10;
552 while (retry) {
553 if (GetSecondaryThread()->SetOutMsg(msg)) {
554 bsent = true;
555 not_done = false;
556 break;
557 } else
558 retry--;
559 }
560 } else {
561 wxMilliSleep(100);
562 if (ntry_outer-- <= 0) not_done = false;
563 }
564 }
565
566 if (!bsent) return 1;
567
568 bool bOK = false;
569 if (timeout_msec) {
570 int timeout = timeout_msec;
571 while (timeout > 0) {
572 wxYieldIfNeeded();
573 wxMilliSleep(100);
574 if (response_flag) {
575 if (*response_flag) {
576 bOK = true;
577 break;
578 }
579 }
580 timeout -= 100;
581 }
582 } else
583 bOK = true;
584
585 if (!bOK) {
586 // printf( "***Err-1\n");
587 return 1;
588 }
589 // else
590 // printf("***OK-1 %d\n", timeout);
591#endif
592 return 0;
593}
594
595int CommDriverN2KSerial::SetTXPGN(int pgn) {
596 // Enable PGN message
597 unsigned char request_enable[] = {0x47, 0x00, 0x00, 0x00, // pgn
598 0x00, 0x01, 0xFF, 0xFF, 0xFF, 0xFF};
599
600 int PGN = 0;
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];
605
606 int aa = SendMgmtMsg(request_enable, sizeof(request_enable), 0x47, 2000,
607 &m_bmg47_resp);
608 if (aa) return 0; // Probably YDNU-02 device
609
610 // Commit message
611 unsigned char request_commit[] = {0x01};
612 int bb = SendMgmtMsg(request_commit, sizeof(request_commit), 0x01, 2000,
613 &m_bmg01_resp);
614
615 // Activate message
616 unsigned char request_activate[] = {0x4B};
617 int cc = SendMgmtMsg(request_activate, sizeof(request_activate), 0x4B, 2000,
618 &m_bmg4B_resp);
619
620 return 0;
621}
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())
625 return;
626 else {
627 SetTXPGN(pgn);
628 pgn_tx_list.push_back(pgn);
629 }
630}
631
632#ifndef __ANDROID__
633
640// Commonly used raw format is actually inherited from an old paketizing format:
641// <10><02><application data><CRC (1)><10><03>
642
643// Actisense application data, from NGT-1 to PC
644// <data code=93><length (1)><priority (1)><PGN (3)><destination(1)><source
645// (1)><time (4)><len (1)><data (len)>
646
647// As applied to a real application data element, after extraction from packet
648// format: 93 13 02 01 F8 01 FF 01 76 C2 52 00 08 08 70 EB 14 E8 8E 52 D2 BB 10
649
650// length (1): 0x13
651// priority (1); 0x02
652// PGN (3): 0x01 0xF8 0x01
653// destination(1): 0xFF
654// source (1): 0x01
655// time (4): 0x76 0xC2 0x52 0x00
656// len (1): 0x08
657// data (len): 08 70 EB 14 E8 8E 52 D2
658// packet CRC: 0xBB
659
660#define DS_RX_BUFFER_SIZE 4096
661
662CommDriverN2KSerialThread::CommDriverN2KSerialThread(
663 CommDriverN2KSerial* Launcher, const wxString& PortName,
664 const wxString& strBaudRate) {
665 m_pParentDriver = Launcher; // This thread's immediate "parent"
666
667 m_PortName = PortName;
668 m_FullPortName = _T("Serial:") + PortName;
669
670 rx_buffer = new unsigned char[DS_RX_BUFFER_SIZE + 1];
671
672 put_ptr = rx_buffer; // local circular queue
673 tak_ptr = rx_buffer;
674
675 m_baud = 9600; // default
676 long lbaud;
677 if (strBaudRate.ToLong(&lbaud)) m_baud = (int)lbaud;
678 {
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;
683 }
684
685 Create();
686}
687
688CommDriverN2KSerialThread::~CommDriverN2KSerialThread(void) {
689 delete[] rx_buffer;
690}
691
692void CommDriverN2KSerialThread::OnExit(void) {}
693
694DriverStats CommDriverN2KSerialThread::GetStats() const {
695 std::lock_guard lock(m_stats_mutex);
696 return m_driver_stats;
697}
698
699bool CommDriverN2KSerialThread::OpenComPortPhysical(const wxString& com_name,
700 int baud_rate) {
701 try {
702 m_serial.setPort(com_name.ToStdString());
703 m_serial.setBaudrate(baud_rate);
704 m_serial.open();
705 m_serial.setTimeout(250, 250, 0, 250, 0);
706 } catch (std::exception&) {
707 // std::cerr << "Unhandled Exception while opening serial port: " <<
708 // e.what() << std::endl;
709 }
710 return m_serial.isOpen();
711}
712
713void CommDriverN2KSerialThread::CloseComPortPhysical() {
714 try {
715 m_serial.close();
716 } catch (std::exception&) {
717 // std::cerr << "Unhandled Exception while closing serial port: " <<
718 // e.what() << std::endl;
719 }
720 std::lock_guard lock(m_stats_mutex);
721 m_driver_stats.available = false;
722}
723
724void CommDriverN2KSerialThread::SetGatewayOperationMode(void) {
725 // For YDNU-02 device
726 // From Device User Manual
727 // Set the mode to "N2K"
728 unsigned char config_string[] = {0x10, 0x02, 0xA1, 0x03, 0x11,
729 0x02, 0x00, 0x49, 0x10, 0x03};
730 // std::vector<byte>writeBuffer {DLE,STX,NGT_TX_CMD,0x03,0x11,0x02,0x00,0x49,
731 // DLE,ETX};
732
733 WriteComPortPhysical(config_string, 10);
734}
735
736void CommDriverN2KSerialThread::ThreadMessage(const wxString& msg) {
737 // Signal the main program thread
738 // OCPN_ThreadMessageEvent event(wxEVT_OCPN_THREADMSG, 0);
739 // event.SetSString(std::string(msg.mb_str()));
740 // if (gFrame) gFrame->GetEventHandler()->AddPendingEvent(event);
741}
742
743size_t CommDriverN2KSerialThread::WriteComPortPhysical(
744 std::vector<unsigned char> msg) {
745 return WriteComPortPhysical(msg.data(), msg.size());
746}
747
748size_t CommDriverN2KSerialThread::WriteComPortPhysical(unsigned char* msg,
749 size_t length) {
750 if (!m_serial.isOpen()) return 0;
751 try {
752 size_t status = m_serial.write((uint8_t*)msg, length);
753 if (status) m_serial.flushOutput();
754 return status;
755 } catch (std::exception& e) {
756 DEBUG_LOG << "Unhandled Exception while writing to serial port: "
757 << e.what();
758 }
759 return 0;
760}
761
762bool CommDriverN2KSerialThread::SetOutMsg(
763 const std::vector<unsigned char>& msg) {
764 if (out_que.size() < OUT_QUEUE_LENGTH) {
765 out_que.push(msg);
766 return true;
767 }
768 return false;
769}
770
771#ifndef __WXMSW__
772void* CommDriverN2KSerialThread::Entry() {
773 bool not_done = true;
774 bool nl_found = false;
775 wxString msg;
776 uint8_t rdata[2000];
777 circular_buffer<uint8_t> circle(DS_RX_BUFFER_SIZE);
778 int ib = 0;
779
780 // Request the com port from the comm manager
781 if (!OpenComPortPhysical(m_PortName, m_baud)) {
782 wxString msg(_T("NMEA input device open failed: "));
783 msg.Append(m_PortName);
784 ThreadMessage(msg);
785 std::lock_guard lock(m_stats_mutex);
786 m_driver_stats.available = false;
787
788 // goto thread_exit; // This means we will not be trying to connect = The
789 // device must be connected when the thread is created. Does not seem to be
790 // needed/what we want as the reconnection logic is able to pick it up
791 // whenever it actually appears (Of course given it appears with the
792 // expected device name).
793 } else {
794 wxMilliSleep(100);
795 std::lock_guard lock(m_stats_mutex);
796 m_driver_stats.available = true;
797 SetGatewayOperationMode();
798 }
799
800 m_pParentDriver->SetSecThreadActive(); // I am alive
801
802 // The main loop
803 static size_t retries = 0;
804
805 bool bInMsg = false;
806 bool bGotESC = false;
807 bool bGotSOT = false;
808
809 while ((not_done) && (m_pParentDriver->m_Thread_run_flag > 0)) {
810 if (TestDestroy()) not_done = false; // smooth exit
811
812 uint8_t next_byte = 0;
813 int newdata = 0;
814 if (m_serial.isOpen()) {
815 try {
816 newdata = m_serial.read(rdata, 1000);
817 } catch (std::exception& e) {
818 // std::cerr << "Serial read exception: " << e.what() << std::endl;
819 std::lock_guard lock(m_stats_mutex);
820 m_driver_stats.error_count++;
821
822 if (10 < retries++) {
823 // We timed out waiting for the next character 10 times, let's close
824 // the port so that the reconnection logic kicks in and tries to fix
825 // our connection.
826 CloseComPortPhysical();
827 retries = 0;
828 }
829 }
830 } else {
831 // Reconnection logic. Let's try to reopen the port while waiting longer
832 // every time (until we simply keep trying every 2.5 seconds)
833 // std::cerr << "Serial port seems closed." << std::endl;
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;
840 retries = 0;
841 } else if (retries < 10) {
842 std::lock_guard lock(m_stats_mutex);
843 m_driver_stats.available = false;
844 retries++;
845 }
846 }
847
848 if (newdata > 0) {
849 std::lock_guard lock(m_stats_mutex);
850 m_driver_stats.rx_count += newdata;
851
852 for (int i = 0; i < newdata; i++) {
853 circle.put(rdata[i]);
854 }
855 }
856
857 while (!circle.empty()) {
858 if (ib >= DS_RX_BUFFER_SIZE) ib = 0;
859 uint8_t next_byte = circle.get();
860
861 if (bInMsg) {
862 if (bGotESC) {
863 if (ESCAPE == next_byte) {
864 rx_buffer[ib++] = next_byte;
865 bGotESC = false;
866 }
867 }
868
869 if (bGotESC && (ENDOFTEXT == next_byte)) {
870 // Process packet
871 // Copy the message into a std::vector
872
873 auto buffer = std::make_shared<std::vector<unsigned char>>(
874 rx_buffer, rx_buffer + ib);
875 std::vector<unsigned char>* vec = buffer.get();
876
877 ib = 0;
878 // tak_ptr = tptr;
879 bInMsg = false;
880 bGotESC = false;
881
882 // printf("raw ");
883 // for (unsigned int i = 0; i < vec->size(); i++)
884 // printf("%02X ", vec->at(i));
885 // printf("\n");
886
887 // Message is finished
888 // Send the captured raw data vector pointer to the thread's "parent"
889 // thereby releasing the thread for further data capture
890 CommDriverN2KSerialEvent Nevent(wxEVT_COMMDRIVER_N2K_SERIAL, 0);
891 Nevent.SetPayload(buffer);
892 m_pParentDriver->AddPendingEvent(Nevent);
893
894 } else {
895 bGotESC = (next_byte == ESCAPE);
896
897 if (!bGotESC) {
898 rx_buffer[ib++] = next_byte;
899 }
900 }
901 }
902
903 else {
904 if (STARTOFTEXT == next_byte) {
905 bGotSOT = false;
906 if (bGotESC) {
907 bGotSOT = true;
908 }
909 } else {
910 bGotESC = (next_byte == ESCAPE);
911 if (bGotSOT) {
912 bGotSOT = false;
913 bInMsg = true;
914
915 rx_buffer[ib++] = next_byte;
916 }
917 }
918 }
919 } // if newdata > 0
920
921 // Check for any pending output message
922#if 1
923 bool b_qdata = !out_que.empty();
924
925 while (b_qdata) {
926 // Take a copy of message
927 std::vector<unsigned char> qmsg = out_que.front();
928 out_que.pop();
929
930 if (static_cast<size_t>(-1) == WriteComPortPhysical(qmsg) &&
931 10 < retries++) {
932 // We failed to write the port 10 times, let's close the port so that
933 // the reconnection logic kicks in and tries to fix our connection.
934 retries = 0;
935 CloseComPortPhysical();
936 }
937
938 b_qdata = !out_que.empty();
939 } // while b_qdata
940
941#endif
942 } // while ((not_done)
943
944 // thread_exit:
945 CloseComPortPhysical();
946 m_pParentDriver->SetSecThreadInActive(); // I am dead
947 m_pParentDriver->m_Thread_run_flag = -1;
948
949 std::lock_guard lock(m_stats_mutex);
950 m_driver_stats.available = false;
951
952 return 0;
953}
954
955#else
956void* CommDriverN2KSerialThread::Entry() {
957 bool not_done = true;
958 bool nl_found = false;
959 wxString msg;
960 circular_buffer<uint8_t> circle(DS_RX_BUFFER_SIZE);
961
962 // Request the com port from the comm manager
963 if (!OpenComPortPhysical(m_PortName, m_baud)) {
964 wxString msg(_T("NMEA input device open failed: "));
965 msg.Append(m_PortName);
966 ThreadMessage(msg);
967 std::lock_guard lock(m_stats_mutex);
968 m_driver_stats.available = false;
969
970 // goto thread_exit; // This means we will not be trying to connect = The
971 // device must be connected when the thread is created. Does not seem to be
972 // needed/what we want as the reconnection logic is able to pick it up
973 // whenever it actually appears (Of course given it appears with the
974 // expected device name).
975 } else {
976 SetGatewayOperationMode();
977 std::lock_guard lock(m_stats_mutex);
978 m_driver_stats.available = true;
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 std::lock_guard lock(m_stats_mutex);
1018 m_driver_stats.available = true;
1019 SetGatewayOperationMode();
1020 retries = 0;
1021 } else if (retries < 10)
1022 retries++;
1023 }
1024
1025 if (newdata > 0) {
1026 for (int i = 0; i < newdata; i++) {
1027 circle.put(rdata[i]);
1028 }
1029 }
1030
1031 while (!circle.empty()) {
1032 uint8_t next_byte = circle.get();
1033
1034 if (1) {
1035 if (bInMsg) {
1036 if (bGotESC) {
1037 if (ESCAPE == next_byte) {
1038 *put_ptr++ = next_byte;
1039 if ((put_ptr - rx_buffer) > DS_RX_BUFFER_SIZE)
1040 put_ptr = rx_buffer;
1041 bGotESC = false;
1042 } else if (ENDOFTEXT == next_byte) {
1043 // Process packet
1044 // Copy the message into a std::vector
1045
1046 auto buffer = std::make_shared<std::vector<unsigned char>>();
1047 std::vector<unsigned char>* vec = buffer.get();
1048
1049 unsigned char* tptr;
1050 tptr = tak_ptr;
1051
1052 while ((tptr != put_ptr)) {
1053 vec->push_back(*tptr++);
1054 if ((tptr - rx_buffer) > DS_RX_BUFFER_SIZE) tptr = rx_buffer;
1055 }
1056
1057 tak_ptr = tptr;
1058 bInMsg = false;
1059 bGotESC = false;
1060
1061 // Message is finished
1062 // Send the captured raw data vector pointer to the thread's
1063 // "parent"
1064 // thereby releasing the thread for further data capture
1065 CommDriverN2KSerialEvent Nevent(wxEVT_COMMDRIVER_N2K_SERIAL, 0);
1066 Nevent.SetPayload(buffer);
1067 m_pParentDriver->AddPendingEvent(Nevent);
1068 std::lock_guard lock(m_stats_mutex);
1069 m_driver_stats.rx_count += vec->size();
1070 } else if (next_byte == STARTOFTEXT) {
1071 put_ptr = rx_buffer;
1072 bGotESC = false;
1073 } else {
1074 put_ptr = rx_buffer;
1075 bInMsg = false;
1076 bGotESC = false;
1077 }
1078
1079 } else {
1080 bGotESC = (next_byte == ESCAPE);
1081
1082 if (!bGotESC) {
1083 *put_ptr++ = next_byte;
1084 if ((put_ptr - rx_buffer) > DS_RX_BUFFER_SIZE)
1085 put_ptr = rx_buffer;
1086 }
1087 }
1088 }
1089
1090 else {
1091 if (STARTOFTEXT == next_byte) {
1092 bGotSOT = false;
1093 if (bGotESC) {
1094 bGotSOT = true;
1095 }
1096 } else {
1097 bGotESC = (next_byte == ESCAPE);
1098 if (bGotSOT) {
1099 bGotSOT = false;
1100 bInMsg = true;
1101
1102 *put_ptr++ = next_byte;
1103 if ((put_ptr - rx_buffer) > DS_RX_BUFFER_SIZE)
1104 put_ptr = rx_buffer;
1105 }
1106 }
1107 }
1108 } // if newdata > 0
1109 } // while
1110
1111 // Check for any pending output message
1112 bool b_qdata = !out_que.empty();
1113
1114 while (b_qdata) {
1115 // Take a copy of message
1116 std::vector<unsigned char> qmsg = out_que.front();
1117 out_que.pop();
1118
1119 if (static_cast<size_t>(-1) == WriteComPortPhysical(qmsg) &&
1120 10 < retries++) {
1121 // We failed to write the port 10 times, let's close the port so that
1122 // the reconnection logic kicks in and tries to fix our connection.
1123 retries = 0;
1124 CloseComPortPhysical();
1125 }
1126 std::lock_guard lock(m_stats_mutex);
1127 m_driver_stats.tx_count += qmsg.size();
1128
1129 b_qdata = !out_que.empty();
1130 } // while b_qdata
1131 } // while ((not_done)
1132
1133 // thread_exit:
1134 CloseComPortPhysical();
1135 m_pParentDriver->SetSecThreadInActive(); // I am dead
1136 m_pParentDriver->m_Thread_run_flag = -1;
1137
1138 return 0;
1139}
1140
1141#endif // wxmsw Entry()
1142
1143#endif // Android
1144
1145//*****************************************************************************
1146// Actisense Format:
1147// <10><02><93><length (1)><priority (1)><PGN (3)><destination (1)><source
1148// (1)><time (4)><len (1)><data (len)><CRC (1)><10><03>
1149#define MaxActisenseMsgBuf 400
1150#define MsgTypeN2kTX 0x94
1151
1152void AddByteEscapedToBuf(unsigned char byteToAdd, uint8_t& idx,
1153 unsigned char* buf, int& byteSum);
1154
1155std::vector<unsigned char> BufferToActisenseFormat(tN2kMsg& msg) {
1156 unsigned long _PGN = msg.PGN;
1157 uint8_t msgIdx = 0;
1158 int byteSum = 0;
1159 uint8_t CheckSum;
1160 unsigned char ActisenseMsgBuf[MaxActisenseMsgBuf];
1161
1162 ActisenseMsgBuf[msgIdx++] = ESCAPE;
1163 ActisenseMsgBuf[msgIdx++] = STARTOFTEXT;
1164 AddByteEscapedToBuf(MsgTypeN2kTX, msgIdx, ActisenseMsgBuf, byteSum);
1165 AddByteEscapedToBuf(msg.DataLen + 6, msgIdx, ActisenseMsgBuf,
1166 byteSum); // length does not include escaped chars
1167
1168 AddByteEscapedToBuf(msg.Priority, msgIdx, ActisenseMsgBuf, byteSum);
1169 AddByteEscapedToBuf(_PGN & 0xff, msgIdx, ActisenseMsgBuf, byteSum);
1170 _PGN >>= 8;
1171 AddByteEscapedToBuf(_PGN & 0xff, msgIdx, ActisenseMsgBuf, byteSum);
1172 _PGN >>= 8;
1173 AddByteEscapedToBuf(_PGN & 0xff, msgIdx, ActisenseMsgBuf, byteSum);
1174 AddByteEscapedToBuf(msg.Destination, msgIdx, ActisenseMsgBuf, byteSum);
1175
1176#if 0
1177 // For TX through Actisense compatible gateway, we skip "source" byte and msg time fields
1178 // Source
1179 AddByteEscapedToBuf(msg.Source,msgIdx,ActisenseMsgBuf,byteSum);
1180 // Time
1181 int _MsgTime = 0;
1182 AddByteEscapedToBuf(_MsgTime & 0xff,msgIdx,ActisenseMsgBuf,byteSum); _MsgTime>>=8;
1183 AddByteEscapedToBuf(_MsgTime & 0xff,msgIdx,ActisenseMsgBuf,byteSum); _MsgTime>>=8;
1184 AddByteEscapedToBuf(_MsgTime & 0xff,msgIdx,ActisenseMsgBuf,byteSum); _MsgTime>>=8;
1185 AddByteEscapedToBuf(_MsgTime & 0xff,msgIdx,ActisenseMsgBuf,byteSum);
1186
1187#endif
1188
1189 AddByteEscapedToBuf(msg.DataLen, msgIdx, ActisenseMsgBuf, byteSum);
1190
1191 for (int i = 0; i < msg.DataLen; i++)
1192 AddByteEscapedToBuf(msg.Data[i], msgIdx, ActisenseMsgBuf, byteSum);
1193 byteSum %= 256;
1194
1195 CheckSum = (uint8_t)((byteSum == 0) ? 0 : (256 - byteSum));
1196 ActisenseMsgBuf[msgIdx++] = CheckSum;
1197 if (CheckSum == ESCAPE) ActisenseMsgBuf[msgIdx++] = CheckSum;
1198
1199 ActisenseMsgBuf[msgIdx++] = ESCAPE;
1200 ActisenseMsgBuf[msgIdx++] = ENDOFTEXT;
1201
1202 std::vector<unsigned char> rv;
1203 for (unsigned int i = 0; i < msgIdx; i++) rv.push_back(ActisenseMsgBuf[i]);
1204
1205 return rv;
1206}
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 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.
N2k uses CAN which defines the basic properties of messages.
Definition comm_navmsg.h:70