Further development of Inertialsensor demonstration

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"

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);
 };
 
 

include/opengl-playground/openglgraphics.hpp

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

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

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

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;
+			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 == '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 ){
+				}
+			} 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());

src/main.cpp

@@ -13,8 +13,8 @@
 #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
 
 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());