eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

handling of which GPS result to use
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-08-07 16:44:03 +02:00
parent dfa20545e4
commit fa3d5cbc3e
2 changed files with 32 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
mission/controller/acs/SensorProcessing.cpp
View File

@ -535,14 +535,30 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
const bool validGps, const bool validGps,
const AcsParameters::GpsParameters *gpsParameters, const AcsParameters::GpsParameters *gpsParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed) { 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}; gpsVelocityE[3] = {0, 0, 0};
if (validGps) { uint8_t gpsSource = acs::GpsSource::NONE;
// Transforming from Degree to Radians and calculation geocentric lattitude from geodetic // We do not trust the GPS and therefore it shall die here if SPG4 is running
if (gpsDataProcessed->source.value == acs::GpsSource::SPG4) {
MathOperations<double>::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.; gdLongitude = gpsLongitude * PI / 180.;
double latitudeRad = gpsLatitude * PI / 180.; double latitudeRad = gpsLatitude * PI / 180.;
double eccentricityWgs84 = 0.0818195; double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
double factor = 1 - pow(eccentricityWgs84, 2);
gcLatitude = atan(factor * tan(latitudeRad)); gcLatitude = atan(factor * tan(latitudeRad));
// Altitude FDIR // Altitude FDIR
@ -569,6 +585,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
timeOfSavedPosSatE = gpsUnixSeconds; timeOfSavedPosSatE = gpsUnixSeconds;
validSavedPosSatE = true; validSavedPosSatE = true;
gpsSource = acs::GpsSource::GPS;
} }
{ {
PoolReadGuard pg(gpsDataProcessed); 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->gpsPosition.value, posSatE, 3 * sizeof(double));
std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double)); std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double));
gpsDataProcessed->setValidity(validGps, true); gpsDataProcessed->setValidity(validGps, true);
gpsDataProcessed->source.value = gpsSource;
gpsDataProcessed->source.setValid(true);
} }
} }
} }

15
mission/controller/acs/SensorProcessing.h
View File

@ -1,15 +1,13 @@
#ifndef SENSORPROCESSING_H_ #ifndef SENSORPROCESSING_H_
#define SENSORPROCESSING_H_ #define SENSORPROCESSING_H_
#include <common/config/eive/resultClassIds.h>
#include <fsfw/returnvalues/returnvalue.h> #include <fsfw/returnvalues/returnvalue.h>
#include <stdint.h> //uint8_t #include <mission/acs/defs.h>
#include <time.h> /*purpose, timeval ?*/ #include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include "../controllerdefinitions/AcsCtrlDefinitions.h" #include <mission/controller/acs/SusConverter.h>
#include "AcsParameters.h" #include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include "SensorValues.h"
#include "SusConverter.h"
#include "eive/resultClassIds.h"
class SensorProcessing { class SensorProcessing {
public: public:
@ -25,6 +23,7 @@ class SensorProcessing {
private: private:
static constexpr float ZERO_VEC_F[3] = {0, 0, 0}; static constexpr float ZERO_VEC_F[3] = {0, 0, 0};
static constexpr double ZERO_VEC_D[3] = {0, 0, 0}; static constexpr double ZERO_VEC_D[3] = {0, 0, 0};
static constexpr double ECCENTRICITY_WGS84 = 0.0818195;
protected: protected:
// short description needed for every function // short description needed for every function