From bb5048274fa6adf0e2d13c7572ac0f687ca0bb96 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mario=20H=C3=BCttel?= Date: Mon, 13 Jul 2020 18:20:54 +0200 Subject: [PATCH] Add BMI160 visualization --- .../globalcanvassettings.hpp | 2 +- include/opengl-playground/imu-serial.hpp | 35 ++++ src/imu-serial.cpp | 180 ++++++++++++++++++ src/main.cpp | 79 ++++---- 4 files changed, 251 insertions(+), 45 deletions(-) create mode 100644 include/opengl-playground/imu-serial.hpp create mode 100644 src/imu-serial.cpp diff --git a/include/opengl-playground/globalcanvassettings.hpp b/include/opengl-playground/globalcanvassettings.hpp index a4cec9d..ea22ebb 100644 --- a/include/opengl-playground/globalcanvassettings.hpp +++ b/include/opengl-playground/globalcanvassettings.hpp @@ -1,7 +1,7 @@ #ifndef GLOBALCANVASSETTINGS_H #define GLOBALCANVASSETTINGS_H -//#include +#include class GlobalCanvasSettings { diff --git a/include/opengl-playground/imu-serial.hpp b/include/opengl-playground/imu-serial.hpp new file mode 100644 index 0000000..c574be3 --- /dev/null +++ b/include/opengl-playground/imu-serial.hpp @@ -0,0 +1,35 @@ +#ifndef _IMU_SERIAL_HPP_ +#define _IMU_SERIAL_HPP_ + +#include +#include +#include +#include +#include + +class ImuSerial +{ + public: + ImuSerial(const std::string &serial_device, unsigned int baud); + ~ImuSerial(); + void start(); + void stop(); + + glm::quat get_quaternion(); + const glm::mat4 get_rot_matrix(); + + private: + std::string serial_device; + unsigned int baud; + std::mutex quat_mutex; + glm::quat quaternion; + std::thread worker; + int stop_flag; + int fd; + friend void working_thread(ImuSerial *self); + friend void update_quaternion(ImuSerial *self, const glm::quat &quat); + +}; + + +#endif /* _IMU_SERIAL_HPP_ */ diff --git a/src/imu-serial.cpp b/src/imu-serial.cpp new file mode 100644 index 0000000..5202f45 --- /dev/null +++ b/src/imu-serial.cpp @@ -0,0 +1,180 @@ +#include +#include +#include +#include +// Linux headers +#include // Contains file controls like O_RDWR +#include // Error integer and strerror() function +#include // Contains POSIX terminal control definitions +#include // write(), read(), close() +#include +#include + +ImuSerial::ImuSerial(const std::string &serial_device, unsigned int baud) +{ + this->quaternion = glm::quat(1.0f, 0.0f, 0.0f, 0.0f); + this->serial_device = serial_device; + this->baud = baud; + fd = -1; + stop_flag = 0; +} + +ImuSerial::~ImuSerial() +{ + if (this->fd >= 0) + this->stop(); +} + +static int open_serial(const char *serdev, unsigned int baud) +{ + int fd; + struct termios tty; + + fd = open(serdev, O_RDWR); + memset(&tty, 0, sizeof(tty)); + + if (fd < 0) { + std::cout << "Serial port not found or cannot be opened" << std::endl; + return fd; + } + + if (tcgetattr(fd, &tty) != 0) { + std::cout << "Error getting serial configuration" << std::endl; + close(fd); + return -1; + } + + tty.c_cflag &= ~PARENB; + tty.c_cflag &= ~CSTOPB; + tty.c_cflag |= CS8; + tty.c_cflag &= ~CRTSCTS; + tty.c_cflag |= CREAD | CLOCAL; + tty.c_lflag &= ~ICANON; + tty.c_lflag &= ~ECHO; // Disable echo + tty.c_lflag &= ~ECHOE; // Disable erasure + tty.c_lflag &= ~ECHONL; // Disable new-line echo + tty.c_lflag &= ~ISIG; // Disable interpretation of INTR, QUIT and SUSP + tty.c_lflag &= ~ISIG; // Disable interpretation of INTR, QUIT and SUSP + tty.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL); // Disable any special handling of received bytes + tty.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes (e.g. newline chars) + tty.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed + 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); + if (tcsetattr(fd, TCSANOW, &tty) != 0) { + std::cout << "Error setting serial config" << std::endl; + } + + return fd; +} + +float parse_buffer(const std::string &str) +{ + return std::stof(str); +} + +void update_quaternion(ImuSerial *self, const glm::quat &quat) +{ + std::lock_guard(self->quat_mutex); + self->quaternion = quat; +} + +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; + std::string buffer; + char data; + float value; + glm::quat quat; + + 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 (data == '\n') { + try { + value = parse_buffer(buffer); + switch (state) { + case RECV_W: + quat.w = value; + break; + case RECV_I: + quat.x = value; + break; + case RECV_J: + quat.y = value; + break; + case RECV_K: + quat.z = value; + update_quaternion(self, quat); + break; + } + } catch(...) { + std::cerr << "Invalid data received: " << buffer << std::endl; + } + state = RECV_IDLE; + } else { + buffer = buffer.append(&data, 1); + } + } + + } else { + std::this_thread::sleep_for(50us); + } + } +} + +void ImuSerial::start() +{ + this->fd = open_serial(this->serial_device.c_str(), this->baud); + if (this->fd >= 0) { + stop_flag = 0; + worker = std::thread(working_thread, this); + } +} + +void ImuSerial::stop() +{ + stop_flag = 1; + if (worker.joinable()) + worker.join(); + + if (this->fd >= 0) { + close(this->fd); + this->fd = -1; + } +} + +glm::quat ImuSerial::get_quaternion() +{ + std::lock_guard(this->quat_mutex); + glm::quat ret_val = this->quaternion; + + return ret_val; +} + +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 767a3f3..839d06f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -4,9 +4,10 @@ #include #include -#include +//#include #include #include +#include #include #include @@ -22,7 +23,6 @@ int main(int argc, char **argv) SdlMainWindow *window; bool run = true; glm::mat4 projection_matrix; - glm::vec4 color = glm::vec4(1.0f); float fov_angle = 45.0f; @@ -57,16 +57,21 @@ int main(int argc, char **argv) auto shader = std::make_shared("shaders/mesh-vertex.glsl", "", "shaders/mesh-fragment.glsl"); shader->compile(); - Model m("nanosuit/nanosuit.obj", glm::vec4(1.0f, 0.0f, 0.0f, 1.0f), shader); + //Model imu("/tmp/imu.obj", glm::vec4(1.0f, 0.0f, 0.0f, 1.0f), shader); Model coords("coordinate-system/coordsys.obj", glm::vec4(1.0f, 0.0f, 0.0f, 1.0f), shader); coords.realize(); - m.realize(); - m.setModelMatrix(glm::scale(glm::mat4(1.0f), glm::vec3(0.2f, 0.2f, 0.2f))); + + Model imu = coords; + auto imu_base_mat = glm::translate(glm::mat4(1.0f), glm::vec3(2.0,0.0,0.0)); + imu.setModelMatrix(imu_base_mat); glEnable(GL_DEPTH_TEST); + auto imu_serial = ImuSerial("/dev/ttyUSB0", 115200); + imu_serial.start(); + while (run) { SDL_Event event; @@ -103,6 +108,7 @@ int main(int argc, char **argv) ImGui::Begin("Camera"); + static float cam_rot_y = 0.0f; static int e = 1; ImGui::RadioButton("Orthographic", &e, 0); ImGui::SameLine(); ImGui::RadioButton("Perspective", &e, 1); @@ -111,11 +117,18 @@ int main(int argc, char **argv) ImGui::SliderFloat("Camera x", &view_x, -20.0f, 20.0f); ImGui::SliderFloat("Camera y", &view_y, -20.0f, 20.0f); ImGui::SliderFloat("Camera z", &view_z, 0.0f, 20.0f); + ImGui::SliderFloat("Camera y rot", &cam_rot_y, -180.0, 180.0); ImGui::SliderFloat("Camera FOV", &fov_angle, 1.0f, 60.0f); ImGui::Text("Framerate: %.01f Hz", io.Framerate); - GlobalCanvasSettings::setCameraPosition(glm::vec3(view_x, view_y, view_z)); - const glm::mat4 view = glm::lookAt(GlobalCanvasSettings::getCameraPosition(), + + auto camera_rotation = glm::rotate(glm::mat4(1.0f), + glm::radians(cam_rot_y), + glm::vec3(0.0f, 1.0f, 0.0f)); + + auto camera_pos = camera_rotation * glm::vec4(view_x, view_y, view_z, 1.0f); + GlobalCanvasSettings::setCameraPosition(camera_pos); + glm::mat4 view = glm::lookAt(GlobalCanvasSettings::getCameraPosition(), glm::vec3(view_x, view_y, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f)); @@ -130,47 +143,25 @@ int main(int argc, char **argv) } ImGui::End(); - { - ImGui::Begin("Model Matrix"); - glm::mat4 mat = m.getModelMatrix(); - static float x_pos = 0.0f; - static float y_rot = 0.0f, z_rot =0.0f, x_rot = 0.0f; - ImGui::Text("%.2f %.2f %.2f %.2f", mat[0][0], mat[1][0], mat[2][0], mat[3][0]); - ImGui::Text("%.2f %.2f %.2f %.2f", mat[0][1], mat[1][1], mat[2][1], mat[3][1]); - ImGui::Text("%.2f %.2f %.2f %.2f", mat[0][2], mat[1][2], mat[2][2], mat[3][2]); - ImGui::Text("%.2f %.2f %.2f %.2f", mat[0][3], mat[1][3], mat[2][3], mat[3][3]); - ImGui::SliderFloat("X Pos", &x_pos, -5, 5); - ImGui::SliderFloat("X Rotation", &x_rot, -180.0f, 180.0f); - ImGui::SliderFloat("Y Rotation", &y_rot, -180.0f, 180.0f); - ImGui::SliderFloat("Z Rotation", &z_rot, -180.0f, 180.0f); - mat = glm::scale(glm::mat4(1.0f), glm::vec3(0.2f, 0.2f, 0.2f)); - mat = glm::rotate(mat, glm::radians(x_rot), glm::vec3(1.0f, 0.0f, 0.0f)); - mat = glm::rotate(mat, glm::radians(y_rot), glm::vec3(0.0f, 1.0f, 0.0f)); - mat = glm::rotate(mat, glm::radians(z_rot), glm::vec3(0.0f, 0.0f, 1.0f)); - mat = glm::translate(glm::mat4(1.0), glm::vec3(x_pos, 0.0f, 0.0f)) * mat; - m.setModelMatrix(mat); - ImGui::End(); - } - { - ImGui::Begin("Normal Matrix"); - glm::mat4 mat = m.getNormalMatrix(); - ImGui::Text("%.2f %.2f %.2f %.2f", mat[0][0], mat[1][0], mat[2][0], mat[3][0]); - ImGui::Text("%.2f %.2f %.2f %.2f", mat[0][1], mat[1][1], mat[2][1], mat[3][1]); - ImGui::Text("%.2f %.2f %.2f %.2f", mat[0][2], mat[1][2], mat[2][2], mat[3][2]); - ImGui::Text("%.2f %.2f %.2f %.2f", mat[0][3], mat[1][3], mat[2][3], mat[3][3]); - ImGui::End(); - } - m.render(); coords.render(); + auto imu_rot_mat = imu_serial.get_rot_matrix(); + auto imu_correction_matrix = glm::rotate(glm::mat4(1.0f), glm::radians(90.0f), glm::vec3(1.0f, 0.0f, 0.0f)); + auto imu_correction_inverse = glm::inverse(imu_correction_matrix); - glm::mat4 current_model_matrix = m.getModelMatrix(); - current_model_matrix = glm::translate(glm::mat4(1.0f), glm::vec3(3.0f, 0.0f, 0.0f)) * current_model_matrix; - m.setModelMatrix(current_model_matrix); + auto imu_model_matrix = imu_base_mat * imu_correction_inverse * imu_rot_mat * imu_correction_matrix; - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); - m.render(); - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); + imu.setModelMatrix(imu_model_matrix); + imu.render(); + + { + ImGui::Begin("IMU Rotation"); + 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); + ImGui::Text("Quat WIJK: %.02f | %.02f | %.02f | %.02f", quat.w, quat.x, quat.y, quat.z); + ImGui::End(); + } ImGui::Render(); ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData());