diff --git a/CMakeLists.txt b/CMakeLists.txt index 95e3155..77db17e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,7 +32,9 @@ configure_file("shaders/mesh-fragment.glsl" "shaders/mesh-fragment.glsl" COPYONL configure_file("models/coordinate-system/coordsys.obj" "coordinate-system/coordsys.obj" COPYONLY) configure_file("models/coordinate-system/coordsys.mtl" "coordinate-system/coordsys.mtl" COPYONLY) - +configure_file("models/bmi160/bmi160.obj" "bmi160/bmi160.obj" COPYONLY) +configure_file("models/bmi160/bmi160.mtl" "bmi160/bmi160.mtl" COPYONLY) +configure_file("models/bmi160/texture.png" "bmi160/texture.png" COPYONLY) set(IMGUI_SOURCES "${CMAKE_CURRENT_SOURCE_DIR}/imgui-submodule/imgui/imgui.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/imgui-submodule/imgui/imgui_draw.cpp" diff --git a/include/opengl-playground/imu-serial.hpp b/include/opengl-playground/imu-serial.hpp index c574be3..f88a13a 100644 --- a/include/opengl-playground/imu-serial.hpp +++ b/include/opengl-playground/imu-serial.hpp @@ -16,19 +16,26 @@ class ImuSerial void stop(); glm::quat get_quaternion(); + glm::vec3 get_accel(); + glm::vec3 get_gyro(); const glm::mat4 get_rot_matrix(); private: std::string serial_device; unsigned int baud; std::mutex quat_mutex; + std::mutex acc_mutex; + std::mutex gyr_mutex; glm::quat quaternion; + glm::vec3 acc; + glm::vec3 gyr; std::thread worker; int stop_flag; int fd; friend void working_thread(ImuSerial *self); friend void update_quaternion(ImuSerial *self, const glm::quat &quat); - + friend void update_acc_value(ImuSerial *self, const glm::vec3 &acc); + friend void update_gyr_value(ImuSerial *self, const glm::vec3 &gyr); }; diff --git a/include/opengl-playground/openglgraphics.hpp b/include/opengl-playground/openglgraphics.hpp index 9880854..d0bbc30 100644 --- a/include/opengl-playground/openglgraphics.hpp +++ b/include/opengl-playground/openglgraphics.hpp @@ -4,7 +4,7 @@ #include #include #include -//#include +#include #include class OpenGlGraphics diff --git a/models/bmi160/bmi160.mtl b/models/bmi160/bmi160.mtl new file mode 100644 index 0000000..a5a485a --- /dev/null +++ b/models/bmi160/bmi160.mtl @@ -0,0 +1,13 @@ +# Blender MTL File: 'bmi160.blend1' +# Material Count: 1 + +newmtl Material +Ns 323.999994 +Ka 1.000000 1.000000 1.000000 +Kd 0.800000 0.800000 0.800000 +Ks 0.500000 0.500000 0.500000 +Ke 0.000000 0.000000 0.000000 +Ni 1.450000 +d 1.000000 +illum 2 +map_Kd texture.png diff --git a/models/bmi160/bmi160.obj b/models/bmi160/bmi160.obj new file mode 100644 index 0000000..b9e73d0 --- /dev/null +++ b/models/bmi160/bmi160.obj @@ -0,0 +1,40 @@ +# Blender v2.83.2 OBJ File: 'bmi160.blend1' +# www.blender.org +mtllib bmi160.mtl +o Cube +v 1.250000 0.400000 -1.500000 +v 1.250000 -0.400000 -1.500000 +v 1.250000 0.400000 1.500000 +v 1.250000 -0.400000 1.500000 +v -1.250000 0.400000 -1.500000 +v -1.250000 -0.400000 -1.500000 +v -1.250000 0.400000 1.500000 +v -1.250000 -0.400000 1.500000 +vt 0.575745 0.121275 +vt 0.575745 0.500000 +vt 0.121275 0.500000 +vt 0.121275 0.121275 +vt 0.121275 0.999916 +vt 0.000084 0.999917 +vt 0.000083 0.621192 +vt 0.121275 0.621192 +vt 0.575745 0.621192 +vt 0.575745 0.999917 +vt 0.575745 0.000083 +vt 0.121275 0.000083 +vt 0.696937 0.621192 +vt 0.696937 0.999917 +vn 0.0000 1.0000 0.0000 +vn 0.0000 0.0000 1.0000 +vn -1.0000 0.0000 0.0000 +vn 0.0000 -1.0000 0.0000 +vn 1.0000 0.0000 0.0000 +vn 0.0000 0.0000 -1.0000 +usemtl Material +s off +f 1/1/1 5/2/1 7/3/1 3/4/1 +f 4/5/2 3/6/2 7/7/2 8/8/2 +f 8/8/3 7/3/3 5/2/3 6/9/3 +f 6/9/4 2/10/4 4/5/4 8/8/4 +f 2/11/5 1/1/5 3/4/5 4/12/5 +f 6/9/6 5/13/6 1/14/6 2/10/6 diff --git a/models/bmi160/texture.png b/models/bmi160/texture.png new file mode 100644 index 0000000..4095444 Binary files /dev/null and b/models/bmi160/texture.png differ diff --git a/models/bmi160/uv.png b/models/bmi160/uv.png new file mode 100644 index 0000000..4b4b9c1 Binary files /dev/null and b/models/bmi160/uv.png differ diff --git a/src/imu-serial.cpp b/src/imu-serial.cpp index 5202f45..4ec6e06 100644 --- a/src/imu-serial.cpp +++ b/src/imu-serial.cpp @@ -17,6 +17,8 @@ ImuSerial::ImuSerial(const std::string &serial_device, unsigned int baud) this->baud = baud; fd = -1; stop_flag = 0; + + this->acc = this->gyr = glm::vec3(0.0f, 0.0f, 0.0f); } ImuSerial::~ImuSerial() @@ -25,6 +27,51 @@ ImuSerial::~ImuSerial() this->stop(); } + +static speed_t baudrate(int baud) +{ + switch (baud) { + case 9600: + return B9600; + case 19200: + return B19200; + case 38400: + return B38400; + case 57600: + return B57600; + case 115200: + return B115200; + case 230400: + return B230400; + case 460800: + return B460800; + case 500000: + return B500000; + case 576000: + return B576000; + case 921600: + return B921600; + case 1000000: + return B1000000; + case 1152000: + return B1152000; + case 1500000: + return B1500000; + case 2000000: + return B2000000; + case 2500000: + return B2500000; + case 3000000: + return B3000000; + case 3500000: + return B3500000; + case 4000000: + return B4000000; + default: + return -1; + } +} + static int open_serial(const char *serdev, unsigned int baud) { int fd; @@ -61,7 +108,7 @@ static int open_serial(const char *serdev, unsigned int baud) tty.c_cc[VTIME] = 0; // Wait for up to 1s (10 deciseconds), returning as soon as any data is received. tty.c_cc[VMIN] = 0; - cfsetispeed(&tty, baud); + cfsetispeed(&tty, baudrate(baud)); if (tcsetattr(fd, TCSANOW, &tty) != 0) { std::cout << "Error setting serial config" << std::endl; } @@ -80,60 +127,91 @@ void update_quaternion(ImuSerial *self, const glm::quat &quat) self->quaternion = quat; } +void update_acc_value(ImuSerial *self, const glm::vec3 &acc) +{ + std::lock_guard(self->acc_mutex); + self->acc = acc; +} + +void update_gyr_value(ImuSerial *self, const glm::vec3 &gyr) +{ + std::lock_guard(self->gyr_mutex); + self->gyr = gyr; +} + void working_thread(ImuSerial *self) { using namespace std::literals::chrono_literals; - enum rcv_state {RECV_W, RECV_I, RECV_J, RECV_K, RECV_IDLE}; - enum rcv_state state = RECV_IDLE; - int data_ptr = 0; + char state = 0; std::string buffer; char data; float value; glm::quat quat; + glm::vec3 acc; + glm::vec3 gyr; quat = self->get_quaternion(); while (!self->stop_flag) { if (read(self->fd, &data, 1) == 1) { - if (data == 'W') { - state = RECV_W; - data_ptr = 0; - buffer = ""; - } else if (data == 'I') { - state = RECV_I; - data_ptr = 0; - buffer = ""; - } else if (data == 'J') { - state = RECV_J; - data_ptr = 0; - buffer = ""; - } else if (data == 'K') { - state = RECV_K; - data_ptr = 0; - buffer = ""; - } else if (state != RECV_IDLE ){ + if (state == 0) { + switch (data) { + case 'W': + case 'I': + case 'K': + case 'J': + case 'x': + case 'y': + case 'z': + case 'X': + case 'Y': + case 'Z': + state = data; + buffer = ""; + } + } else { if (data == '\n') { try { value = parse_buffer(buffer); switch (state) { - case RECV_W: + case 'W': quat.w = value; break; - case RECV_I: + case 'I': quat.x = value; break; - case RECV_J: + case 'J': quat.y = value; break; - case RECV_K: + case 'K': quat.z = value; update_quaternion(self, quat); break; + case 'x': + acc.x = value; + break; + case 'y': + acc.y = value; + break; + case 'z': + acc.z = value; + update_acc_value(self, acc); + break; + case 'X': + gyr.x = value; + break; + case 'Y': + gyr.y = value; + break; + case 'Z': + gyr.z = value; + update_gyr_value(self, gyr); + break; } } catch(...) { std::cerr << "Invalid data received: " << buffer << std::endl; } - state = RECV_IDLE; + state = 0; } else { buffer = buffer.append(&data, 1); } @@ -174,6 +252,18 @@ glm::quat ImuSerial::get_quaternion() return ret_val; } +glm::vec3 ImuSerial::get_accel() +{ + std::lock_guard(this->acc_mutex); + return this->acc; +} + +glm::vec3 ImuSerial::get_gyro() +{ + std::lock_guard(this->gyr_mutex); + return this->gyr; +} + const glm::mat4 ImuSerial::get_rot_matrix() { return glm::mat4_cast(this->get_quaternion()); diff --git a/src/main.cpp b/src/main.cpp index 839d06f..293ddb1 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,8 +13,8 @@ #include #include -#define WINDOW_WIDTH 1200 -#define WINDOW_HEIGHT 800 +#define WINDOW_WIDTH 1500 +#define WINDOW_HEIGHT 1000 int main(int argc, char **argv) { @@ -63,7 +63,10 @@ int main(int argc, char **argv) coords.realize(); - Model imu = coords; + Model imu("bmi160/bmi160.obj", glm::vec4(1.0f, 0.0f, 0.0f, 1.0f), shader); + + imu.realize(); + auto imu_base_mat = glm::translate(glm::mat4(1.0f), glm::vec3(2.0,0.0,0.0)); imu.setModelMatrix(imu_base_mat); @@ -155,7 +158,7 @@ int main(int argc, char **argv) imu.render(); { - ImGui::Begin("IMU Rotation"); + ImGui::Begin("IMU Attitude & Heading"); auto quat = imu_serial.get_quaternion(); auto euler = glm::eulerAngles(quat) * 180.0f / (float)M_PI; ImGui::Text("Euler XYZ: %.0f | %.0f | %.0f", euler.x, euler.y, euler.z); @@ -163,6 +166,33 @@ int main(int argc, char **argv) ImGui::End(); } + { + auto accel = imu_serial.get_accel(); + auto gyro = imu_serial.get_gyro(); + auto current_rot_matrix = imu_serial.get_rot_matrix(); + auto global_accel = current_rot_matrix * glm::vec4(accel, 0.0f); + auto global_gyr = current_rot_matrix * glm::vec4(gyro, 0.0f); + + ImGui::Begin("IMU Data"); + ImGui::Text("GYR X: %8.2f\t\t °/s", glm::degrees(gyro.x)); + ImGui::Text("GYR Y: %8.2f\t\t °/s", glm::degrees(gyro.y)); + ImGui::Text("GYR Z: %8.2f\t\t °/s", glm::degrees(gyro.z)); + ImGui::Spacing(); + ImGui::Text("GYR right: %8.2f\t\t °/s", glm::degrees(global_gyr.x)); + ImGui::Text("GYR front: %8.2f\t\t °/s", glm::degrees(global_gyr.y)); + ImGui::Text("GYR up: %8.2f\t\t °/s", glm::degrees(global_gyr.z)); + ImGui::Separator(); + ImGui::Text("ACC X: %8.3f\t\t g", accel.x); + ImGui::Text("ACC Y: %8.3f\t\t g", accel.y); + ImGui::Text("ACC Z: %8.3f\t\t g", accel.z); + ImGui::Spacing(); + ImGui::Text("ACC right\t: %8.3f\t g", global_accel.x); + ImGui::Text("ACC front\t: %8.3f\t g", global_accel.y); + ImGui::Text("ACC up \t: %8.3f\t g", global_accel.z); + + ImGui::End(); + } + ImGui::Render(); ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData());