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 +#include 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(payload[4] | (payload[5] << 8) + | (payload[6] << 16) | (payload[7] << 24)); + sint32 longitude = static_cast(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 -