ttomasini/TUM
0
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

added cpp files

Browse source
This commit is contained in:
Korbinian Ederer 2016-07-08 14:47:27 +02:00
parent c84144e1bd
commit 4475ae6df8
2 changed files with 51 additions and 16 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
2016/ppc/gps/include/gps/GpsConverter.h
View file

@ -37,10 +37,16 @@ public:
private: private:
can::ICANTransceiver& fCanTransceiver; can::ICANTransceiver& fCanTransceiver;
can::IntervalFilter fCanFilter; can::IntervalFilter fCanFilter;
virtual bool checkValid(sint32 value);
IGpsACPusher& fAcPusher; IGpsACPusher& fAcPusher;
sint32 fLastLatInMs; sint32 fLastLatInMs;
sint32 fLastLongInMs; sint32 fLastLongInMs;
sint32 longitude;
sint32 latitude;
}; };
} // namespace gps } // namespace gps

59
2016/ppc/gps/src/gps/GpsConverter.cpp
View file

@ -9,32 +9,56 @@
#include "can/canframes/CANFrame.h" #include "can/canframes/CANFrame.h"
#include "can/ICANTransceiver.h" #include "can/ICANTransceiver.h"
#include <stdio.h> #include <stdio.h>
#include <math.h>
using namespace can; using namespace can;
namespace gps namespace gps {
{
GpsConverter::GpsConverter( GpsConverter::GpsConverter(ICANTransceiver& transceiver, IGpsACPusher& acPusher) :
ICANTransceiver& transceiver, fCanTransceiver(transceiver), fCanFilter(GPS_FRAME_ID, GPS_FRAME_ID), fAcPusher(
IGpsACPusher& acPusher) 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(); 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 latInMs = 0; // here add your converted lat
sint32 longInMs = 0; // here add your converted long 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 // value changed
fAcPusher.pushGPSCoordinates(latInMs, longInMs); fAcPusher.pushGPSCoordinates(latInMs, longInMs);
fLastLatInMs = latInMs; 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 } // namespace gps