46 #ifndef __MYAHRS_PLUS_H_ 47 #define __MYAHRS_PLUS_H_ 54 #define _USE_MATH_DEFINES 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 76 #define DBG_PRINTF(x, ...) {if(x) { printf(__VA_ARGS__);}} 81 #include <semaphore.h> 86 #pragma GCC diagnostic ignored "-Wformat" 87 #define DBG_PRINTF(x, args...) {if(x) { printf(args);}} 94 #define WITHROBOT_MYAHRS_PLUS_SDK_VERSION "myahrs+ sdk. ver. 1.01" 99 #define DEBUG_PLATFORM false 100 #define DEBUG_ASCII_PROTOCOL false 101 #define DEBUG_BINARY_PROTOCOL false 102 #define DEBUG_MYAHRS_PLUS false 114 const char*
what()
const throw() {
131 std::string port_name;
132 unsigned int baudrate;
138 SerialPort(
const char* port=
"COM3",
unsigned int brate=115200)
139 : port_name(port), baudrate(brate)
140 , m_hIDComDev(NULL), m_bOpened(FALSE)
148 bool Open(
const char* port,
int brate) {
156 sprintf(szPort,
"\\\\.\\%s", port_name.c_str());
157 CString port_str = CString::CStringT(CA2CT(szPort));
161 m_hIDComDev = CreateFile( (LPCTSTR)port_str,
162 GENERIC_READ | GENERIC_WRITE,
166 FILE_ATTRIBUTE_NORMAL,
169 if( m_hIDComDev == NULL ) {
176 if (!GetCommState(m_hIDComDev, &dcb)) {
178 CloseHandle( m_hIDComDev );
179 printf(
"ERROR : GetCommState()\n");
183 dcb.DCBlength =
sizeof( DCB );
184 GetCommState( m_hIDComDev, &dcb );
185 dcb.BaudRate = baudrate;
188 if(!SetCommState(m_hIDComDev, &dcb)){
190 CloseHandle( m_hIDComDev );
195 COMMTIMEOUTS CommTimeOuts;
197 CommTimeOuts.ReadIntervalTimeout = 1;
198 CommTimeOuts.ReadTotalTimeoutMultiplier = 1;
199 CommTimeOuts.ReadTotalTimeoutConstant = 1;
200 CommTimeOuts.WriteTotalTimeoutMultiplier = 1;
201 CommTimeOuts.WriteTotalTimeoutConstant = 10;
203 if(!SetCommTimeouts( m_hIDComDev, &CommTimeOuts)) {
204 CloseHandle( m_hIDComDev );
211 return (m_bOpened == TRUE);
215 if( !m_bOpened || m_hIDComDev == NULL ) {
219 CloseHandle( m_hIDComDev );
224 int Read(
unsigned char* buf,
unsigned int buf_len) {
225 if( !m_bOpened || m_hIDComDev == NULL )
return -1;
228 DWORD dwBytesRead, dwErrorFlags;
231 ClearCommError( m_hIDComDev, &dwErrorFlags, &ComStat );
233 if(ComStat.cbInQue <= 0) {
237 dwBytesRead = (DWORD) ComStat.cbInQue;
240 if( buf_len < (
int) dwBytesRead ) dwBytesRead = (DWORD) buf_len;
242 bReadStatus = ReadFile( m_hIDComDev, buf, dwBytesRead, &dwBytesRead, NULL );
251 return( (
int) dwBytesRead );
254 int Write(
unsigned char* data,
unsigned int data_len) {
256 DWORD dwBytesWritten = 0;
258 if(m_bOpened != TRUE) {
262 bWriteStat = WriteFile( m_hIDComDev, (LPSTR)data, data_len, &dwBytesWritten, NULL);
264 if (GetLastError() != ERROR_IO_PENDING) {
266 return dwBytesWritten;
272 return dwBytesWritten;
276 return dwBytesWritten;
281 int len= 0, count = 0;
282 unsigned char buf[256];
283 while((len =
Read(buf,
sizeof(buf))) > 0) {
297 InitializeCriticalSection(&cs);
301 DeleteCriticalSection(&cs);
305 EnterCriticalSection(&cs);
308 inline void unlock() {
309 LeaveCriticalSection(&cs);
319 event = CreateEvent(NULL,
329 inline bool wait(
int timeout_msec=-1) {
330 if(timeout_msec < 0) {
331 timeout_msec = INFINITE;
333 DWORD res = WaitForSingleObject(event, timeout_msec);
335 return (res == WAIT_OBJECT_0);
339 return (SetEvent(event) == TRUE);
350 Thread() : thread(NULL), thread_id(0){}
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);
359 WaitForSingleObject(thread, INFINITE);
367 static void msleep(
unsigned int msec) {
388 : port_name(port), baudrate(brate)
397 bool Open(
const char* port,
int brate) {
405 struct termios options;
411 fd = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
416 fcntl(fd, F_SETFL, 0);
417 tcgetattr(fd, &options);
419 cfsetspeed(&options, baudrate);
422 options.c_cflag |= CREAD | CLOCAL;
423 options.c_cflag |= CS8;
424 options.c_cc[VMIN] = 0;
425 options.c_cc[VTIME] = 1;
428 tcsetattr(fd, TCSANOW, &options);
431 tcflush(fd, TCIOFLUSH);
446 int Read(
unsigned char* buf,
unsigned int buf_len) {
451 int n = read(port_fd, buf, buf_len-1);
459 int Write(
unsigned char* data,
unsigned int data_len) {
464 return write(port_fd, data, data_len);
473 return tcflush(port_fd, TCIOFLUSH);
484 pthread_mutex_init(&mutex, NULL);
488 pthread_mutex_destroy(&mutex);
492 pthread_mutex_lock(&mutex);
496 pthread_mutex_unlock(&mutex);
507 pthread_mutex_init(&mutex, NULL);
508 pthread_cond_init(&cond, NULL);
512 pthread_mutex_destroy(&mutex);
513 pthread_cond_destroy(&cond);
516 inline bool wait(
int timeout_msec=-1) {
518 pthread_mutex_lock(&mutex);
519 if(timeout_msec > 0) {
523 gettimeofday(&tv, NULL);
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);
530 int n = pthread_cond_timedwait(&cond, &mutex, &ts);
534 pthread_cond_wait(&cond, &mutex);
537 pthread_mutex_unlock(&mutex);
542 pthread_mutex_lock(&mutex);
543 pthread_cond_signal(&cond);
544 pthread_mutex_unlock(&mutex);
557 bool start(
void*(*thread_proc)(
void*),
void* arg,
size_t stack_size=16*1024) {
560 pthread_attr_init(&attr);
564 pthread_attr_setstacksize(&attr, stack_size);
565 int res = pthread_create(&thread, &attr, thread_proc, (
void*)arg);
575 pthread_join(thread, NULL);
586 #endif // #ifdef WIN32 613 static void replace(std::string & src, std::string s1, std::string s2) {
617 ofs = src.find(s1, ofs);
621 src.replace(ofs, s1.length(), s2);
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))));
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());
639 static inline std::string &
strip(std::string &s) {
640 return ltrim(rtrim(s));
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);
647 bool check_max = (token_max > 0);
650 while(getline(is, token, delimiter)){
651 token_list.push_back(token);
653 if(--token_max <= 0) {
659 return token_list.size();
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++) {
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;
678 str_list.push_back(s.str());
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;
685 for(
size_t i=0; i<tokens.size(); i++) {
687 if(split(attribute_pair, tokens[i].c_str(),
'=') == 2) {
688 attribute_list[attribute_pair[0]] = attribute_pair[1];
692 return attribute_list.size();
697 unsigned char buffer[1024];
704 void push(
unsigned char byte) {
705 if(offset <
sizeof(buffer)) {
706 buffer[offset++] = byte;
711 memset(buffer, 0,
sizeof(buffer));
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';
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);
753 if(frame_buffer.
offset > 0) {
754 parse_message(reinterpret_cast<const char*>(frame_buffer.
buffer));
756 frame_buffer.
reset();
764 frame_buffer.
push(c);
768 virtual void update_attributes(std::vector<std::string>& tokens) = 0;
772 DBG_PRINTF(debug,
"## ASC FRAME : [%s]\n", ascii_frame);
774 std::vector<std::string> tokens;
779 std::string& str_message = tokens[0];
780 int crc_from_sensor = std::stoul(tokens[1].c_str(),
nullptr, 16);
782 uint8_t crc_calc = 0;
783 for(
size_t i=0; i<str_message.length(); i++) {
784 crc_calc ^= str_message[i];
787 DBG_PRINTF(debug,
"\t= MSG %s, CRC %X, CRC_C %X\n", tokens[0].c_str(), crc_from_sensor, crc_calc);
789 if(crc_calc == crc_from_sensor) {
791 update_attributes(tokens);
836 : stream(s), crc_calc(0), accumulater(0)
837 , state_receiving(false), last_state(STATE_NOP)
847 last_state = (this->*receiver)(byte);
852 return (last_state==STATE_BUSY);
857 state_receiving =
false;
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;
874 uint8_t prv_byte = ((accumulater & 0xFF00) >> 8);
875 crc_calc ^= prv_byte;
884 if(state_receiving) {
900 state_receiving =
true;
905 state_receiving =
false;
906 res = check_crc() ? STATE_COMPLETE : STATE_ERROR;
916 state_receiving =
true;
940 TAG_TYPE_FLOAT32 = 9,
941 TAG_TYPE_FLOAT64 = 10,
942 TAG_TYPE_STRING = 11,
945 TAG_HAS_LEAF_NODES = (0x01<<7),
946 TAG_NEXT_NODE_EXIST = (0x01<<6),
947 TAG_LIST_NODE = (0x01<<5),
949 TAG_TYPE_MASK = 0x0F,
969 template <
typename T>
970 bool set(uint8_t t, T v) {
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;
1007 if(pos < length) {
return buffer[pos++]; }
1012 if(pos < length) {
return buffer[pos]; }
1016 template <
typename T>
1018 T* v = (T*)(&buffer[pos]);
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;
1040 void read_list(uint8_t value_type, std::vector<Varient>& list,
size_t count) {
1042 for(
size_t i=0; i<count; i++) {
1043 if(read_value(value_type, v) ==
true) {
1050 std::string str((
const char*)&buffer[pos]);
1051 pos += str.length() + 1;
1058 for(
size_t i=0; i<length; i++) {
1072 iNodeParser(
unsigned char* stream=0,
size_t stream_len=0) : istream(stream, stream_len), debug(false){}
1077 new_node(node_list);
1080 virtual void new_node(std::vector<Node>& node_list) = 0;
1084 if(istream.
length == 0 || istream.
peek() < 0) {
1088 uint8_t tag = istream.
getc();
1089 uint8_t type_of_value = (tag & TAG_TYPE_MASK);
1094 if(tag&TAG_LIST_NODE) {
1102 node.
list.push_back(value);
1106 node_list.push_back(node);
1108 if((tag & TAG_HAS_LEAF_NODES) || (tag & TAG_NEXT_NODE_EXIST)) {
1143 return filter_byte_stuffing.
is_busy();
1146 virtual void update_attributes(std::vector<iNodeParser::Node>& node_list) = 0;
1163 bool feed(
unsigned char* data,
int data_len) {
1164 if(!data || data_len <= 0) {
1168 for(
int i=0; i<data_len; i++) {
1189 EulerAngle(
double r=0,
double p=0,
double y=0) : roll(r), pitch(p), yaw(
y) {}
1191 set(str_rpy, delimiter);
1198 inline void set(
double r,
double p,
double y) {
1199 roll=r, pitch=p, yaw=
y;
1202 inline void set(std::string str_rpy,
char delimiter=
' ') {
1203 std::vector<std::string> tokens;
1212 inline void set(std::vector<std::string>& str_array) {
1213 if(str_array.size() != 3) {
1216 roll = atof(str_array[0].c_str());
1217 pitch = atof(str_array[1].c_str());
1218 yaw = atof(str_array[2].c_str());
1222 std::stringstream
s;
1223 s << roll <<
", "<< pitch <<
", "<< yaw;
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);
1248 set(str_mat, delimiter);
1252 memset(mat, 0,
sizeof(mat));
1255 inline void set(
double dcm[9]) {
1256 memcpy(mat, dcm,
sizeof(mat));
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;
1267 inline void set(std::string str_mat,
char delimiter=
' ') {
1268 std::vector<std::string> tokens;
1277 inline void set(std::vector<std::string>& str_array) {
1278 if(str_array.size() != 9) {
1281 for(
int i=0; i<9; i++) {
1282 mat[i] = (double)atof(str_array[i].c_str());
1287 std::vector<std::string> temp;
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;
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;
1315 mat[0] = xx - yy - zz + ww;
1316 mat[1] = 2.0*(xy + zw);
1317 mat[2] = 2.0*(xz - yw);
1319 mat[3] = 2.0*(xy - zw);
1320 mat[4] = -xx + yy - zz + ww;
1321 mat[5] = 2.0*(yz + xw);
1323 mat[6] = 2.0*(xz + yw);
1324 mat[7] = 2.0*(yz - xw);
1325 mat[8] = -xx - yy + zz + ww;
1332 inline double MAT(
unsigned int row,
unsigned int col) {
1333 return mat[(row)*3 + col];
1342 Quaternion(
double _x=0,
double _y=0,
double _z=0,
double _w=1): x(_x), y(_y), z(_z), w(_w) {}
1344 set(str_xyzw, delimiter);
1351 inline void set(
double _x,
double _y,
double _z,
double _w) {
1352 x=_x, y=_y, z=_z, w=_w;
1355 inline void set(std::string str_xyzw,
char delimiter=
' ') {
1356 std::vector<std::string> tokens;
1365 inline void set(std::vector<std::string>& str_array) {
1366 if(str_array.size() != 4) {
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());
1376 std::stringstream
s;
1377 s << x <<
", "<< y <<
", "<< z <<
", "<< w;
1382 double norm = sqrt(x*x + y*y + z*z + w*w);
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;
1423 double RAD2DEG = 180/M_PI;
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;
1446 dcm.
mat[0] = xx - yy - zz + ww;
1447 dcm.
mat[1] = 2.0*(xy + zw);
1448 dcm.
mat[2] = 2.0*(xz - yw);
1450 dcm.
mat[3] = 2.0*(xy - zw);
1451 dcm.
mat[4] = -xx + yy - zz + ww;
1452 dcm.
mat[5] = 2.0*(yz + xw);
1454 dcm.
mat[6] = 2.0*(xz + yw);
1455 dcm.
mat[7] = 2.0*(yz - xw);
1456 dcm.
mat[8] = -xx - yy + zz + ww;
1472 template <
typename Type>
1478 ax = ay = az = gx = gy = gz = mx = my = mz = temperature = 0;
1487 memset(d, 0,
sizeof(d));
1491 inline void set(Type data[10]) {
1505 temperature = data[i++];
1508 inline void set(std::string str_mat,
char delimiter=
' ') {
1509 std::vector<std::string> tokens;
1518 inline void set(std::vector<std::string>& str_array) {
1519 if(str_array.size() != 10) {
1523 for(
int i=0; i<10; i++) {
1524 data[i] = (Type)atof(str_array[i].c_str());
1530 std::stringstream
s;
1531 s << ax <<
", "<<ay<<
", "<<az<<
", "<<gx<<
", "<<gy<<
", "<<gz<<
", "<<mx<<
", "<<my<<
", "<<mz<<
", "<<temperature;
1571 sequence_number = -1;
1572 euler_angle.
reset();
1574 imu_rawdata.
reset();
1601 std::vector<std::string> str_array;
1603 sprintf(temp,
"%d", sequence_number);
1604 str_array.push_back(
"sequence = " + std::string(temp));
1606 switch(attitude_type) {
1608 str_array.push_back(
"euler_angle = " + euler_angle.
to_string());
1611 str_array.push_back(
"quaternion = " + quaternion.
to_string());
1614 str_array.push_back(
"no attitude ");
1620 str_array.push_back(
"imu_raw = " + imu_rawdata.
to_string());
1623 str_array.push_back(
"imu_comp = " + imu.
to_string());
1626 str_array.push_back(
"no imu ");
1654 typedef bool (
iMyAhrsPlus::*AscHandler)(std::vector<std::string>& tokens);
1657 typedef bool (
iMyAhrsPlus::*AscRspHandler)(std::map<std::string, std::string>& attributes);
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;
1677 if(tokens.size() >= 2) {
1683 if(node_list.size() > 0) {
1693 std::deque< std::vector<std::string> >
queue;
1706 return event.wait(timeout_msec);
1715 queue.push_back(list);
1721 return queue.size();
1724 bool pop(std::vector<std::string>& out) {
1726 if(queue.size() > 0) {
1727 out = queue.front();
1735 } response_message_queue;
1788 static const size_t EVENT_MAX_NUM = 5;
1793 int num = deque.size();
1794 for(
int i=0; i<num; i++) {
1799 inline bool wait(
int timeout_msec=-1) {
1800 return event.wait(timeout_msec);
1811 std::map<std::string, std::string> attribute;
1812 attribute[attr_name] = attr_value;
1819 if(deque.size() < EVENT_MAX_NUM) {
1828 if(deque.size() > 0) {
1841 : serial(port_name.c_str(), baudrate)
1845 , activate_user_event(false)
1846 , thread_receiver_ready(false)
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";
1890 thread_event.
start(thread_proc_callback, (
void*)
this);
1894 event_queue.push_event_exit();
1896 thread_event.
join();
1903 bool start(std::string port_name=
"",
int baudrate=-1) {
1906 if(port_name ==
"" || baudrate < 0) {
1907 if(serial.
Open() ==
false) {
1912 if(serial.
Open(port_name.c_str(), baudrate) ==
false) {
1919 thread_receiver_ready =
false;
1920 if(thread_receiver.
start(thread_proc_receiver, (
void*)
this) ==
false) {
1924 while(thread_receiver_ready ==
false) {
1929 for(
int i=0; i<3; i++) {
1933 activate_user_event = resync();
1934 return activate_user_event;
1939 clear_all_attribute();
1940 activate_user_event =
false;
1942 thread_receiver.
join();
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;
1979 std::map<std::string, std::string>::iterator it = attribute_map.find(attribute_name);
1980 if(it != attribute_map.end()) {
1988 void set_attribute(
const std::string& attribute_name,
const std::string& value) {
1989 attribute_map[attribute_name] = value;
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);
2000 attribute_map.clear();
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);
2039 return attribute_list;
2050 if(cmd_mode() ==
false) {
2051 DBG_PRINTF(
true,
"cmd_mode() returns false\n");
2054 if(get_attribute(
"mode", mode) ==
false) {
2055 DBG_PRINTF(
true,
"get_attribute('mode') returns false\n");
2058 if(cmd_mode(
"T") ==
false) {
2059 DBG_PRINTF(
true,
"cmd_mode(T) returns false\n");
2062 if(cmd_divider() ==
false) {
2063 DBG_PRINTF(
true,
"cmd_divider() returns false\n");
2066 if(cmd_ascii_data_format() ==
false) {
2067 DBG_PRINTF(
true,
"cmd_ascii_data_format() returns false\n");
2070 if(cmd_binary_data_format() ==
false) {
2071 DBG_PRINTF(
true,
"cmd_binary_data_format() returns false\n");
2074 if(cmd_set_user_orientation_offset() ==
false) {
2075 DBG_PRINTF(
true,
"cmd_set_user_orientation_offset() returns false\n");
2078 if(cmd_calibration_parameter(
'A') ==
false) {
2079 DBG_PRINTF(
true,
"cmd_calibration_parameter(A) returns false\n");
2082 if(cmd_calibration_parameter(
'G') ==
false) {
2083 DBG_PRINTF(
true,
"cmd_calibration_parameter(G) returns false\n");
2086 if(cmd_calibration_parameter(
'M') ==
false) {
2087 DBG_PRINTF(
true,
"cmd_calibration_parameter(M) returns false\n");
2090 if(cmd_version() ==
false) {
2091 DBG_PRINTF(
true,
"cmd_version() returns false\n");
2094 if(cmd_id() ==
false) {
2095 DBG_PRINTF(
true,
"cmd_id() returns false\n");
2098 if(cmd_sensitivity() ==
false) {
2099 DBG_PRINTF(
true,
"cmd_sensitivity() returns false\n");
2102 if(cmd_baudrate() ==
false) {
2103 DBG_PRINTF(
true,
"cmd_baudrate() returns false\n");
2106 if(cmd_mode(mode.c_str()) ==
false) {
2107 DBG_PRINTF(
true,
"cmd_mode(restore) returns false\n");
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());
2121 if(debug && ok ==
false) {
2133 send_command(
"@trig", -1);
2137 return send_command(
"@ping", timeout_msec);
2141 return send_command(
"@divider", timeout_msec);
2145 if(strlen(divider) > 100) {
2149 sprintf(buf,
"@divider,%s", divider);
2150 return send_command(buf, timeout_msec);
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);
2158 std::string
command = std::string(
"@mode,") + std::string(mode_string);
2159 return send_command(command.c_str(), timeout_msec);
2164 if(asc_output == 0) {
2165 return send_command(
"@asc_out", timeout_msec);
2168 std::string
command = std::string(
"@asc_out,") + std::string(asc_output);
2169 return send_command(command.c_str(), timeout_msec);
2174 if(bin_output == 0) {
2175 return send_command(
"@bin_out", timeout_msec);
2178 std::string
command = std::string(
"@bin_out,") + std::string(bin_output);
2179 return send_command(command.c_str(), timeout_msec);
2184 return send_command(
"@set_offset", timeout_msec);
2188 std::string
command = std::string(
"@set_offset,") + std::string(enable_yaw_offset);
2189 return send_command(command.c_str(), timeout_msec);
2193 return send_command(
"@clr_offset", timeout_msec);
2198 if(calibration_parameters == 0) {
2199 sprintf(buf,
"@calib,%c", sensor_type);
2200 return send_command(buf, timeout_msec);
2203 if(strlen(calibration_parameters) >
sizeof(buf)-20) {
2206 sprintf(buf,
"@calib,%c,%s", sensor_type, calibration_parameters);
2207 return send_command(buf, timeout_msec);
2212 return send_command(
"@factory", timeout_msec);
2216 return send_command(
"@version", timeout_msec);
2220 return send_command(
"@id", timeout_msec);
2223 bool cmd_id(
const char* str_sensor_id,
int timeout_msec=500) {
2224 if(strlen(str_sensor_id) > 100) {
2228 sprintf(buf,
"@id,%s", str_sensor_id);
2229 return send_command(buf, timeout_msec);
2233 return send_command(
"@sn", timeout_msec);
2237 return send_command(
"@sensitivity", timeout_msec);
2241 return send_command(
"@baudrate", timeout_msec);
2245 if(strlen(baudrate) > 100) {
2249 sprintf(buf,
"@baudrate,%s", baudrate);
2250 return send_command(buf, timeout_msec);
2254 return send_command(
"@save", timeout_msec);
2268 DBG_PRINTF(debug,
"### SEND : %s\n", command_string.c_str());
2271 char crc_string[16];
2272 for(
size_t i=0; i<command_string.length(); i++) {
2273 crc ^= command_string[i];
2275 sprintf(crc_string,
"*%02X\r\n", crc);
2276 std::string command_with_crc = command_string + std::string(crc_string);
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");
2287 if(timeout_msec < 0) {
2292 std::vector<std::string> tokens;
2293 std::vector<std::string> cmd_tokens;
2300 if(response_message_queue.wait(timeout_msec) ==
false) {
2301 DBG_PRINTF(debug,
"TIMEOUT!!(%d)\n", timeout_msec);
2309 while(response_message_queue.size() == 0);
2316 if(response_message_queue.pop(tokens) ==
false) {
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());
2327 if(tokens[0] == cmd_tokens[0]) {
2328 DBG_PRINTF(debug,
"### RCV OK(%s)\n", cmd_tokens[0].c_str());
2332 DBG_PRINTF(debug,
"ERROR: invalid response. command %s, response %s)\n", cmd_tokens[0].c_str(), tokens[0].c_str());
2341 if(tokens[1] != std::string(
"OK")) {
2343 DBG_PRINTF(debug,
"ERROR: status is not OK. command %s)\n", cmd_tokens[0].c_str());
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;
2356 res = (this->*(it->second))(attributes);
2358 DBG_PRINTF(debug,
"ERROR: message hander returns false. command %s)\n", cmd_tokens[0].c_str());
2363 DBG_PRINTF(debug,
"### OK : message hander returns true. command %s, rcvq_sz %d)\n", cmd_tokens[0].c_str(), response_message_queue.size());
2378 unsigned char buffer[1024];
2380 thread_receiver_ready =
true;
2383 memset(buffer, 0,
sizeof(buffer));
2384 len = serial.
Read(buffer,
sizeof(buffer)-1);
2393 DBG_PRINTF(debug,
"### SZ(%d) [%s]\n", len, buffer);
2394 protocol.feed(buffer, len);
2398 DBG_PRINTF(debug,
"### %s() exit\n", __FUNCTION__);
2403 bool exit_thread =
false;
2405 while(exit_thread ==
false) {
2406 event = event_queue.pop_event();
2414 case EventItem::EXIT:
2415 DBG_PRINTF(debug,
"receive EventItem::EXIT\n");
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);
2425 case EventItem::DATA: {
2426 SensorData* sensor_data =
event->get_sensor_data();
2427 OnSensorData(sensor_id, *sensor_data);
2439 DBG_PRINTF(debug,
"### %s() exit\n", __FUNCTION__);
2459 response_message_queue.push_back(tokens);
2465 sensor_data.
reset();
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);
2473 if(res && activate_user_event) {
2474 event_queue.push_event_data(sensor_data);
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());
2496 dbg_show_all_attributes(attributes);
2498 set_attribute(
"divider", attributes[
"divider"]);
2499 set_attribute(
"max_rate", attributes[
"max_rate"]);
2505 dbg_show_all_attributes(attributes);
2507 set_attribute(
"mode", attributes[
"mode"]);
2513 dbg_show_all_attributes(attributes);
2515 set_attribute(
"ascii_format", attributes[
"fmt"]);
2521 dbg_show_all_attributes(attributes);
2523 std::vector<std::string> prop_bin_format;
2531 dbg_show_all_attributes(attributes);
2535 set_attribute(
"yaw_offset", attributes[
"yaw_offset"]);
2537 std::vector<std::string> parameters;
2542 set_attribute(
"coordinate_transformation_sensor_to_vehicle",
StringUtil::join(parameters,
", "));
2547 set_attribute(
"coordinate_transformation_global_to_user",
StringUtil::join(parameters,
", "));
2553 dbg_show_all_attributes(attributes);
2557 std::string sensor = attributes[
"sensor"];
2559 switch((
char)sensor[0]) {
2573 std::vector<std::string> parameters;
2578 std::vector<std::string> transform(9,
"");
2579 std::vector<std::string> bias(3,
"");
2581 std::copy_n(parameters.begin(), 9, transform.begin());
2582 std::copy_n(parameters.begin()+9, 3, bias.begin());
2584 set_attribute(sensor +
"_calibration_matrix",
StringUtil::join(transform,
", "));
2591 dbg_show_all_attributes(attributes);
2597 dbg_show_all_attributes(attributes);
2603 dbg_show_all_attributes(attributes);
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"]);
2615 dbg_show_all_attributes(attributes);
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());
2625 dbg_show_all_attributes(attributes);
2627 set_attribute(
"sensor_serial_number", attributes[
"sn"]);
2633 dbg_show_all_attributes(attributes);
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"]);
2644 dbg_show_all_attributes(attributes);
2646 set_attribute(
"baudrate", attributes[
"baudrate"]);
2652 dbg_show_all_attributes(attributes);
2664 static const int DATA_NUM = 3;
2665 if(tokens.size() != DATA_NUM + 2) {
2671 std::vector<std::string> str_euler(DATA_NUM,
"");
2672 std::copy_n(tokens.begin() + 2, DATA_NUM, str_euler.begin());
2685 static const int DATA_NUM = 4;
2686 if(tokens.size() != DATA_NUM + 2) {
2692 std::vector<std::string> str_quat(DATA_NUM,
"");
2693 std::copy_n(tokens.begin() + 2, DATA_NUM, str_quat.begin());
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) {
2714 std::vector<std::string> str_euler(DATA_NUM_EULER,
"");
2715 std::copy_n(tokens.begin() + 2, DATA_NUM_EULER, str_euler.begin());
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());
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) {
2743 std::vector<std::string> str_quat(DATA_NUM_QUAT,
"");
2744 std::copy_n(tokens.begin() + 2, DATA_NUM_QUAT, str_quat.begin());
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());
2765 static const int DATA_NUM = 10;
2766 if(tokens.size() != DATA_NUM + 2) {
2772 std::vector<std::string> str_imu(DATA_NUM,
"");
2773 std::copy_n(tokens.begin() + 2, DATA_NUM, str_imu.begin());
2776 imu_rawdata.
set(str_imu);
2786 static const int DATA_NUM = 10;
2787 if(tokens.size() != DATA_NUM + 2) {
2793 std::vector<std::string> str_imu(DATA_NUM,
"");
2794 std::copy_n(tokens.begin() + 2, DATA_NUM, str_imu.begin());
2812 sensor_data.
reset();
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());
2820 for(
size_t i=0; i<node_list.size(); 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) {
2830 if(activate_user_event) {
2831 event_queue.push_event_data(sensor_data);
2838 static const int SCALE_FACTOR = 0x7FFF;
2839 double ratio = ((double)value)/SCALE_FACTOR;
2840 return (
float)(ratio*value_max);
2846 DBG_PRINTF(debug,
"### binary_update_sequence(%d)\n", node.
list[0].value.ui8);
2852 static const int DATA_NUM = 3;
2853 if(node.
list.size() != DATA_NUM) {
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);
2862 e.
set(roll, pitch, yaw);
2865 DBG_PRINTF(debug,
"### %s(%f, %f, %f)\n", __FUNCTION__, roll, pitch, yaw);
2871 static const int DATA_NUM = 4;
2872 if(node.
list.size() != DATA_NUM) {
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);
2885 DBG_PRINTF(debug,
"### %s(%f, %f, %f, %f)\n", __FUNCTION__, x,y,z,w);
2891 static const int DATA_NUM = 10;
2892 if(node.
list.size() != DATA_NUM) {
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);
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);
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);
2910 m[9] = int16tofloat(node.
list[9].value.i16, (
float)TEMP_RANGE);
2917 std::vector<std::string> str_list;
2926 static const int DATA_NUM = 10;
2927 if(node.
list.size() != DATA_NUM) {
2932 for(
int i=0; i<DATA_NUM; i++) {
2933 m[i] = node.
list[i].value.i16;
2941 std::vector<std::string> str_list;
2956 void(*attribute_callback)(
void* context,
int sensor_id,
const char* attribute_name,
const char* value);
2959 void(*data_callback)(
void* context,
int sensor_id,
SensorData* sensor_data);
2967 , attribute_callback(0), attribute_callback_context(0)
2968 , data_callback(0), data_callback_context(0)
2972 attribute_callback = 0;
2973 attribute_callback_context = 0;
2975 data_callback_context = 0;
2979 return event.wait(timeout_msec);
2982 void register_attribute_callback(
void(*callback)(
void* context,
int sensor_id,
const char* attribute_name,
const char* value),
void* callback_context) {
2984 attribute_callback_context = callback_context;
2985 attribute_callback = callback;
2990 data_callback_context = callback_context;
2991 data_callback = callback;
2996 return sample_count;
3002 if(attribute_callback) {
3004 attribute_callback(attribute_callback_context, sensor_id, attribute_name.c_str(), value.c_str());
3015 data_callback(data_callback_context, sensor_id, &data);
3022 #endif // __MYAHRS_PLUS_H_
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)
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)
bool wait(int timeout_msec)
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)
virtual ~myAhrsException()
ReturnCode state_control(uint8_t byte)
#define DBG_PRINTF(x, args...)
DirectionCosineMatrix(double &m11, double &m12, double &m13, double &m21, double &m22, double &m23, double &m31, double &m32, double &m33)
Platform::Mutex mutex_communication
EulerAngle to_euler_angle()
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
#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)
const char * what() const
void read_value(T &value)
EventItem(EventId id=NONE)
SensorData * get_sensor_data()
void update_attitude(EulerAngle &e)
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
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)
iBinaryProtocol * protocol
MyAhrsPlus(std::string port="", unsigned int baudrate=115200)
LockGuard(Platform::Mutex &m)
void set(double r, double p, double y)
void clear_all_attribute()
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
virtual ~iBinaryProtocol()
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)
DirectionCosineMatrix(double dcm[9])
bool ascii_rsp_baudrate(std::map< std::string, std::string > &attributes)
bool ascii_update_quatimu(std::vector< std::string > &tokens)
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="")
std::string read_string()
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=' ')
uint32_t get_sample_count()
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 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 * data_callback_context
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
FrameBuffer binary_stream
EventItemData(SensorData &d)
void * attribute_callback_context
void dbg_show_all_attributes(std::map< std::string, std::string > &attributes)
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)
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)
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
bool thread_receiver_ready
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)
virtual ~iAsciiProtocol()
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)
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)
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=' ')
#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)