maszyna/utilities/motiontelemetry.cpp

#include "stdafx.h"
#include "utilities/motiontelemetry.h"
#include "utilities/Globals.h"
#include "utilities/Logs.h"
#include "vehicle/Train.h"
#include "utilities/Timer.h"
#include "vehicle/Driver.h"
#include "simulation/simulation.h"

#ifdef _WIN32
#include <winsock2.h>
#include <ws2tcpip.h>
#else
#include <sys/types.h>
#include <sys/socket.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#endif

motiontelemetry::motiontelemetry()
{
	conf = Global.motiontelemetry_conf;

#ifdef _WIN32
	WSADATA wsd;
	if (WSAStartup(MAKEWORD(2, 2), &wsd))
		throw std::runtime_error("failed to init winsock");
#endif

	struct addrinfo hints;
	struct addrinfo *res;
	memset(&hints, 0, sizeof(hints));

	hints.ai_family = AF_UNSPEC;
	hints.ai_socktype = conf.proto == "tcp" ? SOCK_STREAM : SOCK_DGRAM;

	if (getaddrinfo(conf.address.c_str(), conf.port.c_str(), &hints, &res))
		throw std::runtime_error("motiontelemetry: getaddrinfo failed");

	sock = socket(res->ai_family, res->ai_socktype, res->ai_protocol);

	if (sock == -1)
		throw std::runtime_error("motiontelemetry: failed to create socket");

	if (connect(sock, res->ai_addr, res->ai_addrlen) == -1)
		throw std::runtime_error("motiontelemetry: failed to connect socket");

	WriteLog("motiontelemetry: socket connected");
}

motiontelemetry::~motiontelemetry()
{
	shutdown(sock, 2);
#ifdef _WIN32
	WSACleanup();
#endif
}

void motiontelemetry::update()
{
	auto now = std::chrono::high_resolution_clock::now();
	if (std::chrono::duration<float>(now - last_update).count() < conf.updatetime)
		return;
	last_update = now;

	if (Global.iPause)
		return;

    const TTrain *t = simulation::Train;
	if (!t)
		return;

	double dt = Timer::GetDeltaTime();

	glm::dvec3 frontvector = t->Dynamic()->VectorFront();
	glm::dvec3 up = t->Dynamic()->VectorUp();
	glm::dvec3 leftvector = 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, leftvector), glm::dot(gravity, up), glm::dot(gravity, frontvector));
	}

	if (conf.latposbased)
		local_acc.x -= glm::dot(acc, leftvector);
	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, frontvector);
	else
		local_acc.z += t->Occupied()->AccSVBased;

	local_acc /= 9.81;

	// roll calculation, maybe too complicated?

	// transform to left-handed
	frontvector.z *= -1;
	up.z *= -1;

	// make sure that vectors are orthonormal
	glm::dvec3 oright = glm::normalize(glm::cross(up, frontvector));
	glm::dvec3 oup = glm::cross(frontvector, oright);

	// right and up vector without roll
	glm::dvec3 right0 = glm::normalize(glm::cross(glm::dvec3(0.0, 1.0, 0.0), frontvector));
	glm::dvec3 up0 = glm::cross(frontvector, 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(-frontvector.y), atan2(frontvector.x, frontvector.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()->CabActive < 0)
	{
		velocity *= -1;
		local_acc.x *= -1;
		local_acc.z *= -1;
		yaw_vel *= -1;
		rot *= -1;
	}

	std::array<float, 12> buffer{};
	buffer[0] = static_cast<float>(Timer::GetTime());
	buffer[1] = static_cast<float>(velocity);
	buffer[2] = static_cast<float>(local_acc.y);
	buffer[3] = static_cast<float>(local_acc.z);
	buffer[4] = static_cast<float>(local_acc.x);
	buffer[5] = static_cast<float>(glm::degrees(rot.x));
	buffer[6] = static_cast<float>(glm::degrees(rot.z));
	buffer[7] = static_cast<float>(glm::degrees(rot.y));
	buffer[8] = static_cast<float>(yaw_vel);
	buffer[9] = 1.0f;

	if (send(sock, reinterpret_cast<char const *>(buffer.data()), sizeof(buffer), 0) == -1)
		WriteLog("motiontelemetry: socket send failed");
}