Skip to content

Commit

Permalink
実機とテストの両方でBinaryとASCIIの判定に成功、BinaryでもASCIIでもないデータには未対応
Browse files Browse the repository at this point in the history
  • Loading branch information
@YusukeKato
YusukeKato committed Apr 10, 2024
1 parent 8323674 commit 82cb6db
Show file tree
Hide file tree
Showing 3 changed files with 81 additions and 80 deletions.
1 change: 1 addition & 0 deletions include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,7 @@ class RtUsb9axisimuRosDriver
rt_usb_9axisimu::ImuData<int16_t> extractBinarySensorData(unsigned char * imu_data_buf);
bool isBinarySensorData(unsigned char * imu_data_buf);
bool readBinaryData(void);
bool isAsciiSensorData(unsigned char * imu_data_buf, int data_size);
bool isValidAsciiSensorData(std::vector<std::string> imu_data_vector_buf);
bool readAsciiData(void);

Expand Down
98 changes: 48 additions & 50 deletions src/rt_usb_9axisimu_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,8 +87,12 @@ RtUsb9axisimuRosDriver::extractBinarySensorData(unsigned char * imu_data_buf)

bool RtUsb9axisimuRosDriver::isBinarySensorData(unsigned char * imu_data_buf)
{
if (imu_data_buf[consts.IMU_BIN_HEADER_R] == 'R' &&
imu_data_buf[consts.IMU_BIN_HEADER_T] == 'T')
if (imu_data_buf[consts.IMU_BIN_HEADER_FF0] == 0xff &&
imu_data_buf[consts.IMU_BIN_HEADER_FF1] == 0xff &&
imu_data_buf[consts.IMU_BIN_HEADER_R] == 'R' &&
imu_data_buf[consts.IMU_BIN_HEADER_T] == 'T' &&
imu_data_buf[consts.IMU_BIN_HEADER_ID0] == 0x39 &&
imu_data_buf[consts.IMU_BIN_HEADER_ID1] == 0x41)
{
return true;
}
Expand Down Expand Up @@ -139,6 +143,32 @@ bool RtUsb9axisimuRosDriver::readBinaryData(void)
return true;
}

bool RtUsb9axisimuRosDriver::isAsciiSensorData(unsigned char * imu_data_buf, int data_size)
{
// convert imu data to vector in ascii format
static std::vector<std::string> data_vector_ascii;
std::string data_oneline_ascii;
for (int char_count = 0; char_count < data_size; char_count++) {
if (imu_data_buf[char_count] == ',' || imu_data_buf[char_count] == '\n') {
data_vector_ascii.push_back(data_oneline_ascii);
data_oneline_ascii.clear();
} else {
data_oneline_ascii += imu_data_buf[char_count];
}
if (imu_data_buf[char_count] == '\n') {
break;
}
}

// check data is in ascii format
if (data_vector_ascii.size() == consts.IMU_ASCII_DATA_SIZE &&
data_vector_ascii[consts.IMU_ASCII_TIMESTAMP].find(".") == std::string::npos &&
isValidAsciiSensorData(data_vector_ascii)) {
return true;
}
return false;
}

