Compare commits

..

7 Commits

12 changed files with 2269 additions and 53 deletions

1
.gitignore vendored
View File

@ -7,7 +7,6 @@
*.core
*.moc
*.o
*.obj
*.orig
*.rej
*.so

View File

@ -13,11 +13,8 @@ set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
#find_package(OpenCL REQUIRED)
include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include" "${CMAKE_CURRENT_SOURCE_DIR}/imgui-submodule/imgui/examples" "${CMAKE_CURRENT_SOURCE_DIR}/imgui-submodule/imgui" ${EPOXY_INCLUDE_DIRS} ${ASSIMP_INCLUDE_DIRS} ${SDL2_INCLUDE_DIRS})
configure_file("shaders/2d-passthrough-vertex.glsl" "shaders/2d-passthrough-vertex.glsl" COPYONLY)
configure_file("shaders/fixed-red-fragment.glsl" "shaders/fixed-red-fragment.glsl" COPYONLY)
configure_file("shaders/triangle-gen-geometry.glsl" "shaders/triangle-gen-geometry.glsl" COPYONLY)
@ -32,7 +29,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"

View File

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

View File

@ -0,0 +1,47 @@
#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();
void set_device(const std::string &device);
void set_baudrate(const unsigned int baud);
unsigned int get_baudrate();
const std::string &get_device();
bool runs();
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);
};
#endif /* _IMU_SERIAL_HPP_ */

View File

@ -4,7 +4,7 @@
#include <string>
#include <epoxy/gl.h>
#include <opengl-playground/openglshader.hpp>
//#include <glm/glm.hpp>
#include <glm/glm.hpp>
#include <memory>
class OpenGlGraphics

13
models/bmi160/bmi160.mtl Normal file
View File

@ -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

40
models/bmi160/bmi160.obj Normal file
View File

@ -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

BIN
models/bmi160/texture.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 35 KiB

BIN
models/bmi160/uv.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 22 KiB

File diff suppressed because it is too large Load Diff

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

@ -0,0 +1,298 @@
#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;
this->acc = this->gyr = glm::vec3(0.0f, 0.0f, 0.0f);
}
ImuSerial::~ImuSerial()
{
if (this->fd >= 0)
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;
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, baudrate(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 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;
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 (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 'W':
quat.w = value;
break;
case 'I':
quat.x = value;
break;
case 'J':
quat.y = value;
break;
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 = 0;
} 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;
}
}
void ImuSerial::set_device(const std::string &device)
{
this->serial_device = device;
}
void ImuSerial::set_baudrate(const unsigned int baud)
{
this->baud = baud;
}
unsigned int ImuSerial::get_baudrate()
{
return this->baud;
}
const std::string &ImuSerial::get_device()
{
return this->serial_device;
}
bool ImuSerial::runs()
{
if (this->worker.joinable() && this->stop_flag == 0)
return true;
else
return false;
}
glm::quat ImuSerial::get_quaternion()
{
std::lock_guard(this->quat_mutex);
glm::quat ret_val = this->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());
}

View File

@ -4,16 +4,50 @@
#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>
#include <imgui_impl_opengl3.h>
#define WINDOW_WIDTH 1200
#define WINDOW_HEIGHT 800
#define WINDOW_WIDTH 1500
#define WINDOW_HEIGHT 1000
static void handle_serial_port_gui(ImuSerial &imu_comm)
{
static char foo[512] = {0};
static int baud_rate;
static bool first = true;
bool runs = imu_comm.runs();
if (first) {
imu_comm.get_device().copy(foo, sizeof(foo), 0);
baud_rate = imu_comm.get_baudrate();
first = false;
}
ImGui::Begin("Serial Config");
ImGui::InputText("Serial Device", foo, sizeof(foo));
ImGui::InputInt("Baudrate", &baud_rate);
bool button = ImGui::Button((runs ? "Stop" : "Start"));
ImGui::Text("(c) Mario Huettel, GPLv2");
ImGui::End();
if (button) {
if (runs) {
imu_comm.stop();
} else {
imu_comm.set_device(std::string(foo));
imu_comm.set_baudrate((unsigned int)baud_rate);
imu_comm.start();
}
}
}
int main(int argc, char **argv)
{
@ -22,7 +56,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 +90,25 @@ 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("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);
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);
glEnable(GL_DEPTH_TEST);
auto imu_serial = ImuSerial("/dev/ttyUSB0", 115200);
while (run) {
SDL_Event event;
@ -100,9 +142,11 @@ int main(int argc, char **argv)
ImGui_ImplSDL2_NewFrame(window->getWindow());
ImGui::NewFrame();
handle_serial_port_gui(imu_serial);
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 +155,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 +181,50 @@ 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();
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 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);
ImGui::Text("Quat WIJK: %.02f | %.02f | %.02f | %.02f", quat.w, quat.x, quat.y, quat.z);
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());