diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 511cae35..d7e6794e 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -535,14 +535,30 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong const bool validGps, const AcsParameters::GpsParameters *gpsParameters, acsctrl::GpsDataProcessed *gpsDataProcessed) { - double gdLongitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0}, + // init variables + double gdLongitude = 0, gdLatitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0}; - if (validGps) { - // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic + uint8_t gpsSource = acs::GpsSource::NONE; + // We do not trust the GPS and therefore it shall die here if SPG4 is running + if (gpsDataProcessed->source.value == acs::GpsSource::SPG4) { + MathOperations::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude, + gdLongitude, altitude); + double factor = 1 - pow(ECCENTRICITY_WGS84, 2); + gcLatitude = atan(factor * tan(gdLatitude)); + { + PoolReadGuard pg(gpsDataProcessed); + if (pg.getReadResult() == returnvalue::OK) { + gpsDataProcessed->gdLongitude.value = gdLongitude; + gpsDataProcessed->gcLatitude.value = gcLatitude; + gpsDataProcessed->altitude.value = altitude; + gpsDataProcessed->setValidity(true, true); + } + } + } else if (validGps) { + // Transforming from Degree to Radians and calculation geocentric latitude from geodetic gdLongitude = gpsLongitude * PI / 180.; double latitudeRad = gpsLatitude * PI / 180.; - double eccentricityWgs84 = 0.0818195; - double factor = 1 - pow(eccentricityWgs84, 2); + double factor = 1 - pow(ECCENTRICITY_WGS84, 2); gcLatitude = atan(factor * tan(latitudeRad)); // Altitude FDIR @@ -569,6 +585,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong timeOfSavedPosSatE = gpsUnixSeconds; validSavedPosSatE = true; + + gpsSource = acs::GpsSource::GPS; } { PoolReadGuard pg(gpsDataProcessed); @@ -579,6 +597,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double)); std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double)); gpsDataProcessed->setValidity(validGps, true); + gpsDataProcessed->source.value = gpsSource; + gpsDataProcessed->source.setValid(true); } } } diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 6dbc5d58..7bd88fa2 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -1,15 +1,13 @@ #ifndef SENSORPROCESSING_H_ #define SENSORPROCESSING_H_ +#include #include -#include //uint8_t -#include /*purpose, timeval ?*/ - -#include "../controllerdefinitions/AcsCtrlDefinitions.h" -#include "AcsParameters.h" -#include "SensorValues.h" -#include "SusConverter.h" -#include "eive/resultClassIds.h" +#include +#include +#include +#include +#include class SensorProcessing { public: @@ -25,6 +23,7 @@ class SensorProcessing { private: static constexpr float ZERO_VEC_F[3] = {0, 0, 0}; static constexpr double ZERO_VEC_D[3] = {0, 0, 0}; + static constexpr double ECCENTRICITY_WGS84 = 0.0818195; protected: // short description needed for every function