motion telemetry preparations

CMakeLists.txt

@@ -77,6 +77,7 @@ set(SOURCES
 "vertex.cpp"
 "audio.cpp"
 "audiorenderer.cpp"
+"motiontelemetry.cpp"
 )
 
 set (PREFIX "")
@@ -114,8 +115,7 @@ if (WIN32)
 	set(LIBSNDFILE_LIBRARY ${LIBSNDFILE_LIBRARY} "${DEPS_DIR}/libsndfile/lib/${ARCH}/libsndfile-1.lib")
 	set(LUAJIT_LIBRARIES ${LUAJIT_LIBRARIES} "${DEPS_DIR}/luajit/lib/${ARCH}/lua51.lib")
 	set(PYTHON_LIBRARY ${PYTHON_LIBRARY} "${DEPS_DIR}/python/lib/${ARCH}/python27.lib")	
-	set(libserialport_LIBRARY ${LIBSERIALPORT_LIBRARY} "${DEPS_DIR}/libserialport/lib/${ARCH}/libserialport-0.lib")	
-	
+	set(libserialport_LIBRARY ${LIBSERIALPORT_LIBRARY} "${DEPS_DIR}/libserialport/lib/${ARCH}/libserialport-0.lib")
 endif()
 
 if (${CMAKE_CXX_COMPILER_ID} STREQUAL MSVC)
@@ -191,3 +191,7 @@ target_link_libraries(${PROJECT_NAME} ${LUAJIT_LIBRARIES})
 find_package(libserialport REQUIRED)
 include_directories(${libserialport_INCLUDE_DIR})
 target_link_libraries(${PROJECT_NAME} ${libserialport_LIBRARY})
+
+if (WIN32)
+	target_link_libraries(${PROJECT_NAME} ws2_32)
+endif()

EU07.cpp

@@ -40,6 +40,7 @@ Stele, firleju, szociu, hunter, ZiomalCl, OLI_EU and others
 #include "resource.h"
 #include "uilayer.h"
 #include "uart.h"
+#include "motiontelemetry.h"
 
 #pragma comment (lib, "glu32.lib")
 #pragma comment (lib, "dsound.lib")
@@ -61,6 +62,7 @@ mouse_input Mouse;
 gamepad_input Gamepad;
 glm::dvec2 mouse_pickmodepos;  // stores last mouse position in control picking mode
 std::unique_ptr<uart_input> uart;
+std::unique_ptr<motiontelemetry> motiontelemetry;
 }
 
 void screenshot_save_thread( char *img )
@@ -356,6 +358,8 @@ int main(int argc, char *argv[])
 		input::Gamepad.init();
         if (Global::uart_conf.enable)
             input::uart = std::make_unique<uart_input>();
+		if (Global::motiontelemetry_conf.enable)
+			input::motiontelemetry = std::make_unique<motiontelemetry>();
 
 		Global::pWorld = &World;
 		if( false == World.Init( window ) ) {
@@ -398,6 +402,8 @@ int main(int argc, char *argv[])
         while( ( false == glfwWindowShouldClose( window ) )
             && ( true == World.Update() )
             && ( true == GfxRenderer.Render() ) ) {
+			if (input::motiontelemetry)
+				input::motiontelemetry->update();
             glfwPollEvents();
             input::Keyboard.poll();
             if (input::uart)

Globals.cpp

@@ -166,6 +166,7 @@ double Global::fTimeSpeed = 1.0; // przyspieszenie czasu, zmienna do testów
 bool Global::bHideConsole = false; // hunter-271211: ukrywanie konsoli
 
 Global::uart_conf_t Global::uart_conf;
+motiontelemetry::conf_t Global::motiontelemetry_conf;
 
 //randomizacja
 std::mt19937 Global::random_engine = std::mt19937(std::time(NULL));
@@ -777,6 +778,15 @@ void Global::ConfigParse(cParser &Parser)
 			Parser.getTokens(1);
 			Parser >> Global::uart_conf.debug;
 		}
+		else if (token == "motiontelemetry")
+		{
+			Parser.getTokens(4);
+			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;
+		}
     } while ((token != "") && (token != "endconfig")); //(!Parser->EndOfFile)
     // na koniec trochę zależności
     if (!bLoadTraction) // wczytywanie drutów i słupów

Globals.h

@@ -12,6 +12,7 @@ http://mozilla.org/MPL/2.0/.
 #include <string>
 #include "renderer.h"
 #include "dumb3d.h"
+#include "motiontelemetry.h"
 
 // definicje klawiszy
 const int k_IncMainCtrl = 0; //[Num+]
@@ -311,6 +312,7 @@ public:
 		bool debug = false;
     };
     static uart_conf_t uart_conf;
+	static motiontelemetry::conf_t motiontelemetry_conf;
 
 	static opengl_light DayLight;
 

Train.h

@@ -492,6 +492,8 @@ private:
     inline TDynamicObject const *Dynamic() const { return DynamicObject; };
     inline TMoverParameters *Controlled() { return mvControlled; };
     inline TMoverParameters const *Controlled() const { return mvControlled; };
+	inline TMoverParameters *Occupied() { return mvOccupied; };
+	inline TMoverParameters const *Occupied() const { return mvOccupied; };
     void DynamicSet(TDynamicObject *d);
     void Silence();
 

motiontelemetry.cpp

@@ -0,0 +1,68 @@
+#include "motiontelemetry.h"
+#include "Globals.h"
+#include "Logs.h"
+#include "Train.h"
+#include "Timer.h"
+#include "Driver.h"
+#include <ws2tcpip.h>
+
+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, *res;
+	memset(&hints, 0, sizeof(hints));
+
+	protoent *pe = getprotobyname(conf.proto.c_str());
+	if (!pe)
+		throw std::runtime_error("motiontelemetry: unknown protocol");
+	hints.ai_family = AF_UNSPEC;
+	hints.ai_socktype = 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;
+
+	TTrain *t = Global::pWorld->train();
+	if (!t)
+		return;
+
+	float speed = std::abs(t->Occupied()->V);
+	float forward = t->Occupied()->AccS;
+	float lateral = t->Occupied()->AccN;
+	float vertical = t->Occupied()->AccV;
+	float 
+
+	WriteLog("forward: " + std::to_string(forward) + ", lateral: " + std::to_string(lateral) + ", z: " + std::to_string(vertical));
+}

motiontelemetry.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <string>
+#include <chrono>
+
+#ifdef _WIN32
+#include <winsock2.h>
+#endif
+
+class motiontelemetry
+{
+public:
+	struct conf_t
+	{
+		bool enable;
+		std::string proto;
+		std::string address;
+		std::string port;
+		float updatetime;
+	};
+
+private:
+	int sock;
+	std::chrono::time_point<std::chrono::high_resolution_clock> last_update;
+	conf_t conf;
+
+public:
+	motiontelemetry();
+	~motiontelemetry();
+	void update();
+};

stdafx.h

@@ -24,6 +24,7 @@
 #ifdef _WINDOWS
 #include "targetver.h"
 #define NOMINMAX
+#define WIN32_LEAN_AND_MEAN
 #include <windows.h>
 #include <shlobj.h>
 #undef NOMINMAX