mhu/opengl-playground
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add BMI160 visualization

Browse Source
This commit is contained in:
Mario Hüttel 2020-07-13 18:20:54 +02:00
parent adf1841592
commit bb5048274f
4 changed files with 251 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/opengl-playground/globalcanvassettings.hpp
View File

@ -1,7 +1,7 @@
#ifndef GLOBALCANVASSETTINGS_H
#define GLOBALCANVASSETTINGS_H
//#include <glm/glm.hpp>
#include <glm/glm.hpp>
class GlobalCanvasSettings
{

35
include/opengl-playground/imu-serial.hpp Normal file
View File

@ -0,0 +1,35 @@
#ifndef _IMU_SERIAL_HPP_
#define _IMU_SERIAL_HPP_
#include <string>
#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>
#include <mutex>
#include <thread>
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_ */

180
src/imu-serial.cpp Normal file
View File

@ -0,0 +1,180 @@
#include <opengl-playground/imu-serial.hpp>
#include <iostream>
#include <stdio.h>
#include <string.h>
// Linux headers
#include <fcntl.h> // Contains file controls like O_RDWR
#include <errno.h> // Error integer and strerror() function
#include <termios.h> // Contains POSIX terminal control definitions
#include <unistd.h> // write(), read(), close()
#include <chrono>
#include <string>
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());
}

79
src/main.cpp
View File

@ -4,9 +4,10 @@
#include <opengl-playground/textured-rectangle.hpp>
#include <opengl-playground/globalcanvassettings.hpp>
#include <glm/glm.hpp>
//#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <opengl-playground/model.hpp>
#include <opengl-playground/imu-serial.hpp>
#include <imgui.h>
#include <imgui_impl_sdl.h>
@ -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<OpenGlShaderProgram>("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());