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 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);
}
}
}

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

@ -1,15 +1,13 @@
#ifndef SENSORPROCESSING_H_
#define SENSORPROCESSING_H_
#include <common/config/eive/resultClassIds.h>
#include <fsfw/returnvalues/returnvalue.h>
#include <stdint.h> //uint8_t
#include <time.h> /*purpose, timeval ?*/
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h"
#include "SensorValues.h"
#include "SusConverter.h"
#include "eive/resultClassIds.h"
#include <mission/acs/defs.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <mission/controller/acs/SusConverter.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
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