bool RtUsb9axisimuRosDriver::isValidAsciiSensorData(std::vector<std::string> str_vector)
{
for (int i = 1; i < consts.IMU_ASCII_DATA_SIZE; i++) {
Expand Down Expand Up @@ -262,58 +292,26 @@ void RtUsb9axisimuRosDriver::stopCommunication(void)
void RtUsb9axisimuRosDriver::checkDataFormat(void)
{
if (data_format_ == DataFormat::NONE) {
// read data in binary format
unsigned char data_buf_binary[consts.IMU_BIN_DATA_SIZE];
int data_size_binary = serial_port_->readFromDevice(data_buf_binary, consts.IMU_BIN_DATA_SIZE);

// read data in ascii format
unsigned char data_buf_ascii[256];
int data_size_ascii = serial_port_->readFromDevice(data_buf_ascii, sizeof(data_buf_ascii));

// convert ascii data to vector
static std::vector<std::string> data_vector_ascii;
std::string data_oneline_ascii;
for (int char_count = 0; char_count < data_size_ascii; char_count++) {
if (data_buf_ascii[char_count] == ',' || data_buf_ascii[char_count] == '\n') {
data_vector_ascii.push_back(data_oneline_ascii);
data_oneline_ascii.clear();
} else {
data_oneline_ascii += data_buf_ascii[char_count];
}
if (data_buf_ascii[char_count] == '\n') {
break;
}
unsigned char read_buffer[256];
int read_size = serial_port_->readFromDevice(read_buffer, sizeof(read_buffer));

if(read_size <= 0) {
data_format_ = DataFormat::NONE;
has_completed_format_check_ = false;
return;
}

// debug
std::cout << "--- debug start ---" << std::endl;;
for(int i = 0; i < (int)sizeof(data_buf_ascii); i++) {
std::cout << data_buf_ascii[i];
if (isBinarySensorData(read_buffer)) {
data_format_ = DataFormat::BINARY;
has_completed_format_check_ = true;
}
std::cout << std::endl;
std::cout << data_size_ascii << std::endl;;
std::cout << data_vector_ascii.size() << std::endl;;
std::cout << "--- debug start ---" << std::endl;;

// check data format
if (data_size_binary == consts.IMU_BIN_DATA_SIZE) {
if (isBinarySensorData(data_buf_binary)) {
data_format_ = DataFormat::BINARY;
has_completed_format_check_ = true;
} else {
data_format_ = DataFormat::NOT_BINARY;
has_completed_format_check_ = true;
}
else if (isAsciiSensorData(read_buffer, read_size)) {
data_format_ = DataFormat::ASCII;
has_completed_format_check_ = true;
}
if (data_vector_ascii.size() == consts.IMU_ASCII_DATA_SIZE) {
if (data_vector_ascii[consts.IMU_ASCII_TIMESTAMP].find(".") == std::string::npos &&
isValidAsciiSensorData(data_vector_ascii)) {
data_format_ = DataFormat::ASCII;
has_completed_format_check_ = true;
} else {
data_format_ = DataFormat::NOT_ASCII;
has_completed_format_check_ = true;
}
else {
data_format_ = DataFormat::NONE;
has_completed_format_check_ = false;
}
}
}
Expand Down
62 changes: 32 additions & 30 deletions test/test_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,8 +91,12 @@ TEST(TestDriver, checkDataFormat_Binary)
unsigned char* buf, unsigned int buf_size) {
rt_usb_9axisimu::Consts consts;
unsigned char dummy_bin_imu_data[consts.IMU_BIN_DATA_SIZE] = {0};
dummy_bin_imu_data[consts.IMU_BIN_HEADER_FF0] = 0xff;
dummy_bin_imu_data[consts.IMU_BIN_HEADER_FF1] = 0xff;
dummy_bin_imu_data[consts.IMU_BIN_HEADER_R] = 0x52; // R
dummy_bin_imu_data[consts.IMU_BIN_HEADER_T] = 0x54; // T
dummy_bin_imu_data[consts.IMU_BIN_HEADER_ID0] = 0x39;
dummy_bin_imu_data[consts.IMU_BIN_HEADER_ID1] = 0x41;
for(int i = 0; i < consts.IMU_BIN_DATA_SIZE; i++) {
buf[i] = dummy_bin_imu_data[i];
}
Expand All @@ -118,35 +122,31 @@ TEST(TestDriver, checkDataFormat_ASCII)
When(Method(mock, readFromDevice)).AlwaysDo([](
unsigned char* buf, unsigned int buf_size) {
rt_usb_9axisimu::Consts consts;
//std::vector<const char*> dummy_ascii_imu_data(consts.IMU_ASCII_DATA_SIZE);
//dummy_ascii_imu_data[consts.IMU_ASCII_TIMESTAMP] = "0";
//dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_X] = "0.000000";
//dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_Y] = "0.000000";
//dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_Z] = "0.000000";
//dummy_ascii_imu_data[consts.IMU_ASCII_ACC_X] = "0.000000";
//ummy_ascii_imu_data[consts.IMU_ASCII_ACC_Y] = "0.000000";
//dummy_ascii_imu_data[consts.IMU_ASCII_ACC_Z] = "0.000000";
//dummy_ascii_imu_data[consts.IMU_ASCII_MAG_X] = "0.000000";
//dummy_ascii_imu_data[consts.IMU_ASCII_MAG_Y] = "0.000000";
//dummy_ascii_imu_data[consts.IMU_ASCII_MAG_Z] = "0.000000";
//dummy_ascii_imu_data[consts.IMU_ASCII_TEMP] = "0.000000";
//const char split_char = ',';
//const char newline_char = 0x0a; // new line
//int char_count = 0;
//for(int i = 0; i < consts.IMU_ASCII_DATA_SIZE; i++) {
// for(int j = 0; j < (int)strlen(dummy_ascii_imu_data[i]); j++) {
// buf[char_count] = (unsigned char)dummy_ascii_imu_data.at(i)[j];
// char_count++;
// }
// if(i != consts.IMU_ASCII_DATA_SIZE - 1) buf[char_count] = split_char;
// else buf[char_count] = newline_char;
// char_count++;
//}
//buf_size = char_count;
unsigned char dummy_ascii_imu_data[] =
"0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000\n";
buf = dummy_ascii_imu_data;
buf_size = (int)strlen((char*)buf);
std::vector<const char*> dummy_ascii_imu_data(consts.IMU_ASCII_DATA_SIZE);
dummy_ascii_imu_data[consts.IMU_ASCII_TIMESTAMP] = "0";
dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_X] = "0.000000";
dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_Y] = "0.000000";
dummy_ascii_imu_data[consts.IMU_ASCII_GYRO_Z] = "0.000000";
dummy_ascii_imu_data[consts.IMU_ASCII_ACC_X] = "0.000000";
dummy_ascii_imu_data[consts.IMU_ASCII_ACC_Y] = "0.000000";
dummy_ascii_imu_data[consts.IMU_ASCII_ACC_Z] = "0.000000";
dummy_ascii_imu_data[consts.IMU_ASCII_MAG_X] = "0.000000";
dummy_ascii_imu_data[consts.IMU_ASCII_MAG_Y] = "0.000000";
dummy_ascii_imu_data[consts.IMU_ASCII_MAG_Z] = "0.000000";
dummy_ascii_imu_data[consts.IMU_ASCII_TEMP] = "0.000000";
const char split_char = ',';
const char newline_char = '\n';
int char_count = 0;
for(int i = 0; i < consts.IMU_ASCII_DATA_SIZE; i++) {
for(int j = 0; j < (int)strlen(dummy_ascii_imu_data.at(i)); j++) {
buf[char_count] = (unsigned char)dummy_ascii_imu_data.at(i)[j];
char_count++;
}
if(i != consts.IMU_ASCII_DATA_SIZE - 1) buf[char_count] = split_char;
else buf[char_count] = newline_char;
char_count++;
}
buf_size = char_count;
return buf_size;
});

Expand All @@ -170,7 +170,9 @@ TEST(TestDriver, checkDataFormat_not_Binary_or_ASCII)
rt_usb_9axisimu::Consts consts;
unsigned char dummy_data_not_binary_or_ascii[] =
"dummy_data_not_binary_or_ascii";
buf = dummy_data_not_binary_or_ascii;
for(int i = 0; i < (int)sizeof(dummy_data_not_binary_or_ascii); i++) {
buf[i] = dummy_data_not_binary_or_ascii[i];
}
buf_size = strlen((char*)buf);
return buf_size;
});
Expand Down

0 comments on commit 82cb6db

Please sign in to comment.