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

added fdir for unrealistic gps altitudes
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-03-07 11:10:47 +01:00
parent d8a07312f2
commit fd436dbe8b
7 changed files with 35 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -550,8 +550,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
const bool validGps,
const AcsParameters::GpsParameters *gpsParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed) {
// name to convert not process
double gdLongitude = 0, gcLatitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0};
double gdLongitude = 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
gdLongitude = gpsLongitude * PI / 180.;
@ -560,9 +560,17 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
double factor = 1 - pow(eccentricityWgs84, 2);
gcLatitude = atan(factor * tan(latitudeRad));
// Altitude FDIR
if (gpsAltitude > gpsParameters->maximumFdirAltitude ||
gpsAltitude < gpsParameters->maximumFdirAltitude) {
altitude = gpsParameters->fdirAltitude;
} else {
altitude = gpsAltitude;
}
// Calculation of the satellite velocity in earth fixed frame
double deltaDistance[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, gpsAltitude, posSatE);
MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
if (validSavedPosSatE &&
(gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
@ -581,6 +589,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->gdLongitude.value = gdLongitude;
gpsDataProcessed->gcLatitude.value = gcLatitude;
gpsDataProcessed->altitude.value = altitude;
std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double));
std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double));
gpsDataProcessed->setValidity(validGps, true);