motiontelemetry

Globals.cpp

@@ -780,12 +780,16 @@ void Global::ConfigParse(cParser &Parser)
 		}
 		else if (token == "motiontelemetry")
 		{
-			Parser.getTokens(4);
+			Parser.getTokens(8);
 			Global::motiontelemetry_conf.enable = true;
 			Parser >> Global::motiontelemetry_conf.proto;
 			Parser >> Global::motiontelemetry_conf.address;
 			Parser >> Global::motiontelemetry_conf.port;
 			Parser >> Global::motiontelemetry_conf.updatetime;
+			Parser >> Global::motiontelemetry_conf.includegravity;
+			Parser >> Global::motiontelemetry_conf.fwdposbased;
+			Parser >> Global::motiontelemetry_conf.latposbased;
+			Parser >> Global::motiontelemetry_conf.axlebumpscale;
 		}
     } while ((token != "") && (token != "endconfig")); //(!Parser->EndOfFile)
     // na koniec trochę zależności

dumb3d.h

@@ -118,9 +118,9 @@ class vector3
         return true;
     };
 
-	operator glm::vec3() const
+	operator glm::dvec3() const
 	{
-		return glm::vec3(x, y, z);
+		return glm::dvec3(x, y, z);
 	}
   private:
 };
@@ -212,7 +212,7 @@ class matrix4x4
         return true;
     }
 
-	operator glm::mat4() const
+	operator glm::dmat4() const
 	{
 		return glm::make_mat4(e);
 	}

motiontelemetry.cpp

@@ -1,3 +1,4 @@
+#include "stdafx.h"
 #include "motiontelemetry.h"
 #include "Globals.h"
 #include "Logs.h"
@@ -54,21 +55,99 @@ void motiontelemetry::update()
 		return;
 	last_update = now;
 
+	if (Global::iPause)
+		return;
+
 	TTrain *t = Global::pWorld->train();
 	if (!t)
 		return;
 
-	float velocity = t->Occupied()->V;
+	double dt = Timer::GetDeltaTime();
-	float forward = t->Occupied()->AccSVBased / 9.81;
+
-	float lateral = t->Occupied()->AccN / 9.81;
+	glm::dvec3 front = t->Dynamic()->VectorFront();
-	float vertical = t->Occupied()->AccVert / 9.81 + 1.0;
+	glm::dvec3 up = t->Dynamic()->VectorUp();
+	glm::dvec3 left = t->Dynamic()->VectorLeft();
+
+	glm::dvec3 pos = t->Dynamic()->GetPosition();
+	glm::dvec3 vel = (pos - last_pos) / dt;
+	glm::dvec3 acc = (vel - last_vel) / dt;
+	
+	glm::dvec3 local_acc;
+	
+	if (conf.includegravity)
+	{
+		glm::dvec3 gravity(0.0, 9.81, 0.0);
+		local_acc = glm::dvec3(-glm::dot(gravity, left), glm::dot(gravity, up), glm::dot(gravity, front));
+	}
+
+	if (conf.latposbased)
+		local_acc.x -= glm::dot(acc, left);
+	else
+		local_acc.x -= t->Occupied()->AccN;
+
+	local_acc.y += glm::dot(acc, up) + t->Occupied()->AccVert * conf.axlebumpscale;
+
+	if (conf.fwdposbased)
+		local_acc.z += glm::dot(acc, front);
+	else
+		local_acc.z += t->Occupied()->AccSVBased;
+
+	local_acc /= 9.81;
+
+	// roll calculation, maybe too complicated?
+
+	// transform to left-handed
+	front.z *= -1;
+	up.z *= -1;
+
+	// make sure that vectors are orthonormal
+	glm::dvec3 oright = glm::normalize(glm::cross(up, front));
+	glm::dvec3 oup = glm::cross(front, oright);
+
+	// right and up vector without roll
+	glm::dvec3 right0 = glm::normalize(glm::cross(glm::dvec3(0.0, 1.0, 0.0), front));
+	glm::dvec3 up0 = glm::cross(front, right0);
+
+	double cosroll = glm::dot(up0, oup);
+	double sinroll;
+
+	if (abs(right0.x) > abs(right0.y) && abs(right0.x) > abs(right0.z))
+		sinroll = (up0.x * cosroll - oup.x) / right0.x;
+	else if (abs(right0.y) > abs(right0.x) && abs(right0.y) > abs(right0.z))
+		sinroll = (up0.y * cosroll - oup.y) / right0.y;
+	else
+		sinroll = (up0.z * cosroll - oup.z) / right0.z;
+
+	glm::dvec3 rot(asin(-front.y), atan2(front.x, front.z), asin(sinroll));
+
+	double velocity = t->Occupied()->V;
+	double yaw_vel = (rot.y - last_yaw) / dt;
+
+	last_yaw = rot.y;
+	last_pos = pos;
+	last_vel = vel;
 
 	if (t->Occupied()->ActiveCab < 0)
 	{
 		velocity *= -1;
-		forward *= -1;
+		local_acc.x *= -1;
-		lateral *= -1;
+		local_acc.z *= -1;
+		yaw_vel *= -1;
+		rot *= -1;
 	}
 
-	WriteLog("forward: " + std::to_string(forward) + ", lateral: " + std::to_string(lateral) + ", vertical: " + std::to_string(vertical));
+	float buffer[12] = { 0 };
+	buffer[0] = Timer::GetTime();
+	buffer[1] = velocity;
+	buffer[2] = local_acc.y;
+	buffer[3] = local_acc.z;
+	buffer[4] = local_acc.x;
+	buffer[5] = glm::degrees(rot.x);
+	buffer[6] = glm::degrees(rot.z);
+	buffer[7] = glm::degrees(rot.y);
+	buffer[8] = yaw_vel;
+	buffer[9] = 1.0f;
+
+	if (send(sock, (char*)buffer, sizeof(buffer), 0) == -1)
+		WriteLog("motiontelemetry: socket send failed");
 }

motiontelemetry.h

@@ -17,12 +17,19 @@ public:
 		std::string address;
 		std::string port;
 		float updatetime;
+		bool includegravity;
+		bool fwdposbased;
+		bool latposbased;
+		float axlebumpscale;
 	};
 
 private:
 	int sock;
 	std::chrono::time_point<std::chrono::high_resolution_clock> last_update;
 	conf_t conf;
+	glm::dvec3 last_pos;
+	glm::dvec3 last_vel;
+	double last_yaw;
 
 public:
 	motiontelemetry();