From 4475ae6df8fbabdf6afec12e9e7b96af8813e50c Mon Sep 17 00:00:00 2001
From: Korbinian Ederer <korbinian.ederer@in.tum.de>
Date: Fri, 8 Jul 2016 14:47:27 +0200
Subject: [PATCH] added cpp files

---
 2016/ppc/gps/include/gps/GpsConverter.h |  8 +++-
 2016/ppc/gps/src/gps/GpsConverter.cpp   | 59 ++++++++++++++++++-------
 2 files changed, 51 insertions(+), 16 deletions(-)

diff --git a/2016/ppc/gps/include/gps/GpsConverter.h b/2016/ppc/gps/include/gps/GpsConverter.h
index b353bdc..086d9ed 100644
--- a/2016/ppc/gps/include/gps/GpsConverter.h
+++ b/2016/ppc/gps/include/gps/GpsConverter.h
@@ -37,10 +37,16 @@ public:
 private:
 	can::ICANTransceiver& fCanTransceiver;
 	can::IntervalFilter	fCanFilter;
-
+	
+	virtual bool checkValid(sint32 value);
+	
 	IGpsACPusher& fAcPusher;
 	sint32 fLastLatInMs;
 	sint32 fLastLongInMs;
+	sint32 longitude;
+	sint32 latitude;
+	
+
 };
 
 } // namespace gps
diff --git a/2016/ppc/gps/src/gps/GpsConverter.cpp b/2016/ppc/gps/src/gps/GpsConverter.cpp
index acf60ab..aed8e01 100644
--- a/2016/ppc/gps/src/gps/GpsConverter.cpp
+++ b/2016/ppc/gps/src/gps/GpsConverter.cpp
@@ -9,32 +9,56 @@
 #include "can/canframes/CANFrame.h"
 #include "can/ICANTransceiver.h"
 #include <stdio.h>
+#include <math.h>
 
 using namespace can;
 
-namespace gps
-{
+namespace gps {
 
-GpsConverter::GpsConverter(
-		ICANTransceiver& transceiver,
-		IGpsACPusher& acPusher)
-	: fCanTransceiver(transceiver)
-	, fCanFilter(GPS_FRAME_ID, GPS_FRAME_ID)
-	, fAcPusher(acPusher)
-{
+GpsConverter::GpsConverter(ICANTransceiver& transceiver, IGpsACPusher& acPusher) :
+		fCanTransceiver(transceiver), fCanFilter(GPS_FRAME_ID, GPS_FRAME_ID), fAcPusher(
+				acPusher) {
 }
 
-void GpsConverter::frameReceived(const CANFrame& canFrame)
-{
+void GpsConverter::frameReceived(const CANFrame& canFrame) {
 	const uint8* payload = canFrame.getPayload();
-    // TOOD implement conversion to arc-msec and call IGpsACPusher
+	// TODO implement conversion to arc-msec and call IGpsACPusher
 
 	sint32 latInMs = 0; // here add your converted lat
 	sint32 longInMs = 0; // here add your converted long
 
+	//Building the coordiantes out of the payload
+	sint32 latitude = static_cast<sint32>(payload[4] | (payload[5] << 8)
+			| (payload[6] << 16) | (payload[7] << 24));
+	sint32 longitude = static_cast<sint32>(payload[0] | (payload[1] << 8)
+			| (payload[2] << 16) | (payload[3] << 24));
 
-	if (latInMs != fLastLatInMs || longInMs != fLastLongInMs)
-	{
+	printf("lat raw: %d, long raw: %d\n", latitude, longitude);
+
+	//check for valid Data
+	if (!checkValid(latitude) && !checkValid(longitude)) {
+		return;
+	}
+
+	//calculate raw to angle factor with scaling for precission
+	const sint32 fak = 180 * 9000000000 / (pow(2, 31) - 1);
+	//calculate descaling factor including the conversion to degree ms
+	const sint32 divfak = 9000000000 / 3600000;
+	printf("fak: %d\n", fak);
+
+	//division first because of limited length of variable
+	latInMs = latitude/divfak;
+	longInMs = longitude/divfak;
+
+	printf("lat: %d, long: %d\n", latInMs, longInMs);
+
+	latInMs *= fak;
+	longInMs *= fak;
+
+	printf("lat: %d, long: %d\n", latInMs, longInMs);
+
+
+	if (latInMs != fLastLatInMs || longInMs != fLastLongInMs) {
 		// value changed
 		fAcPusher.pushGPSCoordinates(latInMs, longInMs);
 		fLastLatInMs = latInMs;
@@ -42,7 +66,12 @@ void GpsConverter::frameReceived(const CANFrame& canFrame)
 	}
 }
 
+bool GpsConverter::checkValid(sint32 value) {
+	if (value == 0x7fffffff || value == 0xffffffff || value == 0x80000000) {
+		return false;
+	}
+	return true;
+}
 
 } // namespace gps
 
-