Skip to content

Commit

Permalink
Binaryデータ出力時は実機とテストの両方で動作確認完了、ASCIIデータには未対応、デバッグ用出力あり
Browse files Browse the repository at this point in the history
  • Loading branch information
@YusukeKato
YusukeKato committed Apr 10, 2024
1 parent faf39b0 commit 8323674
Show file tree
Hide file tree
Showing 2 changed files with 85 additions and 37 deletions.
56 changes: 49 additions & 7 deletions src/rt_usb_9axisimu_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,8 @@
* POSSIBILITY OF SUCH DAMAGE.
*/

#include <iostream>

#include <cstring>
#include <memory>
#include <string>
Expand Down Expand Up @@ -260,19 +262,59 @@ void RtUsb9axisimuRosDriver::stopCommunication(void)
void RtUsb9axisimuRosDriver::checkDataFormat(void)
{
if (data_format_ == DataFormat::NONE) {
unsigned char data_buf[256];
int data_size_of_buf = serial_port_->readFromDevice(data_buf, consts.IMU_BIN_DATA_SIZE);
if (data_size_of_buf == consts.IMU_BIN_DATA_SIZE) {
if (isBinarySensorData(data_buf)) {
// 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;
}
}

// debug
std::cout << "--- debug start ---" << std::endl;;
for(int i = 0; i < (int)sizeof(data_buf_ascii); i++) {
std::cout << data_buf_ascii[i];
}
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;
}
}
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 if (data_format_ == DataFormat::NOT_BINARY) {
data_format_ = DataFormat::ASCII;
has_completed_format_check_ = true;
}
}

Expand Down
66 changes: 36 additions & 30 deletions test/test_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,9 @@
*/

#include <array>
#include <vector>
#include <string>
#include <iostream>
#include <gtest/gtest.h>
#include "fakeit.hpp"
#include "rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp"
Expand Down Expand Up @@ -85,7 +87,7 @@ TEST(TestDriver, checkDataFormat_Binary)
// Expect to check correctly when read data in binary format
auto mock = create_serial_port_mock();

When(Method(mock, readFromDevice)).Do([](
When(Method(mock, readFromDevice)).AlwaysDo([](
unsigned char* buf, unsigned int buf_size) {
rt_usb_9axisimu::Consts consts;
unsigned char dummy_bin_imu_data[consts.IMU_BIN_DATA_SIZE] = {0};
Expand Down Expand Up @@ -113,39 +115,45 @@ TEST(TestDriver, checkDataFormat_ASCII)
// Expect to check correctly when read data in ASCII format
auto mock = create_serial_port_mock();

When(Method(mock, readFromDevice)).Do([](
When(Method(mock, readFromDevice)).AlwaysDo([](
unsigned char* buf, unsigned int buf_size) {
rt_usb_9axisimu::Consts consts;
std::array<std::string, consts.IMU_ASCII_DATA_SIZE> dummy_ascii_imu_data;
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";
std::string str = "";
std::string split_char = ",";
for(int i = 0; i < consts.IMU_ASCII_DATA_SIZE; i++) {
str += dummy_ascii_imu_data[i];
if(i != consts.IMU_ASCII_DATA_SIZE - 1) str += split_char;
}
for(int i = 0; i < int(str.length()); i++) {
buf[i] = str[i];
}
buf_size = consts.IMU_BIN_DATA_SIZE;
//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);
return buf_size;
});

RtUsb9axisimuRosDriver driver(
std::unique_ptr<SerialPort>(&mock.get()));

driver.checkDataFormat();
driver.checkDataFormat();

EXPECT_TRUE(driver.hasCompletedFormatCheck());
EXPECT_TRUE(driver.hasAsciiDataFormat());
Expand All @@ -157,15 +165,13 @@ TEST(TestDriver, checkDataFormat_not_Binary_or_ASCII)
// Expect to check correctly when read data in not Binary or ASCII format
auto mock = create_serial_port_mock();

When(Method(mock, readFromDevice)).Do([](
When(Method(mock, readFromDevice)).AlwaysDo([](
unsigned char* buf, unsigned int buf_size) {
rt_usb_9axisimu::Consts consts;
unsigned char dummy_data_not_binary_or_ascii[] =
"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 = consts.IMU_BIN_DATA_SIZE;
buf = dummy_data_not_binary_or_ascii;
buf_size = strlen((char*)buf);
return buf_size;
});

Expand Down

0 comments on commit 8323674

Please sign in to comment.