eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 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
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 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<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.;
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);
}
}
}