myahrs_plus.hpp
Go to the documentation of this file.
1 //*****************************************************************************
2 //
3 // Copyright (c) 2014 Withrobot, Inc. All rights reserved.
4 //
5 // Software License Agreement
6 //
7 // Withrobot, Inc.(Withrobot) is supplying this software for use solely and
8 // exclusively on Withrobot's products.
9 //
10 // The software is owned by Withrobot and/or its suppliers, and is protected
11 // under applicable copyright laws. All rights are reserved.
12 // Any use in violation of the foregoing restrictions may subject the user
13 // to criminal sanctions under applicable laws, as well as to civil liability
14 // for the breach of the terms and conditions of this license.
15 //
16 // THIS SOFTWARE IS PROVIDED "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED
17 // OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
18 // MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
19 // WITHROBOT SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL,
20 // OR CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
21 //
22 //*****************************************************************************
23 
24 /*
25  * myahrs_plus.hpp
26  * - myAHRS+ SDK
27  *
28  * 2014.07.05 ('c')void
29  * 2014.07.15 ('c')void
30  * 2014.07.28 ('c')void
31  * 2014.08.02 ('c')void
32  * ver. 1.0
33  * 2014.08.14 ('c')void
34  * version string 추가
35  * 2014.08.15 ('c')void
36  * 통신 이외의 모든 실수형은 double
37  * 2014.08.27 ('c')void
38  * 2014.09.09 ('c')void
39  *
40  */
41 
42 /*
43  * DO NOT EDIT THIS FILE
44  */
45 
46 #ifndef __MYAHRS_PLUS_H_
47 #define __MYAHRS_PLUS_H_
48 
49 #include <stdio.h>
50 #include <stdlib.h>
51 #include <string.h>
52 #include <stdint.h>
53 
54 #define _USE_MATH_DEFINES
55 #include <math.h>
56 
57 #include <exception>
58 #include <string>
59 #include <vector>
60 #include <deque>
61 #include <map>
62 #include <iostream>
63 #include <sstream>
64 #include <algorithm>
65 
66 #ifdef WIN32
67  #define _AFXDLL
68  #include <afx.h>
69  #include <windows.h>
70  #include <cctype>
71 
72  #pragma warning( disable : 4996 ) // sprintf
73  #pragma warning( disable : 4355 ) // warning C4355: 'this' : used in base member initializer list
74  #pragma warning( disable : 4244 ) // warning C4244: '=' : conversion from 'double' to 'float', possible loss of data
75 
76  #define DBG_PRINTF(x, ...) {if(x) { printf(__VA_ARGS__);}}
77 
78 #else // LINUX
79  #include <unistd.h>
80  #include <pthread.h>
81  #include <semaphore.h>
82  #include <fcntl.h>
83  #include <termios.h>
84  #include <sys/time.h>
85 
86  #pragma GCC diagnostic ignored "-Wformat"
87  #define DBG_PRINTF(x, args...) {if(x) { printf(args);}}
88 #endif // WIN32
89 
90 
91 /*
92  * Version
93  */
94 #define WITHROBOT_MYAHRS_PLUS_SDK_VERSION "myahrs+ sdk. ver. 1.01"
95 
96 /*
97  * debugging sw
98  */
99 #define DEBUG_PLATFORM false
100 #define DEBUG_ASCII_PROTOCOL false
101 #define DEBUG_BINARY_PROTOCOL false
102 #define DEBUG_MYAHRS_PLUS false
103 
104 
105 namespace WithRobot {
106 
108  {
109  public:
110  std::string err;
111  myAhrsException(std::string e=""): err(e){}
112  virtual ~myAhrsException() {}
113 
114  const char* what() const throw() {
115  return err.c_str();
116  }
117  };
118 
119 /**********************************************************************************************************
120  *
121  * Platform abstraction
122  *
123  **********************************************************************************************************/
124 
125 #ifdef WIN32
126  class Platform
127  {
128  public:
129  class SerialPort
130  {
131  std::string port_name;
132  unsigned int baudrate;
133 
134  HANDLE m_hIDComDev;
135  BOOL m_bOpened;
136 
137  public:
138  SerialPort(const char* port="COM3", unsigned int brate=115200)
139  : port_name(port), baudrate(brate)
140  , m_hIDComDev(NULL), m_bOpened(FALSE)
141  {
142  }
143 
144  ~SerialPort() {
145  Close();
146  }
147 
148  bool Open(const char* port, int brate) {
149  port_name = port;
150  baudrate = brate;
151  return Open();
152  }
153 
154  bool Open() {
155  char szPort[32];
156  sprintf(szPort, "\\\\.\\%s", port_name.c_str());
157  CString port_str = CString::CStringT(CA2CT(szPort));
158 
159  DBG_PRINTF(DEBUG_PLATFORM, "portname : %s, baudrate %d\n", szPort, baudrate);
160 
161  m_hIDComDev = CreateFile( (LPCTSTR)port_str,
162  GENERIC_READ | GENERIC_WRITE,
163  0,
164  NULL,
165  OPEN_EXISTING,
166  FILE_ATTRIBUTE_NORMAL,
167  NULL );
168 
169  if( m_hIDComDev == NULL ) {
170  DBG_PRINTF(DEBUG_PLATFORM, "ERROR : m_hIDComDev == NULL\n");
171  return false;
172  }
173 
174  DCB dcb = {0};
175 
176  if (!GetCommState(m_hIDComDev, &dcb)) {
177  //error getting state
178  CloseHandle( m_hIDComDev );
179  printf("ERROR : GetCommState()\n");
180  return false;
181  }
182 
183  dcb.DCBlength = sizeof( DCB );
184  GetCommState( m_hIDComDev, &dcb );
185  dcb.BaudRate = baudrate;
186  dcb.ByteSize = 8;
187 
188  if(!SetCommState(m_hIDComDev, &dcb)){
189  //error setting serial port state
190  CloseHandle( m_hIDComDev );
191  DBG_PRINTF(DEBUG_PLATFORM, "ERROR : SetCommState()\n");
192  return false;
193  }
194 
195  COMMTIMEOUTS CommTimeOuts;
196 
197  CommTimeOuts.ReadIntervalTimeout = 1;
198  CommTimeOuts.ReadTotalTimeoutMultiplier = 1;
199  CommTimeOuts.ReadTotalTimeoutConstant = 1;
200  CommTimeOuts.WriteTotalTimeoutMultiplier = 1;
201  CommTimeOuts.WriteTotalTimeoutConstant = 10;
202 
203  if(!SetCommTimeouts( m_hIDComDev, &CommTimeOuts)) {
204  CloseHandle( m_hIDComDev );
205  DBG_PRINTF(DEBUG_PLATFORM, "ERROR : SetCommTimeouts()\n");
206  return false;
207  }
208 
209  m_bOpened = TRUE;
210 
211  return (m_bOpened == TRUE);
212  }
213 
214  void Close() {
215  if( !m_bOpened || m_hIDComDev == NULL ) {
216  return;
217  }
218 
219  CloseHandle( m_hIDComDev );
220  m_bOpened = FALSE;
221  m_hIDComDev = NULL;
222  }
223 
224  int Read(unsigned char* buf, unsigned int buf_len) {
225  if( !m_bOpened || m_hIDComDev == NULL ) return -1;
226 
227  BOOL bReadStatus;
228  DWORD dwBytesRead, dwErrorFlags;
229  COMSTAT ComStat;
230 
231  ClearCommError( m_hIDComDev, &dwErrorFlags, &ComStat );
232 
233  if(ComStat.cbInQue <= 0) {
234  dwBytesRead = 1;
235  }
236  else {
237  dwBytesRead = (DWORD) ComStat.cbInQue;
238  }
239 
240  if( buf_len < (int) dwBytesRead ) dwBytesRead = (DWORD) buf_len;
241 
242  bReadStatus = ReadFile( m_hIDComDev, buf, dwBytesRead, &dwBytesRead, NULL );
243  if( !bReadStatus ){
244  //if( GetLastError() == ERROR_IO_PENDING ){
245  // WaitForSingleObject( m_OverlappedRead.hEvent, 2000 );
246  // return( (int) dwBytesRead );
247  //}
248  return 0;
249  }
250 
251  return( (int) dwBytesRead );
252  }
253 
254  int Write(unsigned char* data, unsigned int data_len) {
255  BOOL bWriteStat;
256  DWORD dwBytesWritten = 0;
257 
258  if(m_bOpened != TRUE) {
259  return -1;
260  }
261 
262  bWriteStat = WriteFile( m_hIDComDev, (LPSTR)data, data_len, &dwBytesWritten, NULL);
263  if(!bWriteStat) {
264  if (GetLastError() != ERROR_IO_PENDING) {
265  // WriteFile failed, but it isn't delayed. Report error and abort.
266  return dwBytesWritten;
267  }
268  else {
269  // Write is pending... 여기로 빠질리는 없지만서도...
270  //Sleep(10);
271  // retry;
272  return dwBytesWritten;
273  }
274  }
275  else {
276  return dwBytesWritten;
277  }
278  }
279 
280  int Flush() {
281  int len= 0, count = 0;
282  unsigned char buf[256];
283  while((len = Read(buf, sizeof(buf))) > 0) {
284  count += len;
285  }
286  return count;
287  }
288  };
289 
290 
291  class Mutex
292  {
293  CRITICAL_SECTION cs;
294 
295  public:
296  Mutex() {
297  InitializeCriticalSection(&cs);
298  }
299 
300  ~Mutex() {
301  DeleteCriticalSection(&cs);
302  }
303 
304  inline void lock() {
305  EnterCriticalSection(&cs);
306  }
307 
308  inline void unlock() {
309  LeaveCriticalSection(&cs);
310  }
311  };
312 
313  class Event
314  {
315  HANDLE event;
316 
317  public:
318  Event() {
319  event = CreateEvent(NULL,
320  TRUE, // bManualReset
321  FALSE, // bInitialState
322  _T("Event"));
323  }
324 
325  ~Event() {
326  CloseHandle(event);
327  }
328 
329  inline bool wait(int timeout_msec=-1) {
330  if(timeout_msec < 0) {
331  timeout_msec = INFINITE;
332  }
333  DWORD res = WaitForSingleObject(event, timeout_msec);
334  ResetEvent(event);
335  return (res == WAIT_OBJECT_0);
336  }
337 
338  inline bool set() {
339  return (SetEvent(event) == TRUE);
340  }
341  };
342 
343 
344  class Thread
345  {
346  DWORD thread_id;
347  HANDLE thread;
348 
349  public:
350  Thread() : thread(NULL), thread_id(0){}
351 
352  bool start(void*(*thread_proc)(void*), void* arg, size_t stack_size=16*1024) {
353  thread = CreateThread(NULL, (DWORD)stack_size, (LPTHREAD_START_ROUTINE)thread_proc, arg, 0, &thread_id);
354  return (thread != NULL);
355  }
356 
357  void join() {
358  if(thread != NULL) {
359  WaitForSingleObject(thread, INFINITE);
360  CloseHandle(thread);
361  }
362  thread = NULL;
363  thread_id = 0;
364  }
365  };
366 
367  static void msleep(unsigned int msec) {
368  Sleep(msec);
369  }
370  };
371 
372 #else
373  /*
374  * Unix-Like OS (Linux/OSX)
375  */
376  class Platform
377  {
378  public:
379 
381  {
382  std::string port_name; // ex) "/dev/tty.usbmodem14241"
383  int port_fd;
384  unsigned int baudrate;
385 
386  public:
387  SerialPort(const char* port = "", unsigned int brate=115200)
388  : port_name(port), baudrate(brate)
389  , port_fd(-1)
390  {
391  }
392 
394  Close();
395  }
396 
397  bool Open(const char* port, int brate) {
398  port_name = port;
399  baudrate = brate;
400  return Open();
401  }
402 
403  bool Open() {
404  int fd = 0;
405  struct termios options;
406 
407  if(port_fd > 0) {
408  return true;
409  }
410 
411  fd = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
412  if(fd < 0) {
413  return false;
414  }
415 
416  fcntl(fd, F_SETFL, 0); // clear all flags on descriptor, enable direct I/O
417  tcgetattr(fd, &options); // read serial port options
418 
419  cfsetspeed(&options, baudrate);
420  cfmakeraw(&options);
421 
422  options.c_cflag |= CREAD | CLOCAL; // turn on READ
423  options.c_cflag |= CS8;
424  options.c_cc[VMIN] = 0;
425  options.c_cc[VTIME] = 1;
426 
427  // set the new port options
428  tcsetattr(fd, TCSANOW, &options);
429 
430  // flush I/O buffers
431  tcflush(fd, TCIOFLUSH);
432 
433  port_fd = fd;
434 
435  return (fd > 0);
436 
437  }
438 
439  void Close() {
440  if(port_fd > 0) {
441  close(port_fd);
442  port_fd = -1;
443  }
444  }
445 
446  int Read(unsigned char* buf, unsigned int buf_len) {
447  if(port_fd < 0) {
448  return -1;
449  }
450 
451  int n = read(port_fd, buf, buf_len-1);
452  if(n > 0) {
453  buf[n] = 0;
454  }
455 
456  return n;
457  }
458 
459  int Write(unsigned char* data, unsigned int data_len) {
460  if(port_fd < 0) {
461  return -1;
462  }
463 
464  return write(port_fd, data, data_len);
465  }
466 
467  int Flush() {
468  if(port_fd < 0) {
469  return -1;
470  }
471 
472  // flush I/O buffers
473  return tcflush(port_fd, TCIOFLUSH);
474  }
475  };
476 
477 
478  class Mutex
479  {
480  pthread_mutex_t mutex;
481 
482  public:
483  Mutex() {
484  pthread_mutex_init(&mutex, NULL);
485  }
486 
487  ~Mutex() {
488  pthread_mutex_destroy(&mutex);
489  }
490 
491  inline void lock() {
492  pthread_mutex_lock(&mutex);
493  }
494 
495  inline void unlock() {
496  pthread_mutex_unlock(&mutex);
497  }
498  };
499 
500  class Event
501  {
502  pthread_mutex_t mutex;
503  pthread_cond_t cond;
504 
505  public:
506  Event() {
507  pthread_mutex_init(&mutex, NULL);
508  pthread_cond_init(&cond, NULL);
509  }
510 
511  ~Event() {
512  pthread_mutex_destroy(&mutex);
513  pthread_cond_destroy(&cond);
514  }
515 
516  inline bool wait(int timeout_msec=-1) {
517  bool res = true;
518  pthread_mutex_lock(&mutex);
519  if(timeout_msec > 0) {
520  struct timeval tv;
521  struct timespec ts;
522 
523  gettimeofday(&tv, NULL);
524 
525  ts.tv_sec = time(NULL) + timeout_msec / 1000;
526  ts.tv_nsec = tv.tv_usec * 1000 + 1000 * 1000 * (timeout_msec % 1000);
527  ts.tv_sec += ts.tv_nsec / (1000 * 1000 * 1000);
528  ts.tv_nsec %= (1000 * 1000 * 1000);
529 
530  int n = pthread_cond_timedwait(&cond, &mutex, &ts);
531  res = (n == 0); // n == 0 : signaled, n == ETIMEDOUT
532  }
533  else {
534  pthread_cond_wait(&cond, &mutex);
535  res = true;
536  }
537  pthread_mutex_unlock(&mutex);
538  return res;
539  }
540 
541  inline bool set() {
542  pthread_mutex_lock(&mutex);
543  pthread_cond_signal(&cond);
544  pthread_mutex_unlock(&mutex);
545  return true;
546  }
547  };
548 
549 
550  class Thread
551  {
552  pthread_t thread;
553 
554  public:
555  Thread() : thread(0){ }
556 
557  bool start(void*(*thread_proc)(void*), void* arg, size_t stack_size=16*1024) {
558  pthread_attr_t attr;
559  size_t stacksize;
560  pthread_attr_init(&attr);
561 
562  //pthread_attr_getstacksize(&attr, &stacksize);
563 
564  pthread_attr_setstacksize(&attr, stack_size);
565  int res = pthread_create(&thread, &attr, thread_proc, (void*)arg);
566  if(res != 0) {
567  thread = 0;
568  }
569 
570  return (res == 0);
571  }
572 
573  void join() {
574  if(thread != 0) {
575  pthread_join(thread, NULL);
576  }
577  thread = 0;
578  }
579  };
580 
581  static void msleep(unsigned int msec) {
582  usleep(msec*1000);
583  }
584  }; // Platform
585 
586 #endif // #ifdef WIN32
587 
588 
589 /**********************************************************************************************************
590  *
591  * platform independent implementation
592  *
593  **********************************************************************************************************/
594 
595  class LockGuard {
596 
598 
599  public:
600  LockGuard(Platform::Mutex& m) : mutex(m) {
601  mutex.lock();
602  }
603 
605  mutex.unlock();
606  }
607  };
608 
609 
611  {
612  public:
613  static void replace(std::string & src, std::string s1, std::string s2) {
614  size_t ofs = 0;
615 
616  do {
617  ofs = src.find(s1, ofs);
618  if(ofs == -1) {
619  return;
620  }
621  src.replace(ofs, s1.length(), s2);
622  ofs += s2.length();
623  }while(1);
624  }
625 
626  // trim from start
627  static inline std::string &ltrim(std::string &s) {
628  s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun<int, int>(std::isspace))));
629  return s;
630  }
631 
632  // trim from end
633  static inline std::string &rtrim(std::string &s) {
634  s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun<int, int>(std::isspace))).base(), s.end());
635  return s;
636  }
637 
638  // trim from both ends
639  static inline std::string &strip(std::string &s) {
640  return ltrim(rtrim(s));
641  }
642 
643  static int split(std::vector<std::string> & token_list, const char* c_str, char delimiter, int token_max=-1) {
644  std::stringstream is(c_str);
645  std::string token;
646 
647  bool check_max = (token_max > 0);
648 
649  token_list.clear();
650  while(getline(is, token, delimiter)){
651  token_list.push_back(token);
652  if(check_max) {
653  if(--token_max <= 0) {
654  break;
655  }
656  }
657  }
658 
659  return token_list.size();
660  }
661 
662  static std::string join(std::vector<std::string>& str_list, const std::string& delimiter) {
663  std::ostringstream s;
664  for(size_t i=0; i<str_list.size(); i++) {
665  if(i != 0) {
666  s << delimiter;
667  }
668  s << str_list[i];
669  }
670  return s.str();
671  }
672 
673  template <typename T>
674  static void to_string_list(std::vector<std::string>& str_list, T* array, size_t array_num) {
675  for(size_t i=0; i<array_num; i++) {
676  std::ostringstream s;
677  s << array[i];
678  str_list.push_back(s.str());
679  }
680  }
681 
682  static int extract_attributes(std::map<std::string, std::string>& attribute_list, std::vector<std::string>& tokens) {
683  std::vector<std::string> attribute_pair;
684 
685  for(size_t i=0; i<tokens.size(); i++) {
686  // "attribute_name=value"
687  if(split(attribute_pair, tokens[i].c_str(), '=') == 2) {
688  attribute_list[attribute_pair[0]] = attribute_pair[1];
689  }
690  }
691 
692  return attribute_list.size();
693  }
694  };
695 
696  struct FrameBuffer {
697  unsigned char buffer[1024];
698  size_t offset;
699 
701  reset();
702  }
703 
704  void push(unsigned char byte) {
705  if(offset < sizeof(buffer)) {
706  buffer[offset++] = byte;
707  }
708  }
709 
710  void reset() {
711  memset(buffer, 0, sizeof(buffer));
712  offset = 0;
713  }
714  };
715 
716  /**************************************************************************
717  *
718  * COMMUNICATION PROTOCOL (ASCII)
719  *
720  **************************************************************************/
721 
723  public:
724  static const uint8_t MSG_HDR_SENSOR_DATA = '$';
725  static const uint8_t MSG_HDR_COMMAND = '@';
726  static const uint8_t MSG_HDR_RESPONSE = '~';
727  static const uint8_t MSG_CRC_DELIMITER = '*';
728  static const uint8_t MSG_TAIL_CR = '\r';
729  static const uint8_t MSG_TAIL_LF = '\n';
730 
731  private:
733  bool debug;
734 
735  public:
737  {}
738  virtual ~iAsciiProtocol() {}
739 
740  protected:
741  void push_byte(unsigned char c) {
742  switch(c) {
743  // start of frame
744  case MSG_HDR_SENSOR_DATA:
745  case MSG_HDR_RESPONSE:
746  case MSG_HDR_COMMAND:
747  frame_buffer.reset();
748  frame_buffer.push(c);
749  break;
750 
751  // end of frame
752  case MSG_TAIL_LF:
753  if(frame_buffer.offset > 0) {
754  parse_message(reinterpret_cast<const char*>(frame_buffer.buffer));
755  }
756  frame_buffer.reset();
757  break;
758 
759  case MSG_TAIL_CR:
760  // nothing to do
761  break;
762 
763  default:
764  frame_buffer.push(c);
765  }
766  }
767 
768  virtual void update_attributes(std::vector<std::string>& tokens) = 0;
769 
770  private:
771  bool parse_message(const char* ascii_frame) {
772  DBG_PRINTF(debug, "## ASC FRAME : [%s]\n", ascii_frame);
773 
774  std::vector<std::string> tokens;
775  if(StringUtil::split(tokens, ascii_frame, '*') != 2) {
776  return false;
777  }
778 
779  std::string& str_message = tokens[0];
780  int crc_from_sensor = std::stoul(tokens[1].c_str(), nullptr, 16);
781 
782  uint8_t crc_calc = 0;
783  for(size_t i=0; i<str_message.length(); i++) {
784  crc_calc ^= str_message[i];
785  }
786 
787  DBG_PRINTF(debug, "\t= MSG %s, CRC %X, CRC_C %X\n", tokens[0].c_str(), crc_from_sensor, crc_calc);
788 
789  if(crc_calc == crc_from_sensor) {
790  StringUtil::split(tokens, str_message.c_str(), ',');
791  update_attributes(tokens);
792  return true;
793  }
794  else {
795  return false;
796  }
797  }
798  };
799 
800  /**************************************************************************
801  *
802  * COMMUNICATION PROTOCOL (BINARY)
803  *
804  **************************************************************************/
805 
807  {
808  public:
809  enum {
810  STX = 0x02,
811  ETX = 0x03,
812  DLE = 0x10,
813  };
814 
815  enum ReturnCode {
816  STATE_NOP = 0,
818  STATE_COMPLETE, // RECEIVE COMPLETE
820  };
821 
822  private:
825 
827  uint8_t crc_calc;
828  uint16_t accumulater;
829 
830  bool debug;
831 
832  ReturnCode (FilterByteStuffing::*receiver)(uint8_t);
833 
834  public:
836  : stream(s), crc_calc(0), accumulater(0)
837  , state_receiving(false), last_state(STATE_NOP)
838  , debug(DEBUG_BINARY_PROTOCOL)
839  {
840  receiver = &FilterByteStuffing::state_data;
841  }
842 
844 
845  ReturnCode operator ()(uint8_t byte) {
846  DBG_PRINTF(debug, "new byte %02X\n", byte);
847  last_state = (this->*receiver)(byte);
848  return last_state;
849  }
850 
851  bool is_busy() {
852  return (last_state==STATE_BUSY);
853  }
854 
855  private:
857  state_receiving = false;
858  stream.reset();
859  crc_calc = 0;
860  accumulater = 0;
861  }
862 
863  bool check_crc() {
864  uint8_t crc_rcv = accumulater & 0x00FF;
865  DBG_PRINTF(debug, "### CRC_R %02X, CRC_C %02X\n", crc_rcv, crc_calc);
866  return crc_rcv == crc_calc;
867  }
868 
869  void push_data(uint8_t byte) {
870  stream.push(byte);
871 
872  accumulater <<= 8;
873  accumulater |= byte;
874  uint8_t prv_byte = ((accumulater & 0xFF00) >> 8);
875  crc_calc ^= prv_byte;
876  }
877 
878  ReturnCode state_data(uint8_t byte) {
879  if(byte == DLE) {
881  return STATE_BUSY;
882  }
883  else {
884  if(state_receiving) {
885  push_data(byte);
886  return STATE_BUSY;
887  }
888  else {
889  return STATE_NOP;
890  }
891  }
892  }
893 
894  ReturnCode state_control(uint8_t byte) {
895  ReturnCode res;
896 
897  switch(byte) {
898  case STX:
899  clear_all_states();
900  state_receiving = true;
901  res = STATE_BUSY;
902  break;
903 
904  case ETX:
905  state_receiving = false;
906  res = check_crc() ? STATE_COMPLETE : STATE_ERROR;
907  break;
908 
909  case DLE:
910  push_data(byte);
911  res = STATE_BUSY;
912  break;
913 
914  default:
915  clear_all_states();
916  state_receiving = true;
917  res = STATE_ERROR;
918  }
919 
920  receiver = &FilterByteStuffing::state_data;
921  return res;
922  }
923  }; // FilterDleToPlain
924 
925 
927  {
928  public:
929  enum Tag {
930  // value type
931  TAG_TYPE_NONE = 0,
932  TAG_TYPE_INT8 = 1,
933  TAG_TYPE_UINT8 = 2,
934  TAG_TYPE_INT16 = 3,
935  TAG_TYPE_UINT16 = 4,
936  TAG_TYPE_INT32 = 5,
937  TAG_TYPE_UINT32 = 6,
938  TAG_TYPE_INT64 = 7,
939  TAG_TYPE_UINT64 = 8,
940  TAG_TYPE_FLOAT32 = 9,
941  TAG_TYPE_FLOAT64 = 10,
942  TAG_TYPE_STRING = 11,
943 
944  // node attribute
945  TAG_HAS_LEAF_NODES = (0x01<<7),
946  TAG_NEXT_NODE_EXIST = (0x01<<6),
947  TAG_LIST_NODE = (0x01<<5),
948 
949  TAG_TYPE_MASK = 0x0F,
950  };
951 
952  protected:
953  struct Varient {
954  Varient() : type(TAG_TYPE_NONE) {}
955  uint8_t type;
956  union {
957  int8_t i8;
958  uint8_t ui8;
959  int16_t i16;
960  uint16_t ui16;
961  int32_t i32;
962  uint32_t ui32;
963  int64_t i64;
964  uint64_t ui64;
965  float f32;
966  double f64;
967  } value;
968 
969  template <typename T>
970  bool set(uint8_t t, T v) {
971  type = t;
972  switch(type) {
973  case TAG_TYPE_INT8: value.i8 = (int8_t)v; break;
974  case TAG_TYPE_UINT8: value.ui8 = (uint8_t)v; break;
975  case TAG_TYPE_INT16: value.i16 = (int16_t)v; break;
976  case TAG_TYPE_UINT16: value.ui16 = (uint16_t)v; break;
977  case TAG_TYPE_INT32: value.i32 = (int32_t)v; break;
978  case TAG_TYPE_UINT32: value.ui32 = (uint32_t)v; break;
979  case TAG_TYPE_INT64: value.i64 = (int64_t)v; break;
980  case TAG_TYPE_UINT64: value.ui64 = (uint64_t)v; break;
981  case TAG_TYPE_FLOAT32: value.f32 = (float)v; break;
982  case TAG_TYPE_FLOAT64: value.f64 = (double)v; break;
983  default: return false;
984  }
985  return true;
986  }
987  };
988 
989  public:
990  struct Node {
991  std::string name;
992  std::vector<Varient> list;
993  };
994 
995  private:
996  struct Stream {
997  unsigned char* buffer;
998  size_t pos;
999  size_t length;
1000  bool debug;
1001 
1002  Stream(unsigned char* s, size_t l) : buffer(s), length(l), pos(0), debug(DEBUG_BINARY_PROTOCOL) {
1003  print_buffer();
1004  }
1005 
1006  int getc() {
1007  if(pos < length) { return buffer[pos++]; }
1008  else { return -1; }
1009  }
1010 
1011  int peek() {
1012  if(pos < length) { return buffer[pos]; }
1013  else { return -1; }
1014  }
1015 
1016  template <typename T>
1017  void read_value(T& value) {
1018  T* v = (T*)(&buffer[pos]);
1019  pos += sizeof(T);
1020  value = *v;
1021  }
1022 
1023  bool read_value(uint8_t value_type, Varient& v) {
1024  switch(value_type) {
1025  case TAG_TYPE_INT8: { int8_t value; read_value(value); v.set(value_type, value);} break;
1026  case TAG_TYPE_UINT8: { uint8_t value; read_value(value); v.set(value_type, value);} break;
1027  case TAG_TYPE_INT16: { int16_t value; read_value(value); v.set(value_type, value);} break;
1028  case TAG_TYPE_UINT16: {uint16_t value; read_value(value); v.set(value_type, value);} break;
1029  case TAG_TYPE_INT32: { int32_t value; read_value(value); v.set(value_type, value);} break;
1030  case TAG_TYPE_UINT32: {uint32_t value; read_value(value); v.set(value_type, value);} break;
1031  case TAG_TYPE_INT64: { int64_t value; read_value(value); v.set(value_type, value);} break;
1032  case TAG_TYPE_UINT64: {uint64_t value; read_value(value); v.set(value_type, value);} break;
1033  case TAG_TYPE_FLOAT32: { float value; read_value(value); v.set(value_type, value);} break;
1034  case TAG_TYPE_FLOAT64: { double value; read_value(value); v.set(value_type, value);} break;
1035  default: return false;
1036  }
1037  return true;
1038  }
1039 
1040  void read_list(uint8_t value_type, std::vector<Varient>& list, size_t count) {
1041  Varient v;
1042  for(size_t i=0; i<count; i++) {
1043  if(read_value(value_type, v) == true) {
1044  list.push_back(v);
1045  }
1046  }
1047  }
1048 
1049  std::string read_string() {
1050  std::string str((const char*)&buffer[pos]);
1051  pos += str.length() + 1; // '\0'
1052  return str;
1053  }
1054 
1055  private:
1056  void print_buffer() {
1057  if(debug) {
1058  for(size_t i=0; i<length; i++) {
1059  DBG_PRINTF(debug, "%02X ", buffer[i]);
1060  }
1061  DBG_PRINTF(debug, "\n");
1062  }
1063  }
1064  };
1065 
1067  bool debug;
1068 
1069  std::vector<Node> node_list;
1070 
1071  public:
1072  iNodeParser(unsigned char* stream=0, size_t stream_len=0) : istream(stream, stream_len), debug(false){}
1073  virtual ~iNodeParser() {}
1074 
1075  void parse() {
1076  read_nodes();
1077  new_node(node_list);
1078  }
1079 
1080  virtual void new_node(std::vector<Node>& node_list) = 0;
1081 
1082  private:
1083  void read_nodes() {
1084  if(istream.length == 0 || istream.peek() < 0) {
1085  return;
1086  }
1087 
1088  uint8_t tag = istream.getc();
1089  uint8_t type_of_value = (tag & TAG_TYPE_MASK);
1090 
1091  Node node;
1092  node.name = istream.read_string();
1093  if(type_of_value) {
1094  if(tag&TAG_LIST_NODE) {
1095  uint16_t count = 0;
1096  istream.read_value(count);
1097  istream.read_list(type_of_value, node.list, count);
1098  }
1099  else {
1100  Varient value;
1101  istream.read_value(type_of_value, value);
1102  node.list.push_back(value);
1103  }
1104  }
1105 
1106  node_list.push_back(node);
1107 
1108  if((tag & TAG_HAS_LEAF_NODES) || (tag & TAG_NEXT_NODE_EXIST)) {
1109  read_nodes();
1110  }
1111  }
1112  };
1113 
1114 
1118 
1120  {
1122 
1123  public:
1124  BinaryNodeParser(iBinaryProtocol* s, unsigned char* stream, size_t stream_len) : protocol(s), iNodeParser(stream, stream_len) {}
1125  void new_node(std::vector<Node>& node_list) {
1126  protocol->update_attributes(node_list);
1127  }
1128  };
1129 
1130  public:
1131  iBinaryProtocol() : filter_byte_stuffing(binary_stream) {}
1132  virtual ~iBinaryProtocol() {}
1133 
1134  protected:
1135  void push_byte(unsigned char c) {
1136  if(filter_byte_stuffing(c) == FilterByteStuffing::STATE_COMPLETE) {
1137  BinaryNodeParser parser(this, binary_stream.buffer, binary_stream.offset);
1138  parser.parse();
1139  }
1140  }
1141 
1142  bool is_receiving() {
1143  return filter_byte_stuffing.is_busy();
1144  }
1145 
1146  virtual void update_attributes(std::vector<iNodeParser::Node>& node_list) = 0;
1147 
1148  }; // BinaryProtocol
1149 
1150 
1151  /**************************************************************************
1152  *
1153  * COMMUNICATION PROTOCOL
1154  *
1155  **************************************************************************/
1156 
1158  {
1159  public:
1161  virtual ~iProtocol() {}
1162 
1163  bool feed(unsigned char* data, int data_len) {
1164  if(!data || data_len <= 0) {
1165  return false;
1166  }
1167 
1168  for(int i=0; i<data_len; i++) {
1169  iBinaryProtocol::push_byte(data[i]);
1170  if(iBinaryProtocol::is_receiving() == false) {
1171  iAsciiProtocol::push_byte(data[i]);
1172  }
1173  }
1174 
1175  return true;
1176  }
1177  };
1178 
1179  /**************************************************************************
1180  *
1181  * ATTITUDE REPRESENTATIONS
1182  *
1183  **************************************************************************/
1184 
1185  struct EulerAngle
1186  {
1187  double roll, pitch, yaw;
1188 
1189  EulerAngle(double r=0, double p=0, double y=0) : roll(r), pitch(p), yaw(y) {}
1190  EulerAngle(std::string str_rpy, char delimiter=' ') {
1191  set(str_rpy, delimiter);
1192  }
1193 
1194  inline void reset() {
1195  set(0, 0, 0);
1196  }
1197 
1198  inline void set(double r, double p, double y) {
1199  roll=r, pitch=p, yaw=y;
1200  }
1201 
1202  inline void set(std::string str_rpy, char delimiter=' ') {
1203  std::vector<std::string> tokens;
1204  if(StringUtil::split(tokens, str_rpy.c_str(), delimiter) == 3) {
1205  set(tokens);
1206  }
1207  else {
1208  throw(myAhrsException("EulerAngle: Invalid String"));
1209  }
1210  }
1211 
1212  inline void set(std::vector<std::string>& str_array) {
1213  if(str_array.size() != 3) {
1214  throw(myAhrsException("EulerAngle: size error"));
1215  }
1216  roll = atof(str_array[0].c_str());
1217  pitch = atof(str_array[1].c_str());
1218  yaw = atof(str_array[2].c_str());
1219  }
1220 
1221  inline std::string to_string() {
1222  std::stringstream s;
1223  s << roll <<", "<< pitch <<", "<< yaw;
1224  return s.str();
1225  }
1226  };
1227 
1228  struct Quaternion;
1230  {
1231  double mat[9];
1232 
1234  reset();
1235  }
1236 
1237  DirectionCosineMatrix(double dcm[9]) {
1238  set(dcm);
1239  }
1240 
1241  DirectionCosineMatrix(double& m11, double& m12, double& m13,
1242  double& m21, double& m22, double& m23,
1243  double& m31, double& m32, double& m33) {
1244  set(m11, m12, m13, m21, m22, m23, m31, m32, m33);
1245  }
1246 
1247  DirectionCosineMatrix(std::string str_mat, char delimiter=' ') {
1248  set(str_mat, delimiter);
1249  }
1250 
1251  inline void reset() {
1252  memset(mat, 0, sizeof(mat));
1253  }
1254 
1255  inline void set(double dcm[9]) {
1256  memcpy(mat, dcm, sizeof(mat));
1257  }
1258 
1259  inline void set(double& m11, double& m12, double& m13,
1260  double& m21, double& m22, double& m23,
1261  double& m31, double& m32, double& m33) {
1262  mat[0] = m11, mat[1] = m12, mat[2] = m13,
1263  mat[3] = m21, mat[4] = m22, mat[5] = m23,
1264  mat[6] = m31, mat[7] = m32, mat[8] = m33;
1265  }
1266 
1267  inline void set(std::string str_mat, char delimiter=' ') {
1268  std::vector<std::string> tokens;
1269  if(StringUtil::split(tokens, str_mat.c_str(), delimiter) == 9) {
1270  set(tokens);
1271  }
1272  else {
1273  throw(myAhrsException("Matrix3x3: Invalid String"));
1274  }
1275  }
1276 
1277  inline void set(std::vector<std::string>& str_array) {
1278  if(str_array.size() != 9) {
1279  throw(myAhrsException("Matrix3x3: size error"));
1280  }
1281  for(int i=0; i<9; i++) {
1282  mat[i] = (double)atof(str_array[i].c_str());
1283  }
1284  }
1285 
1286  inline std::string to_string() {
1287  std::vector<std::string> temp;
1288  StringUtil::to_string_list(temp, mat, 9);
1289  return StringUtil::join(temp, ", ");
1290  }
1291 
1293  EulerAngle e;
1294  double RAD2DEG = 180/M_PI;
1295  e.roll = atan2(MAT(1, 2), MAT(2, 2))*RAD2DEG;
1296  e.pitch = -asin(MAT(0, 2))*RAD2DEG;
1297  e.yaw = atan2(MAT(0, 1), MAT(0, 0))*RAD2DEG;
1298  return e;
1299  }
1300 
1301 #if 0
1302  void set(Quaternion& q) {
1303  // http://www.mathworks.co.kr/kr/help/aeroblks/quaternionstodirectioncosinematrix.html
1304  double xx = q.x*q.x;
1305  double xy = q.x*q.y;
1306  double xz = q.x*q.z;
1307  double xw = q.x*q.w;
1308  double yy = q.y*q.y;
1309  double yz = q.y*q.z;
1310  double yw = q.y*q.w;
1311  double zz = q.z*q.z;
1312  double zw = q.z*q.w;
1313  double ww = q.w*q.w;
1314 
1315  mat[0] = xx - yy - zz + ww;
1316  mat[1] = 2.0*(xy + zw);
1317  mat[2] = 2.0*(xz - yw);
1318 
1319  mat[3] = 2.0*(xy - zw);
1320  mat[4] = -xx + yy - zz + ww;
1321  mat[5] = 2.0*(yz + xw);
1322 
1323  mat[6] = 2.0*(xz + yw);
1324  mat[7] = 2.0*(yz - xw);
1325  mat[8] = -xx - yy + zz + ww;
1326  }
1327 #else
1328  void set(Quaternion& q);
1329 #endif
1330 
1331  private:
1332  inline double MAT(unsigned int row, unsigned int col) {
1333  return mat[(row)*3 + col];
1334  }
1335  };
1336 
1337 
1338  struct Quaternion
1339  {
1340  double x, y, z, w;
1341 
1342  Quaternion(double _x=0, double _y=0, double _z=0, double _w=1): x(_x), y(_y), z(_z), w(_w) {}
1343  Quaternion(std::string str_xyzw, char delimiter=' ') {
1344  set(str_xyzw, delimiter);
1345  }
1346 
1347  inline void reset() {
1348  set(0, 0, 0, 1);
1349  }
1350 
1351  inline void set(double _x, double _y, double _z, double _w) {
1352  x=_x, y=_y, z=_z, w=_w;
1353  }
1354 
1355  inline void set(std::string str_xyzw, char delimiter=' ') {
1356  std::vector<std::string> tokens;
1357  if(StringUtil::split(tokens, str_xyzw.c_str(), delimiter) == 4) {
1358  set(tokens);
1359  }
1360  else {
1361  throw(myAhrsException("Quaternion: Invalid String"));
1362  }
1363  }
1364 
1365  inline void set(std::vector<std::string>& str_array) {
1366  if(str_array.size() != 4) {
1367  throw(myAhrsException("Quaternion: size error"));
1368  }
1369  x = atof(str_array[0].c_str());
1370  y = atof(str_array[1].c_str());
1371  z = atof(str_array[2].c_str());
1372  w = atof(str_array[3].c_str());
1373  }
1374 
1375  inline std::string to_string() {
1376  std::stringstream s;
1377  s << x <<", "<< y <<", "<< z <<", "<< w;
1378  return s.str();
1379  }
1380 
1381  void normalize() {
1382  double norm = sqrt(x*x + y*y + z*z + w*w);
1383  x = x/norm;
1384  y = y/norm;
1385  z = z/norm;
1386  w = w/norm;
1387  }
1388 
1390  Quaternion q;
1391  q.x = -x;
1392  q.y = -y;
1393  q.z = -z;
1394  q.w = w;
1395  return q;
1396  }
1397 
1398  // http://www.mathworks.co.kr/kr/help/aerotbx/ug/quatmultiply.html
1399  // qxr = q*r
1401  Quaternion qxr;
1402 
1403  qxr.w = r.w*q.w - r.x*q.x - r.y*q.y - r.z*q.z;
1404  qxr.x = r.w*q.x + r.x*q.w - r.y*q.z + r.z*q.y;
1405  qxr.y = r.w*q.y + r.x*q.z + r.y*q.w - r.z*q.x;
1406  qxr.z = r.w*q.z - r.x*q.y + r.y*q.x + r.z*q.w;
1407 
1408  return qxr;
1409  }
1410 
1412  double xx = x*x;
1413  double xy = x*y;
1414  double xz = x*z;
1415  double xw = x*w;
1416  double yy = y*y;
1417  double yz = y*z;
1418  double yw = y*w;
1419  double zz = z*z;
1420  double zw = z*w;
1421  double ww = w*w;
1422 
1423  double RAD2DEG = 180/M_PI;
1424  EulerAngle e;
1425  e.roll = atan2(2.0*(yz + xw), -xx - yy + zz + ww)*RAD2DEG;
1426  e.pitch = -asin(2.0*(xz - yw))*RAD2DEG;
1427  e.yaw = atan2(2.0*(xy + zw), xx - yy - zz + ww)*RAD2DEG;
1428  return e;
1429  }
1430 
1432  // http://www.mathworks.co.kr/kr/help/aeroblks/quaternionstodirectioncosinematrix.html
1433  double xx = x*x;
1434  double xy = x*y;
1435  double xz = x*z;
1436  double xw = x*w;
1437  double yy = y*y;
1438  double yz = y*z;
1439  double yw = y*w;
1440  double zz = z*z;
1441  double zw = z*w;
1442  double ww = w*w;
1443 
1445 
1446  dcm.mat[0] = xx - yy - zz + ww;
1447  dcm.mat[1] = 2.0*(xy + zw);
1448  dcm.mat[2] = 2.0*(xz - yw);
1449 
1450  dcm.mat[3] = 2.0*(xy - zw);
1451  dcm.mat[4] = -xx + yy - zz + ww;
1452  dcm.mat[5] = 2.0*(yz + xw);
1453 
1454  dcm.mat[6] = 2.0*(xz + yw);
1455  dcm.mat[7] = 2.0*(yz - xw);
1456  dcm.mat[8] = -xx - yy + zz + ww;
1457 
1458  return dcm;
1459  }
1460  };
1461 
1463  *this = q.to_dcm();
1464  }
1465 
1466 
1467  /**************************************************************************
1468  *
1469  * IMU
1470  *
1471  **************************************************************************/
1472  template <typename Type>
1473  struct ImuData
1474  {
1475  Type ax, ay, az, gx, gy, gz, mx, my, mz, temperature;
1476 
1478  ax = ay = az = gx = gy = gz = mx = my = mz = temperature = 0;
1479  }
1480 
1481  ImuData(Type data[10]) {
1482  set(data);
1483  }
1484 
1485  inline void reset() {
1486  Type d[10];
1487  memset(d, 0, sizeof(d));
1488  set(d);
1489  }
1490 
1491  inline void set(Type data[10]) {
1492  int i=0;
1493  ax = data[i++];
1494  ay = data[i++];
1495  az = data[i++];
1496 
1497  gx = data[i++];
1498  gy = data[i++];
1499  gz = data[i++];
1500 
1501  mx = data[i++];
1502  my = data[i++];
1503  mz = data[i++];
1504 
1505  temperature = data[i++];
1506  }
1507 
1508  inline void set(std::string str_mat, char delimiter=' ') {
1509  std::vector<std::string> tokens;
1510  if(StringUtil::split(tokens, str_mat.c_str(), delimiter) == 10) {
1511  set(tokens);
1512  }
1513  else {
1514  throw(myAhrsException("imu: Invalid String"));
1515  }
1516  }
1517 
1518  inline void set(std::vector<std::string>& str_array) {
1519  if(str_array.size() != 10) {
1520  throw(myAhrsException("imu: size error"));
1521  }
1522  Type data[10];
1523  for(int i=0; i<10; i++) {
1524  data[i] = (Type)atof(str_array[i].c_str());
1525  }
1526  set(data);
1527  }
1528 
1529  inline std::string to_string() {
1530  std::stringstream s;
1531  s << ax <<", "<<ay<<", "<<az<<", "<<gx<<", "<<gy<<", "<<gz<<", "<<mx<<", "<<my<<", "<<mz<<", "<<temperature;
1532  return s.str();
1533  }
1534  };
1535 
1536 
1537  /**************************************************************************
1538  *
1539  * AHRS
1540  *
1541  **************************************************************************/
1542 
1543  enum Attitude {
1547  };
1548 
1549  enum Imu {
1553  };
1554 
1555  struct SensorData {
1557 
1561 
1565 
1567  reset();
1568  }
1569 
1570  void reset() {
1571  sequence_number = -1;
1572  euler_angle.reset();
1573  quaternion.reset();
1574  imu_rawdata.reset();
1575  imu.reset();
1576  attitude_type = NOT_DEF_ATTITUDE;
1577  imu_type = NOT_DEF_IMU;
1578  }
1579 
1581  euler_angle = e;
1582  attitude_type = EULER_ANGLE;
1583  }
1584 
1586  quaternion = q;
1587  attitude_type = QUATERNION;
1588  }
1589 
1591  imu_rawdata = i;
1592  imu_type = RAW;
1593  }
1594 
1596  imu = i;
1597  imu_type = COMPENSATED;
1598  }
1599 
1600  std::string to_string() {
1601  std::vector<std::string> str_array;
1602  char temp[32];
1603  sprintf(temp, "%d", sequence_number);
1604  str_array.push_back("sequence = " + std::string(temp));
1605 
1606  switch(attitude_type) {
1607  case EULER_ANGLE:
1608  str_array.push_back("euler_angle = " + euler_angle.to_string());
1609  break;
1610  case QUATERNION:
1611  str_array.push_back("quaternion = " + quaternion.to_string());
1612  break;
1613  default:
1614  str_array.push_back("no attitude ");
1615  break;
1616  }
1617 
1618  switch(imu_type) {
1619  case RAW:
1620  str_array.push_back("imu_raw = " + imu_rawdata.to_string());
1621  break;
1622  case COMPENSATED:
1623  str_array.push_back("imu_comp = " + imu.to_string());
1624  break;
1625  default:
1626  str_array.push_back("no imu ");
1627  break;
1628  }
1629  return StringUtil::join(str_array, "\n");
1630  }
1631  };
1632 
1633 
1635  {
1636  bool debug;
1637 
1639 
1642 
1645 
1648 
1650 
1652  std::map<std::string, std::string> attribute_map;
1653 
1654  typedef bool (iMyAhrsPlus::*AscHandler)(std::vector<std::string>& tokens);
1655  std::map<std::string, AscHandler> ascii_handler_data_map;
1656 
1657  typedef bool (iMyAhrsPlus::*AscRspHandler)(std::map<std::string, std::string>& attributes);
1658  std::map<std::string, AscRspHandler> ascii_handler_rsp_map;
1659 
1660  typedef bool (iMyAhrsPlus::*BinHandler)(iNodeParser::Node& node);
1661  std::map<std::string, BinHandler> binary_handler_data_map;
1662 
1663  static const int ACCEL_RANGE = 16;
1664  static const int GYRO_RANGE = 2000;
1665  static const int MAGNET_RANGE = 300;
1666  static const int TEMP_RANGE = 200;
1667  static const int EULER_RANGE = 180;
1668 
1669  class Protocol : public iProtocol
1670  {
1672 
1673  public:
1674  Protocol(iMyAhrsPlus* s) : ahrs(s) {}
1675 
1676  void update_attributes(std::vector<std::string>& tokens) {
1677  if(tokens.size() >= 2) {
1678  ahrs->ascii_parse_response(tokens);
1679  }
1680  }
1681 
1682  void update_attributes(std::vector<iNodeParser::Node>& node_list) {
1683  if(node_list.size() > 0) {
1684  ahrs->binary_parse_response(node_list);
1685  }
1686  }
1687  } protocol;
1688 
1690 
1693  std::deque< std::vector<std::string> > queue;
1694 
1695  public:
1698 
1699  void clear() {
1700  LockGuard _l(lock);
1701  queue.clear();
1702  }
1703 
1704  bool wait(int timeout_msec) {
1705  if(size() <= 0) {
1706  return event.wait(timeout_msec);
1707  }
1708  else {
1709  return true;
1710  }
1711  }
1712 
1713  void push_back(std::vector<std::string>& list) {
1714  LockGuard _l(lock);
1715  queue.push_back(list);
1716  event.set();
1717  }
1718 
1719  size_t size() {
1720  LockGuard _l(lock);
1721  return queue.size();
1722  }
1723 
1724  bool pop(std::vector<std::string>& out) {
1725  LockGuard _l(lock);
1726  if(queue.size() > 0) {
1727  out = queue.front();
1728  queue.pop_front();
1729  return true;
1730  }
1731  else {
1732  return false;
1733  }
1734  }
1735  } response_message_queue;
1736 
1738  {
1739  public:
1740  enum EventId {
1741  NONE = 0,
1745  };
1746 
1748 
1749  EventItem(EventId id=NONE) : event_id(id) {}
1750  virtual ~EventItem() {}
1751 
1752  virtual SensorData* get_sensor_data() { return 0; }
1753  virtual std::map<std::string, std::string>* get_attribute() { return 0; }
1754  };
1755 
1756  class EventQueue {
1757  private:
1758  class EventItemExit : public EventItem
1759  {
1760  public:
1762  };
1763 
1764  class EventItemData : public EventItem
1765  {
1767 
1768  public:
1769  EventItemData(SensorData& d) : EventItem(DATA), data(d) {}
1771  SensorData* get_sensor_data() { return &data; }
1772  };
1773 
1775  {
1776  std::map<std::string, std::string> attribute;
1777 
1778  public:
1779  EventItemAttribute(std::map<std::string, std::string>& a) : EventItem(ATTRIBUTE), attribute(a) {}
1781  std::map<std::string, std::string>* get_attribute() { return &attribute; }
1782  };
1783 
1784  std::deque<EventItem*> deque;
1787 
1788  static const size_t EVENT_MAX_NUM = 5;
1789 
1790  public:
1793  int num = deque.size();
1794  for(int i=0; i<num; i++) {
1795  delete pop_event();
1796  }
1797  }
1798 
1799  inline bool wait(int timeout_msec=-1) {
1800  return event.wait(timeout_msec);
1801  }
1802 
1804  LockGuard _l(lock);
1805  deque.push_front(new EventItemExit()); // highest priority
1806  event.set();
1807  }
1808 
1809  void push_event_attribute_change(std::string& attr_name, std::string& attr_value) {
1810  LockGuard _l(lock);
1811  std::map<std::string, std::string> attribute;
1812  attribute[attr_name] = attr_value;
1813  deque.push_back(new EventItemAttribute(attribute));
1814  event.set();
1815  }
1816 
1818  LockGuard _l(lock);
1819  if(deque.size() < EVENT_MAX_NUM) {
1820  deque.push_back(new EventItemData(data));
1821  }
1822  event.set();
1823  }
1824 
1826  LockGuard _l(lock);
1827  EventItem* i = 0;
1828  if(deque.size() > 0) {
1829  i = deque.front();
1830  deque.pop_front();
1831  }
1832  return i;
1833  }
1834  } event_queue;
1835 
1836  // prevent copy
1837  iMyAhrsPlus(iMyAhrsPlus& rhs):protocol(this) {}
1838 
1839  public:
1840  iMyAhrsPlus(std::string port_name="", unsigned int baudrate=115200)
1841  : serial(port_name.c_str(), baudrate)
1842  , debug(DEBUG_MYAHRS_PLUS)
1843  , protocol(this)
1844  , sensor_id(-1)
1845  , activate_user_event(false)
1846  , thread_receiver_ready(false)
1847  {
1848  // response message parser (ascii)
1849  ascii_handler_rsp_map[std::string("~trig")] = &iMyAhrsPlus::ascii_rsp_trigger;
1850  ascii_handler_rsp_map[std::string("~ping")] = &iMyAhrsPlus::ascii_rsp_ping;
1851  ascii_handler_rsp_map[std::string("~divider")] = &iMyAhrsPlus::ascii_rsp_divider;
1852  ascii_handler_rsp_map[std::string("~mode")] = &iMyAhrsPlus::ascii_rsp_mode;
1853  ascii_handler_rsp_map[std::string("~asc_out")] = &iMyAhrsPlus::ascii_rsp_asc_out;
1854  ascii_handler_rsp_map[std::string("~bin_out")] = &iMyAhrsPlus::ascii_rsp_bin_out;
1855  ascii_handler_rsp_map[std::string("~set_offset")] = &iMyAhrsPlus::ascii_rsp_user_orientation;
1856  ascii_handler_rsp_map[std::string("~clr_offset")] = &iMyAhrsPlus::ascii_rsp_user_orientation;
1857  ascii_handler_rsp_map[std::string("~calib")] = &iMyAhrsPlus::ascii_rsp_calib;
1858  ascii_handler_rsp_map[std::string("~factory")] = &iMyAhrsPlus::ascii_rsp_factory;
1859  ascii_handler_rsp_map[std::string("~stat")] = &iMyAhrsPlus::ascii_rsp_stat;
1860  ascii_handler_rsp_map[std::string("~version")] = &iMyAhrsPlus::ascii_rsp_version;
1861  ascii_handler_rsp_map[std::string("~id")] = &iMyAhrsPlus::ascii_rsp_id;
1862  ascii_handler_rsp_map[std::string("~sn")] = &iMyAhrsPlus::ascii_rsp_serial_number;
1863  ascii_handler_rsp_map[std::string("~sensitivity")] = &iMyAhrsPlus::ascii_rsp_sensitivity;
1864  ascii_handler_rsp_map[std::string("~baudrate")] = &iMyAhrsPlus::ascii_rsp_baudrate;
1865  ascii_handler_rsp_map[std::string("~save")] = &iMyAhrsPlus::ascii_rsp_save;
1866 
1867  // data message (ascii)
1868  ascii_handler_data_map[std::string("$RPY")] = &iMyAhrsPlus::ascii_update_euler;
1869  ascii_handler_data_map[std::string("$QUAT")] = &iMyAhrsPlus::ascii_update_quaternion;
1870  ascii_handler_data_map[std::string("$RPYIMU")] = &iMyAhrsPlus::ascii_update_rpyimu;
1871  ascii_handler_data_map[std::string("$QUATIMU")] = &iMyAhrsPlus::ascii_update_quatimu;
1872  ascii_handler_data_map[std::string("$RIIMU")] = &iMyAhrsPlus::ascii_update_riimu;
1873  ascii_handler_data_map[std::string("$IMU")] = &iMyAhrsPlus::ascii_update_imu;
1874 
1875  // data node (binary)
1876  static const char* NAME_DATA_ROOT = "d";
1877  static const char* NAME_RIIMU = "r";
1878  static const char* NAME_IMU = "i";
1879  static const char* NAME_EULER = "e";
1880  static const char* NAME_QUATERNION = "q";
1881  static const char* NAME_DCM = "c";
1882  static const char* NAME_SEQUANCE = "s";
1883 
1884  binary_handler_data_map[std::string(NAME_SEQUANCE)] = &iMyAhrsPlus::binary_update_sequence;
1885  binary_handler_data_map[std::string(NAME_EULER)] = &iMyAhrsPlus::binary_update_euler;
1886  binary_handler_data_map[std::string(NAME_QUATERNION)] = &iMyAhrsPlus::binary_update_quaternion;
1887  binary_handler_data_map[std::string(NAME_IMU)] = &iMyAhrsPlus::binary_update_imu;
1888  binary_handler_data_map[std::string(NAME_RIIMU)] = &iMyAhrsPlus::binary_update_riimu;
1889 
1890  thread_event.start(thread_proc_callback, (void*)this);
1891  }
1892 
1893  virtual ~iMyAhrsPlus() {
1894  event_queue.push_event_exit();
1895  stop();
1896  thread_event.join();
1897  }
1898 
1899  const char* sdk_version() {
1901  }
1902 
1903  bool start(std::string port_name="", int baudrate=-1) {
1904  {
1905  LockGuard _l(mutex_communication);
1906  if(port_name == "" || baudrate < 0) {
1907  if(serial.Open() == false) {
1908  return false;
1909  }
1910  }
1911  else {
1912  if(serial.Open(port_name.c_str(), baudrate) == false) {
1913  return false;
1914  }
1915  }
1916 
1917  serial.Flush();
1918 
1919  thread_receiver_ready = false;
1920  if(thread_receiver.start(thread_proc_receiver, (void*)this) == false) {
1921  return false;
1922  }
1923 
1924  while(thread_receiver_ready == false) {
1925  Platform::msleep(10);
1926  }
1927  }
1928 
1929  for(int i=0; i<3; i++) {
1930  cmd_ping(1000);
1931  }
1932 
1933  activate_user_event = resync();
1934  return activate_user_event;
1935  }
1936 
1937  void stop() {
1938  LockGuard _l(mutex_communication);
1939  clear_all_attribute();
1940  activate_user_event = false;
1941  serial.Close();
1942  thread_receiver.join();
1943  }
1944 
1945  inline int get_sensor_id() {
1946  LockGuard _l(mutex_attribute);
1947  return sensor_id;
1948  }
1949 
1951  LockGuard _l(mutex_attribute);
1952  return sensor_data;
1953  }
1954 
1955  inline void get_data(SensorData& data) {
1956  LockGuard _l(mutex_attribute);
1957  data = sensor_data;
1958  }
1959 
1960  /*
1961  * access attributes
1962  */
1963  bool get_attribute(const char* attrib_name, std::string& attrib_value) {
1964  LockGuard _l(mutex_attribute);
1965 
1966  std::string attribute_name(attrib_name);
1967  std::map<std::string, std::string>::iterator it = attribute_map.find(attribute_name);
1968  if(it != attribute_map.end()) {
1969  attrib_value = it->second;
1970  return true;
1971  }
1972  else {
1973  return false;
1974  }
1975  }
1976 
1977  private:
1978  bool is_exist(const std::string& attribute_name) {
1979  std::map<std::string, std::string>::iterator it = attribute_map.find(attribute_name);
1980  if(it != attribute_map.end()) {
1981  return true;
1982  }
1983  else {
1984  return false;
1985  }
1986  }
1987 
1988  void set_attribute(const std::string& attribute_name, const std::string& value) {
1989  attribute_map[attribute_name] = value;
1990 
1991  if(activate_user_event) {
1992  std::string attr_name = attribute_name;
1993  std::string attr_value = value;
1994  event_queue.push_event_attribute_change(attr_name, attr_value);
1995  }
1996  }
1997 
1999  LockGuard _l(mutex_attribute);
2000  attribute_map.clear();
2001  sensor_id = -1;
2002  }
2003 
2004  public:
2005  // ########################################################################################################################
2006  // # accel_bias : -1.334012e+01, 2.941270e+01, 1.579460e+02
2007  // # accel_max : 16
2008  // # accel_sensitivity : 4.882813e-04
2009  // # accel_sensitivity_matrix : 4.888283e-04, -1.201399e-06, -4.818384e-06, 0.000000e+00, 4.901073e-04, -1.257056e-06, 0.000000e+00, 0.000000e+00, 4.853439e-04
2010  // # ascii_format : QUATIMU
2011  // # baudrate : 115200
2012  // # binary_format : QUATERNION, IMU
2013  // # build_date : Jul 12 2014 18:55:53
2014  // # coordinate_transformation_global_to_user : 0.000000e+00, 0.000000e+00, 0.000000e+00, 1.000000e+00
2015  // # coordinate_transformation_sensor_to_vehicle : 0.000000e+00, 0.000000e+00, 0.000000e+00, 1.000000e+00
2016  // # divider : 1
2017  // # firmware_version : 1.5
2018  // # gyro_bias : -1.633780e+01, -7.488200e+00, -1.367940e+01
2019  // # gyro_max : 2000
2020  // # gyro_sensitivity : 6.097561e-02
2021  // # gyro_sensitivity_matrix : 6.128651e-02, -2.204396e-04, -9.754782e-04, 1.441085e-05, 6.082033e-02, 5.099393e-04, -1.803549e-04, 7.779496e-04, 6.148182e-02
2022  // # magnet_bias : 9.936985e+01, 1.039952e+01, -9.331067e+01
2023  // # magnet_sensitivity_matrix : 6.128308e-01, 1.720049e-03, 9.577197e-03, 1.720049e-03, 5.859380e-01, 1.020694e-03, 9.577197e-03, 1.020694e-03, 6.279327e-01
2024  // # max_rate : 100
2025  // # mode : BT
2026  // # platform : myAhrsRevC
2027  // # product_name : myAHRS+
2028  // # sensor_id : 0
2029  // # sensor_serial_number : 464432970808430886
2030  // # yaw_offset : OFF
2031  // ########################################################################################################################
2032 
2033  std::vector<std::string> get_attribute_list() {
2034  LockGuard _l(mutex_attribute);
2035  std::vector<std::string> attribute_list;
2036  for(std::map<std::string, std::string>::iterator it=attribute_map.begin(); it!=attribute_map.end(); it++) {
2037  attribute_list.push_back(it->first);
2038  }
2039  return attribute_list;
2040  }
2041 
2042  /*
2043  * re-sync all attributes
2044  */
2045  bool resync() {
2046  bool ok = false;
2047  std::string mode;
2048 
2049  do {
2050  if(cmd_mode() == false) {
2051  DBG_PRINTF(true, "cmd_mode() returns false\n");
2052  break;
2053  }
2054  if(get_attribute("mode", mode) == false) {
2055  DBG_PRINTF(true, "get_attribute('mode') returns false\n");
2056  break;
2057  }
2058  if(cmd_mode("T") == false) {
2059  DBG_PRINTF(true, "cmd_mode(T) returns false\n");
2060  break;
2061  }
2062  if(cmd_divider() == false) {
2063  DBG_PRINTF(true, "cmd_divider() returns false\n");
2064  break;
2065  }
2066  if(cmd_ascii_data_format() == false) {
2067  DBG_PRINTF(true, "cmd_ascii_data_format() returns false\n");
2068  break;
2069  }
2070  if(cmd_binary_data_format() == false) {
2071  DBG_PRINTF(true, "cmd_binary_data_format() returns false\n");
2072  break;
2073  }
2074  if(cmd_set_user_orientation_offset() == false) {
2075  DBG_PRINTF(true, "cmd_set_user_orientation_offset() returns false\n");
2076  break;
2077  }
2078  if(cmd_calibration_parameter('A') == false) {
2079  DBG_PRINTF(true, "cmd_calibration_parameter(A) returns false\n");
2080  break;
2081  }
2082  if(cmd_calibration_parameter('G') == false) {
2083  DBG_PRINTF(true, "cmd_calibration_parameter(G) returns false\n");
2084  break;
2085  }
2086  if(cmd_calibration_parameter('M') == false) {
2087  DBG_PRINTF(true, "cmd_calibration_parameter(M) returns false\n");
2088  break;
2089  }
2090  if(cmd_version() == false) {
2091  DBG_PRINTF(true, "cmd_version() returns false\n");
2092  break;
2093  }
2094  if(cmd_id() == false) {
2095  DBG_PRINTF(true, "cmd_id() returns false\n");
2096  break;
2097  }
2098  if(cmd_sensitivity() == false) {
2099  DBG_PRINTF(true, "cmd_sensitivity() returns false\n");
2100  break;
2101  }
2102  if(cmd_baudrate() == false) {
2103  DBG_PRINTF(true, "cmd_baudrate() returns false\n");
2104  break;
2105  }
2106  if(cmd_mode(mode.c_str()) == false) {
2107  DBG_PRINTF(true, "cmd_mode(restore) returns false\n");
2108  break;
2109  }
2110 
2111  ok = true;
2112  }while(0);
2113 
2114  if(ok == true && debug) {
2115  DBG_PRINTF(debug, "\n\n### ATTRIBUTES #####\n");
2116  for (std::map<std::string, std::string>::iterator it=attribute_map.begin(); it!=attribute_map.end(); ++it) {
2117  DBG_PRINTF(debug, "\t- attribute(%s) = \"%s\"\n", it->first.c_str(), it->second.c_str());
2118  }
2119  }
2120 
2121  if(debug && ok == false) {
2122  Platform::msleep(100);
2123  }
2124 
2125  return ok;
2126  }
2127 
2128  /*
2129  * myahrs_plus commands
2130  */
2131  void cmd_trigger() {
2132  // no response, no wait
2133  send_command("@trig", -1);
2134  }
2135 
2136  bool cmd_ping(int timeout_msec=500) {
2137  return send_command("@ping", timeout_msec);
2138  }
2139 
2140  bool cmd_divider(int timeout_msec=500) {
2141  return send_command("@divider", timeout_msec);
2142  }
2143 
2144  bool cmd_divider(const char* divider, int timeout_msec=500) {
2145  if(strlen(divider) > 100) {
2146  return false;
2147  }
2148  char buf[128];
2149  sprintf(buf, "@divider,%s", divider);
2150  return send_command(buf, timeout_msec);
2151  }
2152 
2153  bool cmd_mode(const char* mode_string=0, int timeout_msec=500) {
2154  if(mode_string == 0) {
2155  return send_command("@mode", timeout_msec);
2156  }
2157  else {
2158  std::string command = std::string("@mode,") + std::string(mode_string);
2159  return send_command(command.c_str(), timeout_msec);
2160  }
2161  }
2162 
2163  bool cmd_ascii_data_format(const char* asc_output=0, int timeout_msec=500) {
2164  if(asc_output == 0) {
2165  return send_command("@asc_out", timeout_msec);
2166  }
2167  else {
2168  std::string command = std::string("@asc_out,") + std::string(asc_output);
2169  return send_command(command.c_str(), timeout_msec);
2170  }
2171  }
2172 
2173  bool cmd_binary_data_format(const char* bin_output=0, int timeout_msec=500) {
2174  if(bin_output == 0) {
2175  return send_command("@bin_out", timeout_msec);
2176  }
2177  else {
2178  std::string command = std::string("@bin_out,") + std::string(bin_output);
2179  return send_command(command.c_str(), timeout_msec);
2180  }
2181  }
2182 
2183  bool cmd_set_user_orientation_offset(int timeout_msec=500) {
2184  return send_command("@set_offset", timeout_msec);
2185  }
2186 
2187  bool cmd_set_user_orientation_offset(const char* enable_yaw_offset, int timeout_msec=500) {
2188  std::string command = std::string("@set_offset,") + std::string(enable_yaw_offset);
2189  return send_command(command.c_str(), timeout_msec);
2190  }
2191 
2192  bool cmd_clear_user_orientation_offset(int timeout_msec=500) {
2193  return send_command("@clr_offset", timeout_msec);
2194  }
2195 
2196  bool cmd_calibration_parameter(char sensor_type, const char* calibration_parameters=0, int timeout_msec=500) {
2197  char buf[512];
2198  if(calibration_parameters == 0) {
2199  sprintf(buf, "@calib,%c", sensor_type);
2200  return send_command(buf, timeout_msec);
2201  }
2202  else {
2203  if(strlen(calibration_parameters) > sizeof(buf)-20) {
2204  return false;
2205  }
2206  sprintf(buf, "@calib,%c,%s", sensor_type, calibration_parameters);
2207  return send_command(buf, timeout_msec);
2208  }
2209  }
2210 
2211  bool cmd_restore_all_default(int timeout_msec=500) {
2212  return send_command("@factory", timeout_msec);
2213  }
2214 
2215  bool cmd_version(int timeout_msec=500) {
2216  return send_command("@version", timeout_msec);
2217  }
2218 
2219  bool cmd_id(int timeout_msec=500) {
2220  return send_command("@id", timeout_msec);
2221  }
2222 
2223  bool cmd_id(const char* str_sensor_id, int timeout_msec=500) {
2224  if(strlen(str_sensor_id) > 100) {
2225  return false;
2226  }
2227  char buf[128];
2228  sprintf(buf, "@id,%s", str_sensor_id);
2229  return send_command(buf, timeout_msec);
2230  }
2231 
2232  bool cmd_serial_number(int timeout_msec=500) {
2233  return send_command("@sn", timeout_msec);
2234  }
2235 
2236  bool cmd_sensitivity(int timeout_msec=500) {
2237  return send_command("@sensitivity", timeout_msec);
2238  }
2239 
2240  bool cmd_baudrate(int timeout_msec=500) {
2241  return send_command("@baudrate", timeout_msec);
2242  }
2243 
2244  bool cmd_baudrate(const char* baudrate, int timeout_msec=500) {
2245  if(strlen(baudrate) > 100) {
2246  return false;
2247  }
2248  char buf[128];
2249  sprintf(buf, "@baudrate,%s", baudrate);
2250  return send_command(buf, timeout_msec);
2251  }
2252 
2253  bool cmd_save(int timeout_msec=500) {
2254  return send_command("@save", timeout_msec);
2255  }
2256 
2257  protected:
2258  /*
2259  * Event handler interface
2260  */
2261  virtual void OnSensorData(int sensor_id, SensorData sensor_data) {}
2262  virtual void OnAttributeChange(int sensor_id, std::string attribute_name, std::string value) {}
2263 
2264  private:
2265  bool send_command(std::string command_string, int timeout_msec) {
2266  LockGuard _l(mutex_communication);
2267 
2268  DBG_PRINTF(debug, "### SEND : %s\n", command_string.c_str());
2269 
2270  uint8_t crc = 0;
2271  char crc_string[16];
2272  for(size_t i=0; i<command_string.length(); i++) {
2273  crc ^= command_string[i];
2274  }
2275  sprintf(crc_string, "*%02X\r\n", crc);
2276  std::string command_with_crc = command_string + std::string(crc_string);
2277 
2278  /*
2279  * send command
2280  */
2281  response_message_queue.clear();
2282  if(serial.Write((unsigned char*)command_with_crc.c_str(), command_with_crc.length()) <= 0) {
2283  DBG_PRINTF(debug, "serial.Write() return false\n");
2284  return false;
2285  }
2286 
2287  if(timeout_msec < 0) {
2288  // no wait
2289  return true;
2290  }
2291 
2292  std::vector<std::string> tokens;
2293  std::vector<std::string> cmd_tokens;
2294 
2295  do {
2296  /*
2297  * wait for response
2298  */
2299  DBG_PRINTF(debug, "### WAITING RESPONSE\n");
2300  if(response_message_queue.wait(timeout_msec) == false) {
2301  DBG_PRINTF(debug, "TIMEOUT!!(%d)\n", timeout_msec);
2302  return false;
2303  }
2304 
2305  #ifdef WIN32
2306  /*
2307  * stupid windows...
2308  */
2309  while(response_message_queue.size() == 0);
2310  #endif // WIN32
2311 
2312  /*
2313  * check response
2314  */
2315  tokens.clear();
2316  if(response_message_queue.pop(tokens) == false) {
2317  if(debug) {
2318  Platform::msleep(10);
2319  DBG_PRINTF(debug, "ERROR??? : response_queue.pop() returns false (response_queue.size() %lu, cmd=%s)\n", response_message_queue.size(), command_string.c_str());
2320  }
2321  return false;
2322  }
2323 
2324  cmd_tokens.clear();
2325  StringUtil::split(cmd_tokens, command_string.c_str(), ',');
2326  StringUtil::replace(cmd_tokens[0], "@", "~");
2327  if(tokens[0] == cmd_tokens[0]) {
2328  DBG_PRINTF(debug, "### RCV OK(%s)\n", cmd_tokens[0].c_str());
2329  break;
2330  }
2331  else {
2332  DBG_PRINTF(debug, "ERROR: invalid response. command %s, response %s)\n", cmd_tokens[0].c_str(), tokens[0].c_str());
2333  continue;
2334  }
2335  }while(1);
2336 
2337 
2338  /*
2339  * handle error response
2340  */
2341  if(tokens[1] != std::string("OK")) {
2342  // print error message
2343  DBG_PRINTF(debug, "ERROR: status is not OK. command %s)\n", cmd_tokens[0].c_str());
2344  return false;
2345  }
2346 
2347  /*
2348  * run response handler
2349  */
2350  std::map<std::string, AscRspHandler>::iterator it = ascii_handler_rsp_map.find(tokens[0]);
2351  if(it != ascii_handler_rsp_map.end()) {
2352  std::map<std::string, std::string> attributes;
2353  bool res = true;
2354  if(StringUtil::extract_attributes(attributes, tokens) > 0) {
2355  LockGuard _l(mutex_attribute);
2356  res = (this->*(it->second))(attributes);
2357  if(res == false) {
2358  DBG_PRINTF(debug, "ERROR: message hander returns false. command %s)\n", cmd_tokens[0].c_str());
2359  }
2360  }
2361 
2362  if(res) {
2363  DBG_PRINTF(debug, "### OK : message hander returns true. command %s, rcvq_sz %d)\n", cmd_tokens[0].c_str(), response_message_queue.size());
2364  }
2365 
2366  return res;
2367  }
2368  else {
2369  return false;
2370  }
2371  }
2372 
2373  /*
2374  * threads
2375  */
2376  void proc_receiver() {
2377  int len;
2378  unsigned char buffer[1024];
2379 
2380  thread_receiver_ready = true;
2381 
2382  while(true) {
2383  memset(buffer, 0, sizeof(buffer));
2384  len = serial.Read(buffer, sizeof(buffer)-1);
2385  if(len == 0) {
2386  Platform::msleep(1);
2387  continue;
2388  }
2389  else if(len < 0) {
2390  break; // stop thread
2391  }
2392  else if(len > 0) {
2393  DBG_PRINTF(debug, "### SZ(%d) [%s]\n", len, buffer);
2394  protocol.feed(buffer, len);
2395  }
2396  }
2397 
2398  DBG_PRINTF(debug, "### %s() exit\n", __FUNCTION__);
2399  }
2400 
2401  void proc_callback() {
2402  EventItem* event;
2403  bool exit_thread = false;
2404 
2405  while(exit_thread == false) {
2406  event = event_queue.pop_event();
2407  if(event == 0) {
2408  event_queue.wait();
2409  continue;
2410  }
2411 
2412  try {
2413  switch(event->event_id) {
2414  case EventItem::EXIT:
2415  DBG_PRINTF(debug, "receive EventItem::EXIT\n");
2416  exit_thread = true;
2417  break;
2418  case EventItem::ATTRIBUTE: {
2419  std::map<std::string, std::string>* attribute_pair = event->get_attribute();
2420  for(std::map<std::string, std::string>::iterator it=attribute_pair->begin(); it!=attribute_pair->end(); it++) {
2421  OnAttributeChange(sensor_id, it->first, it->second);
2422  }
2423  }
2424  break;
2425  case EventItem::DATA: {
2426  SensorData* sensor_data = event->get_sensor_data();
2427  OnSensorData(sensor_id, *sensor_data);
2428  }
2429  break;
2430  default:
2431  break;
2432  }
2433  }
2434  catch(...) {}
2435 
2436  delete event;
2437  } // while(exit_thread == false)
2438 
2439  DBG_PRINTF(debug, "### %s() exit\n", __FUNCTION__);
2440  }
2441 
2442  static void* thread_proc_receiver(void* arg) {
2443  ((iMyAhrsPlus*)arg)->proc_receiver();
2444  return 0;
2445  }
2446 
2447  static void* thread_proc_callback(void* arg) {
2448  ((iMyAhrsPlus*)arg)->proc_callback();
2449  return 0;
2450  }
2451 
2452 
2453  /*
2454  * ascii response handlers
2455  */
2456  bool ascii_parse_response(std::vector<std::string>& tokens) {
2457  if(tokens[0][0] == iAsciiProtocol::MSG_HDR_RESPONSE) {
2458  // message parsing will be delegated to send_command()
2459  response_message_queue.push_back(tokens);
2460  return true;
2461  }
2462  else {
2463  LockGuard _l(mutex_attribute);
2464 
2465  sensor_data.reset();
2466  bool res = false;
2467 
2468  std::map<std::string, AscHandler>::iterator it = ascii_handler_data_map.find(tokens[0]);
2469  if(it != ascii_handler_data_map.end()) {
2470  res = (this->*(it->second))(tokens);
2471  }
2472 
2473  if(res && activate_user_event) {
2474  event_queue.push_event_data(sensor_data);
2475  }
2476 
2477  return res;
2478  }
2479  }
2480 
2481  void dbg_show_all_attributes(std::map<std::string, std::string>& attributes) {
2482  for (std::map<std::string, std::string>::iterator it=attributes.begin(); it!=attributes.end(); ++it) {
2483  DBG_PRINTF(debug, " --- %s : %s\n", (it->first).c_str(), (it->second).c_str());
2484  }
2485  }
2486 
2487  bool ascii_rsp_trigger(std::map<std::string, std::string>& attributes) {
2488  return true;
2489  }
2490 
2491  bool ascii_rsp_ping(std::map<std::string, std::string>& attributes) {
2492  return true;
2493  }
2494 
2495  bool ascii_rsp_divider(std::map<std::string, std::string>& attributes) {
2496  dbg_show_all_attributes(attributes);
2497 
2498  set_attribute("divider", attributes["divider"]);
2499  set_attribute("max_rate", attributes["max_rate"]);
2500 
2501  return true;
2502  }
2503 
2504  bool ascii_rsp_mode(std::map<std::string, std::string>& attributes) {
2505  dbg_show_all_attributes(attributes);
2506 
2507  set_attribute("mode", attributes["mode"]);
2508 
2509  return true;
2510  }
2511 
2512  bool ascii_rsp_asc_out(std::map<std::string, std::string>& attributes) {
2513  dbg_show_all_attributes(attributes);
2514 
2515  set_attribute("ascii_format", attributes["fmt"]);
2516 
2517  return true;
2518  }
2519 
2520  bool ascii_rsp_bin_out(std::map<std::string, std::string>& attributes) {
2521  dbg_show_all_attributes(attributes);
2522 
2523  std::vector<std::string> prop_bin_format;
2524  StringUtil::split(prop_bin_format, attributes["fmt"].c_str(), ' ');
2525  set_attribute("binary_format", StringUtil::join(prop_bin_format, ", "));
2526 
2527  return true;
2528  }
2529 
2530  bool ascii_rsp_user_orientation(std::map<std::string, std::string>& attributes) {
2531  dbg_show_all_attributes(attributes);
2532 
2533  // ~set_offset,OK,yaw_offset=OFF,q_s2v=0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00,q_g2u=0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00*12
2534 
2535  set_attribute("yaw_offset", attributes["yaw_offset"]);
2536 
2537  std::vector<std::string> parameters;
2538 
2539  if(StringUtil::split(parameters, attributes["q_s2v"].c_str(), ' ') != 4) {
2540  return false;
2541  }
2542  set_attribute("coordinate_transformation_sensor_to_vehicle", StringUtil::join(parameters, ", "));
2543 
2544  if(StringUtil::split(parameters, attributes["q_g2u"].c_str(), ' ') != 4) {
2545  return false;
2546  }
2547  set_attribute("coordinate_transformation_global_to_user", StringUtil::join(parameters, ", "));
2548 
2549  return true;
2550  }
2551 
2552  bool ascii_rsp_calib(std::map<std::string, std::string>& attributes) {
2553  dbg_show_all_attributes(attributes);
2554 
2555  // ~calib,OK,sensor=A,param=4.882813e-04 0.000000e+00 0.000000e+00 0.000000e+00 4.882813e-04 0.000000e+00 0.000000e+00 0.000000e+00 4.882813e-04 0.000000e+00 0.000000e+00 0.000000e+00*0E
2556 
2557  std::string sensor = attributes["sensor"];
2558 
2559  switch((char)sensor[0]) {
2560  case 'A':
2561  sensor = "accel";
2562  break;
2563  case 'G':
2564  sensor = "gyro";
2565  break;
2566  case 'M':
2567  sensor = "magnet";
2568  break;
2569  default:
2570  return false;
2571  }
2572 
2573  std::vector<std::string> parameters;
2574  if(StringUtil::split(parameters, attributes["param"].c_str(), ' ') != 12) {
2575  return false;
2576  }
2577 
2578  std::vector<std::string> transform(9, "");
2579  std::vector<std::string> bias(3, "");
2580 
2581  std::copy_n(parameters.begin(), 9, transform.begin());
2582  std::copy_n(parameters.begin()+9, 3, bias.begin());
2583 
2584  set_attribute(sensor + "_calibration_matrix", StringUtil::join(transform, ", "));
2585  set_attribute(sensor + "_bias", StringUtil::join(bias, ", "));
2586 
2587  return true;
2588  }
2589 
2590  bool ascii_rsp_factory(std::map<std::string, std::string>& attributes) {
2591  dbg_show_all_attributes(attributes);
2592 
2593  return true;
2594  }
2595 
2596  bool ascii_rsp_stat(std::map<std::string, std::string>& attributes) {
2597  dbg_show_all_attributes(attributes);
2598 
2599  return true;
2600  }
2601 
2602  bool ascii_rsp_version(std::map<std::string, std::string>& attributes) {
2603  dbg_show_all_attributes(attributes);
2604 
2605  set_attribute("build_date", attributes["build"]);
2606  set_attribute("platform", attributes["platform"]);
2607  set_attribute("product_name", attributes["product"]);
2608  set_attribute("sensor_serial_number", attributes["sn"]);
2609  set_attribute("firmware_version", attributes["ver"]);
2610 
2611  return true;
2612  }
2613 
2614  bool ascii_rsp_id(std::map<std::string, std::string>& attributes) {
2615  dbg_show_all_attributes(attributes);
2616 
2617  std::string str_sensor_id = attributes["id"];
2618  set_attribute("sensor_id", str_sensor_id);
2619  sensor_id = atoi(str_sensor_id.c_str());
2620 
2621  return true;
2622  }
2623 
2624  bool ascii_rsp_serial_number(std::map<std::string, std::string>& attributes) {
2625  dbg_show_all_attributes(attributes);
2626 
2627  set_attribute("sensor_serial_number", attributes["sn"]);
2628 
2629  return true;
2630  }
2631 
2632  bool ascii_rsp_sensitivity(std::map<std::string, std::string>& attributes) {
2633  dbg_show_all_attributes(attributes);
2634 
2635  set_attribute("accel_max", attributes["acc_range"]);
2636  set_attribute("gyro_max", attributes["gyro_range"]);
2637  set_attribute("accel_sensitivity", attributes["acc_sensitivity"]);
2638  set_attribute("gyro_sensitivity", attributes["gyro_sensitivity"]);
2639 
2640  return true;
2641  }
2642 
2643  bool ascii_rsp_baudrate(std::map<std::string, std::string>& attributes) {
2644  dbg_show_all_attributes(attributes);
2645 
2646  set_attribute("baudrate", attributes["baudrate"]);
2647 
2648  return true;
2649  }
2650 
2651  bool ascii_rsp_save(std::map<std::string, std::string>& attributes) {
2652  dbg_show_all_attributes(attributes);
2653 
2654  return true;
2655  }
2656 
2657 
2658  /*
2659  * ascii data message handlers
2660  */
2661 
2662  // $RPY,04,-1.55,-1.25,96.94*50
2663  bool ascii_update_euler(std::vector<std::string>& tokens) {
2664  static const int DATA_NUM = 3;
2665  if(tokens.size() != DATA_NUM + 2) {
2666  return false;
2667  }
2668 
2669  sensor_data.sequence_number = atoi(tokens[1].c_str());
2670 
2671  std::vector<std::string> str_euler(DATA_NUM, "");
2672  std::copy_n(tokens.begin() + 2, DATA_NUM, str_euler.begin());
2673 
2674  EulerAngle e;
2675  e.set(str_euler);
2676  sensor_data.update_attitude(e);
2677 
2678  DBG_PRINTF(debug, "### %s(%s)\n", __FUNCTION__, StringUtil::join(str_euler, ", ").c_str());
2679 
2680  return true;
2681  }
2682 
2683  // $QUAT,68,0.0006,0.0174,-0.7489,-0.6625*16
2684  bool ascii_update_quaternion(std::vector<std::string>& tokens) {
2685  static const int DATA_NUM = 4;
2686  if(tokens.size() != DATA_NUM + 2) {
2687  return false;
2688  }
2689 
2690  sensor_data.sequence_number = atoi(tokens[1].c_str());
2691 
2692  std::vector<std::string> str_quat(DATA_NUM, "");
2693  std::copy_n(tokens.begin() + 2, DATA_NUM, str_quat.begin());
2694 
2695  Quaternion q;
2696  q.set(str_quat);
2697  sensor_data.update_attitude(q);
2698 
2699  DBG_PRINTF(debug, "### %s(%s)\n", __FUNCTION__, StringUtil::join(str_quat, ", ").c_str());
2700 
2701  return true;
2702  }
2703 
2704  // $RPYIMU,15,-1.55,-1.25,97.31,-0.0142,-0.0010,-0.9224,-0.9756,-0.3659,-0.8537,-8.4000,-46.8000,5.4000,38.3*36
2705  bool ascii_update_rpyimu(std::vector<std::string>& tokens) {
2706  static const int DATA_NUM_EULER = 3;
2707  static const int DATA_NUM_IMU = 10;
2708  if(tokens.size() != DATA_NUM_EULER + DATA_NUM_IMU + 2) {
2709  return false;
2710  }
2711 
2712  sensor_data.sequence_number = atoi(tokens[1].c_str());
2713 
2714  std::vector<std::string> str_euler(DATA_NUM_EULER, "");
2715  std::copy_n(tokens.begin() + 2, DATA_NUM_EULER, str_euler.begin());
2716 
2717  EulerAngle e;
2718  e.set(str_euler);
2719  sensor_data.update_attitude(e);
2720 
2721  std::vector<std::string> str_imu(DATA_NUM_IMU, "");
2722  std::copy_n(tokens.begin() + 2 + DATA_NUM_EULER, DATA_NUM_IMU, str_imu.begin());
2723 
2724  ImuData<float> imu;
2725  imu.set(str_imu);
2726  sensor_data.update_imu(imu);
2727 
2728  DBG_PRINTF(debug, "### %s(euler=(%s), imu=(%s))\n", __FUNCTION__, StringUtil::join(str_euler, ", ").c_str(), StringUtil::join(str_imu, ", ").c_str());
2729 
2730  return true;
2731  }
2732 
2733  // $QUATIMU,53,0.0424,-0.1791,0.2366,0.9540,-0.3636,0.0027,-0.9260,0.0156,0.1537,0.2896,212.2648,-72.7573,168.2144,36.8*7F
2734  bool ascii_update_quatimu(std::vector<std::string>& tokens) {
2735  static const int DATA_NUM_QUAT = 4;
2736  static const int DATA_NUM_IMU = 10;
2737  if(tokens.size() != DATA_NUM_QUAT + DATA_NUM_IMU + 2) {
2738  return false;
2739  }
2740 
2741  sensor_data.sequence_number = atoi(tokens[1].c_str());
2742 
2743  std::vector<std::string> str_quat(DATA_NUM_QUAT, "");
2744  std::copy_n(tokens.begin() + 2, DATA_NUM_QUAT, str_quat.begin());
2745 
2746  Quaternion q;
2747  q.set(str_quat);
2748  sensor_data.update_attitude(q);
2749 
2750  std::vector<std::string> str_imu(DATA_NUM_IMU, "");
2751  std::copy_n(tokens.begin() + 2 + DATA_NUM_QUAT, DATA_NUM_IMU, str_imu.begin());
2752 
2753  ImuData<float> imu;
2754  imu.set(str_imu);
2755  sensor_data.update_imu(imu);
2756 
2757  DBG_PRINTF(debug, "### %s(quaternion=(%s), imu=(%s))\n", __FUNCTION__, StringUtil::join(str_quat, ", ").c_str(), StringUtil::join(str_imu, ", ").c_str());
2758 
2759 
2760  return true;
2761  }
2762 
2763  // $RIIMU,59,-16,-8,-1897,-14,-7,-12,-26,-156,18,1101*79
2764  bool ascii_update_riimu(std::vector<std::string>& tokens) {
2765  static const int DATA_NUM = 10;
2766  if(tokens.size() != DATA_NUM + 2) {
2767  return false;
2768  }
2769 
2770  sensor_data.sequence_number = atoi(tokens[1].c_str());
2771 
2772  std::vector<std::string> str_imu(DATA_NUM, "");
2773  std::copy_n(tokens.begin() + 2, DATA_NUM, str_imu.begin());
2774 
2775  ImuData<int> imu_rawdata;
2776  imu_rawdata.set(str_imu);
2777  sensor_data.update_imu(imu_rawdata);
2778 
2779  DBG_PRINTF(debug, "### %s(%s)\n", __FUNCTION__, StringUtil::join(str_imu, ", ").c_str());
2780 
2781  return true;
2782  }
2783 
2784  // $IMU,74,-0.0054,-0.0015,-0.9204,-0.7317,-0.4878,-0.7317,-7.2000,-45.6000,6.6000,38.2*68
2785  bool ascii_update_imu(std::vector<std::string>& tokens) {
2786  static const int DATA_NUM = 10;
2787  if(tokens.size() != DATA_NUM + 2) {
2788  return false;
2789  }
2790 
2791  sensor_data.sequence_number = atoi(tokens[1].c_str());
2792 
2793  std::vector<std::string> str_imu(DATA_NUM, "");
2794  std::copy_n(tokens.begin() + 2, DATA_NUM, str_imu.begin());
2795 
2796  ImuData<float> imu;
2797  imu.set(str_imu);
2798  sensor_data.update_imu(imu);
2799 
2800  DBG_PRINTF(debug, "### %s(%s)\n", __FUNCTION__, StringUtil::join(str_imu, ", ").c_str());
2801 
2802  return true;
2803  }
2804 
2805 
2806  /*
2807  * binary data message handlers
2808  */
2809  bool binary_parse_response(std::vector<iNodeParser::Node>& node_list) {
2810  LockGuard _l(mutex_attribute);
2811 
2812  sensor_data.reset();
2813 
2814  if(debug) {
2815  for(size_t i=0; i<node_list.size(); i++) {
2816  DBG_PRINTF(debug, "\t#### node(%lu/%lu)->name[%02X] : %s\n", i, node_list.size(), node_list[i].name[0], node_list[i].name.c_str());
2817  }
2818  }
2819 
2820  for(size_t i=0; i<node_list.size(); i++) {
2821  iNodeParser::Node& node = node_list[i];
2822  std::map<std::string, BinHandler>::iterator it = binary_handler_data_map.find(node.name);
2823  if(it != binary_handler_data_map.end()) {
2824  if((this->*(it->second))(node) == false) {
2825  return false;
2826  }
2827  }
2828  }
2829 
2830  if(activate_user_event) {
2831  event_queue.push_event_data(sensor_data);
2832  }
2833 
2834  return true;
2835  }
2836 
2837  float int16tofloat(int16_t value, float value_max) {
2838  static const int SCALE_FACTOR = 0x7FFF;
2839  double ratio = ((double)value)/SCALE_FACTOR;
2840  return (float)(ratio*value_max);
2841  }
2842 
2844  sensor_data.sequence_number = node.list[0].value.ui8;
2845 
2846  DBG_PRINTF(debug, "### binary_update_sequence(%d)\n", node.list[0].value.ui8);
2847 
2848  return true;
2849  }
2850 
2852  static const int DATA_NUM = 3;
2853  if(node.list.size() != DATA_NUM) {
2854  return false;
2855  }
2856 
2857  float roll = int16tofloat(node.list[0].value.i16, (float)EULER_RANGE);
2858  float pitch = int16tofloat(node.list[1].value.i16, (float)EULER_RANGE);
2859  float yaw = int16tofloat(node.list[2].value.i16, (float)EULER_RANGE);
2860 
2861  EulerAngle e;
2862  e.set(roll, pitch, yaw);
2863  sensor_data.update_attitude(e);
2864 
2865  DBG_PRINTF(debug, "### %s(%f, %f, %f)\n", __FUNCTION__, roll, pitch, yaw);
2866 
2867  return true;
2868  }
2869 
2871  static const int DATA_NUM = 4;
2872  if(node.list.size() != DATA_NUM) {
2873  return false;
2874  }
2875 
2876  float x = int16tofloat(node.list[0].value.i16, 1);
2877  float y = int16tofloat(node.list[1].value.i16, 1);
2878  float z = int16tofloat(node.list[2].value.i16, 1);
2879  float w = int16tofloat(node.list[3].value.i16, 1);
2880 
2881  Quaternion q;
2882  q.set(x, y, z, w);
2883  sensor_data.update_attitude(q);
2884 
2885  DBG_PRINTF(debug, "### %s(%f, %f, %f, %f)\n", __FUNCTION__, x,y,z,w);
2886 
2887  return true;
2888  }
2889 
2891  static const int DATA_NUM = 10;
2892  if(node.list.size() != DATA_NUM) {
2893  return false;
2894  }
2895 
2896  float m[DATA_NUM];
2897 
2898  m[0] = int16tofloat(node.list[0].value.i16, (float)ACCEL_RANGE);
2899  m[1] = int16tofloat(node.list[1].value.i16, (float)ACCEL_RANGE);
2900  m[2] = int16tofloat(node.list[2].value.i16, (float)ACCEL_RANGE);
2901 
2902  m[3] = int16tofloat(node.list[3].value.i16, (float)GYRO_RANGE);
2903  m[4] = int16tofloat(node.list[4].value.i16, (float)GYRO_RANGE);
2904  m[5] = int16tofloat(node.list[5].value.i16, (float)GYRO_RANGE);
2905 
2906  m[6] = int16tofloat(node.list[6].value.i16, (float)MAGNET_RANGE);
2907  m[7] = int16tofloat(node.list[7].value.i16, (float)MAGNET_RANGE);
2908  m[8] = int16tofloat(node.list[8].value.i16, (float)MAGNET_RANGE);
2909 
2910  m[9] = int16tofloat(node.list[9].value.i16, (float)TEMP_RANGE);
2911 
2912  ImuData<float> imu;
2913  imu.set(m);
2914  sensor_data.update_imu(imu);
2915 
2916  if(debug) {
2917  std::vector<std::string> str_list;
2918  StringUtil::to_string_list(str_list, m, DATA_NUM);
2919  DBG_PRINTF(debug, "### %s(%s)\n", __FUNCTION__, StringUtil::join(str_list, ", ").c_str());
2920  }
2921 
2922  return true;
2923  }
2924 
2926  static const int DATA_NUM = 10;
2927  if(node.list.size() != DATA_NUM) {
2928  return false;
2929  }
2930 
2931  int m[DATA_NUM];
2932  for(int i=0; i<DATA_NUM; i++) {
2933  m[i] = node.list[i].value.i16;
2934  }
2935 
2936  ImuData<int> imu_rawdata;
2937  imu_rawdata.set(m);
2938  sensor_data.update_imu(imu_rawdata);
2939 
2940  if(debug) {
2941  std::vector<std::string> str_list;
2942  StringUtil::to_string_list(str_list, m, DATA_NUM);
2943  DBG_PRINTF(debug, "### %s(%s)\n", __FUNCTION__, StringUtil::join(str_list, ", ").c_str());
2944  }
2945 
2946  return true;
2947  }
2948  }; // iMyAhrsPlus
2949 
2950 
2951  class MyAhrsPlus : public iMyAhrsPlus
2952  {
2955 
2956  void(*attribute_callback)(void* context, int sensor_id, const char* attribute_name, const char* value);
2958 
2959  void(*data_callback)(void* context, int sensor_id, SensorData* sensor_data);
2961 
2962  uint32_t sample_count;
2963 
2964  public:
2965  MyAhrsPlus(std::string port="", unsigned int baudrate=115200)
2966  : iMyAhrsPlus(port, baudrate), sample_count(0)
2967  , attribute_callback(0), attribute_callback_context(0)
2968  , data_callback(0), data_callback_context(0)
2969  {}
2970 
2971  virtual ~MyAhrsPlus() {
2972  attribute_callback = 0;
2973  attribute_callback_context = 0;
2974  data_callback = 0;
2975  data_callback_context = 0;
2976  }
2977 
2978  bool wait_data(int timeout_msec=500) {
2979  return event.wait(timeout_msec);
2980  }
2981 
2982  void register_attribute_callback(void(*callback)(void* context, int sensor_id, const char* attribute_name, const char* value), void* callback_context) {
2983  LockGuard _l(lock);
2984  attribute_callback_context = callback_context;
2985  attribute_callback = callback;
2986  }
2987 
2988  void register_data_callback(void(*callback)(void* context, int sensor_id, SensorData* sensor_data), void* callback_context) {
2989  LockGuard _l(lock);
2990  data_callback_context = callback_context;
2991  data_callback = callback;
2992  }
2993 
2994  inline uint32_t get_sample_count() {
2995  LockGuard _l(lock);
2996  return sample_count;
2997  }
2998 
2999  protected:
3000  void OnAttributeChange(int sensor_id, std::string attribute_name, std::string value) {
3001  LockGuard _l(lock);
3002  if(attribute_callback) {
3003  try {
3004  attribute_callback(attribute_callback_context, sensor_id, attribute_name.c_str(), value.c_str());
3005  } catch(...) {}
3006  }
3007  }
3008 
3009  void OnSensorData(int sensor_id, SensorData data) {
3010  LockGuard _l(lock);
3011  sample_count++;
3012  event.set();
3013  if(data_callback) {
3014  try {
3015  data_callback(data_callback_context, sensor_id, &data);
3016  } catch(...) {}
3017  }
3018  }
3019  };
3020 }; // WithRobot
3021 
3022 #endif // __MYAHRS_PLUS_H_
std::string to_string()
d
bool cmd_baudrate(int timeout_msec=500)
bool binary_update_sequence(iNodeParser::Node &node)
bool cmd_set_user_orientation_offset(const char *enable_yaw_offset, int timeout_msec=500)
iNodeParser(unsigned char *stream=0, size_t stream_len=0)
DirectionCosineMatrix to_dcm()
void set_attribute(const std::string &attribute_name, const std::string &value)
SerialPort(const char *port="", unsigned int brate=115200)
FilterByteStuffing(FrameBuffer &s)
double MAT(unsigned int row, unsigned int col)
void push_back(std::vector< std::string > &list)
bool ascii_update_quaternion(std::vector< std::string > &tokens)
void update_imu(ImuData< int > &i)
bool cmd_mode(const char *mode_string=0, int timeout_msec=500)
static std::string & strip(std::string &s)
std::vector< std::string > get_attribute_list()
bool binary_update_riimu(iNodeParser::Node &node)
ReturnCode state_control(uint8_t byte)
#define DBG_PRINTF(x, args...)
Definition: myahrs_plus.hpp:87
Write
DirectionCosineMatrix(double &m11, double &m12, double &m13, double &m21, double &m22, double &m23, double &m31, double &m32, double &m33)
Platform::Mutex mutex_communication
bool cmd_version(int timeout_msec=500)
void register_attribute_callback(void(*callback)(void *context, int sensor_id, const char *attribute_name, const char *value), void *callback_context)
EulerAngle to_euler_angle()
void OnAttributeChange(int sensor_id, std::string attribute_name, std::string value)
std::vector< Node > node_list
std::deque< EventItem * > deque
XmlRpcServer s
#define DEBUG_ASCII_PROTOCOL
bool get_attribute(const char *attrib_name, std::string &attrib_value)
void new_node(std::vector< Node > &node_list)
bool ascii_update_euler(std::vector< std::string > &tokens)
static std::string & ltrim(std::string &s)
bool cmd_calibration_parameter(char sensor_type, const char *calibration_parameters=0, int timeout_msec=500)
BinaryNodeParser(iBinaryProtocol *s, unsigned char *stream, size_t stream_len)
bool ascii_rsp_calib(std::map< std::string, std::string > &attributes)
ImuData< float > imu
const char * what() const
bool wait(int timeout_msec=-1)
int Read(unsigned char *buf, unsigned int buf_len)
void update_attitude(EulerAngle &e)
bool Open(const char *port, int brate)
std::vector< Varient > list
virtual void OnAttributeChange(int sensor_id, std::string attribute_name, std::string value)
static void * thread_proc_callback(void *arg)
static void * thread_proc_receiver(void *arg)
ReturnCode state_data(uint8_t byte)
bool cmd_serial_number(int timeout_msec=500)
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define WITHROBOT_MYAHRS_PLUS_SDK_VERSION
Definition: myahrs_plus.hpp:94
static std::string join(std::vector< std::string > &str_list, const std::string &delimiter)
bool ascii_rsp_mode(std::map< std::string, std::string > &attributes)
MyAhrsPlus(std::string port="", unsigned int baudrate=115200)
LockGuard(Platform::Mutex &m)
void set(double r, double p, double y)
void read_list(uint8_t value_type, std::vector< Varient > &list, size_t count)
bool ascii_rsp_stat(std::map< std::string, std::string > &attributes)
bool start(std::string port_name="", int baudrate=-1)
Platform::Thread thread_receiver
bool cmd_clear_user_orientation_offset(int timeout_msec=500)
virtual void update_attributes(std::vector< iNodeParser::Node > &node_list)=0
std::map< std::string, std::string > attribute
bool ascii_update_riimu(std::vector< std::string > &tokens)
float int16tofloat(int16_t value, float value_max)
void update_imu(ImuData< float > &i)
bool feed(unsigned char *data, int data_len)
bool ascii_rsp_baudrate(std::map< std::string, std::string > &attributes)
bool ascii_update_quatimu(std::vector< std::string > &tokens)
std::string to_string()
virtual std::map< std::string, std::string > * get_attribute()
bool ascii_rsp_divider(std::map< std::string, std::string > &attributes)
std::map< std::string, AscRspHandler > ascii_handler_rsp_map
void OnSensorData(int sensor_id, SensorData data)
bool cmd_baudrate(const char *baudrate, int timeout_msec=500)
myAhrsException(std::string e="")
bool parse_message(const char *ascii_frame)
bool binary_update_imu(iNodeParser::Node &node)
ROSLIB_DECL std::string command(const std::string &cmd)
Platform::SerialPort serial
bool cmd_sensitivity(int timeout_msec=500)
bool cmd_divider(const char *divider, int timeout_msec=500)
bool ascii_rsp_save(std::map< std::string, std::string > &attributes)
DirectionCosineMatrix(std::string str_mat, char delimiter=' ')
void push_byte(unsigned char c)
EventItemAttribute(std::map< std::string, std::string > &a)
bool cmd_binary_data_format(const char *bin_output=0, int timeout_msec=500)
bool ascii_rsp_bin_out(std::map< std::string, std::string > &attributes)
static int extract_attributes(std::map< std::string, std::string > &attribute_list, std::vector< std::string > &tokens)
bool start(void *(*thread_proc)(void *), void *arg, size_t stack_size=16 *1024)
bool binary_update_quaternion(iNodeParser::Node &node)
bool ascii_rsp_factory(std::map< std::string, std::string > &attributes)
void set(double _x, double _y, double _z, double _w)
void get_data(SensorData &data)
EulerAngle(double r=0, double p=0, double y=0)
static int split(std::vector< std::string > &token_list, const char *c_str, char delimiter, int token_max=-1)
bool ascii_update_imu(std::vector< std::string > &tokens)
virtual SensorData * get_sensor_data()
void push_event_attribute_change(std::string &attr_name, std::string &attr_value)
bool ascii_rsp_asc_out(std::map< std::string, std::string > &attributes)
TFSIMD_FORCE_INLINE const tfScalar & x() const
void push(unsigned char byte)
bool cmd_save(int timeout_msec=500)
std::map< std::string, std::string > * get_attribute()
bool binary_parse_response(std::vector< iNodeParser::Node > &node_list)
#define DEBUG_MYAHRS_PLUS
void register_data_callback(void(*callback)(void *context, int sensor_id, SensorData *sensor_data), void *callback_context)
bool ascii_update_rpyimu(std::vector< std::string > &tokens)
static void to_string_list(std::vector< std::string > &str_list, T *array, size_t array_num)
bool ascii_rsp_user_orientation(std::map< std::string, std::string > &attributes)
TFSIMD_FORCE_INLINE const tfScalar & z() const
Platform::Mutex & mutex
Platform::Event event
void dbg_show_all_attributes(std::map< std::string, std::string > &attributes)
void set(Type data[10])
static std::string & rtrim(std::string &s)
bool ascii_rsp_serial_number(std::map< std::string, std::string > &attributes)
Platform::Thread thread_event
TFSIMD_FORCE_INLINE const tfScalar & w() const
unsigned char buffer[1024]
Stream(unsigned char *s, size_t l)
Platform::Mutex mutex_attribute
bool cmd_id(const char *str_sensor_id, int timeout_msec=500)
bool ascii_parse_response(std::vector< std::string > &tokens)
EulerAngle(std::string str_rpy, char delimiter=' ')
bool cmd_divider(int timeout_msec=500)
bool cmd_set_user_orientation_offset(int timeout_msec=500)
Read
bool cmd_ping(int timeout_msec=500)
bool cmd_ascii_data_format(const char *asc_output=0, int timeout_msec=500)
std::map< std::string, BinHandler > binary_handler_data_map
bool cmd_id(int timeout_msec=500)
virtual void OnSensorData(int sensor_id, SensorData sensor_data)
Platform::Mutex lock
parser
bool ascii_rsp_ping(std::map< std::string, std::string > &attributes)
bool ascii_rsp_version(std::map< std::string, std::string > &attributes)
void update_attributes(std::vector< iNodeParser::Node > &node_list)
FilterByteStuffing filter_byte_stuffing
ImuData< int > imu_rawdata
#define DEBUG_PLATFORM
Definition: myahrs_plus.hpp:99
std::map< std::string, AscHandler > ascii_handler_data_map
bool read_value(uint8_t value_type, Varient &v)
void push_byte(unsigned char c)
bool binary_update_euler(iNodeParser::Node &node)
void update_attitude(Quaternion &q)
void update_attributes(std::vector< std::string > &tokens)
std::map< std::string, std::string > attribute_map
bool wait_data(int timeout_msec=500)
bool ascii_rsp_trigger(std::map< std::string, std::string > &attributes)
const char * sdk_version()
void push_data(uint8_t byte)
bool wait(int timeout_msec=-1)
ImuData(Type data[10])
std::string to_string()
iMyAhrsPlus(std::string port_name="", unsigned int baudrate=115200)
static void replace(std::string &src, std::string s1, std::string s2)
bool cmd_restore_all_default(int timeout_msec=500)
void push_event_data(SensorData &data)
bool ascii_rsp_id(std::map< std::string, std::string > &attributes)
static Quaternion product(Quaternion &q, Quaternion &r)
std::string to_string()
bool is_exist(const std::string &attribute_name)
Quaternion(double _x=0, double _y=0, double _z=0, double _w=1)
bool send_command(std::string command_string, int timeout_msec)
std::deque< std::vector< std::string > > queue
Quaternion(std::string str_xyzw, char delimiter=' ')
static void msleep(unsigned int msec)
#define DEBUG_BINARY_PROTOCOL
static const uint8_t MSG_HDR_RESPONSE
iMyAhrsPlus(iMyAhrsPlus &rhs)
bool pop(std::vector< std::string > &out)
bool ascii_rsp_sensitivity(std::map< std::string, std::string > &attributes)
int Write(unsigned char *data, unsigned int data_len)


myahrs_driver
Author(s): Yoonseok Pyo
autogenerated on Thu Jul 16 2020 03:08:51