Add BMI160 visualization

include/opengl-playground/globalcanvassettings.hpp

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

include/opengl-playground/imu-serial.hpp

@@ -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_ */

src/imu-serial.cpp

@@ -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());
+}

src/main.cpp

@@ -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();
+		auto imu_model_matrix = imu_base_mat * imu_correction_inverse * imu_rot_mat * imu_correction_matrix;
-		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);
 
-		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
+		imu.setModelMatrix(imu_model_matrix);
-		m.render();
+		imu.render();
-		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+
+		{
+			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());