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

fixed gps velocity merge aftermath

Browse Source
This commit is contained in:
Marius Eggert 2022-12-14 10:04:37 +01:00
parent b80ecfb600
commit d5677c20f7
2 changed files with 22 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -559,29 +559,35 @@ void SensorProcessing::processGyr(
} }
void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude, void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude,
const double gpsAltitude, const double gpsUnixSeconds,
const bool validGps, const bool validGps,
const AcsParameters::GpsParameters *gpsParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed) { acsctrl::GpsDataProcessed *gpsDataProcessed) {
// name to convert not process // name to convert not process
double gdLongitude, gcLatitude; double gdLongitude, gcLatitude;
if (validGps) { if (validGps) {
// Transforming from Degree to Radians and calculation geocentric lattitude from geodetic // Transforming from Degree to Radians and calculation geocentric lattitude 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 eccentricityWgs84 = 0.0818195;
double factor = 1 - pow(eccentricityWgs84, 2); double factor = 1 - pow(eccentricityWgs84, 2);
gcLatitude = atan(factor * tan(latitudeRad)); gcLatitude = atan(factor * tan(latitudeRad));
// Calculation of the satellite velocity in earth fixed frame // Calculation of the satellite velocity in earth fixed frame
double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0}; double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0};
// RADIANS OR DEGREE ? Function needs rad as input MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, gpsAltitude, posSatE);
MathOperations<double>::cartesianFromLatLongAlt(gps0latitude, gps0longitude, gps0altitude,
posSatE);
if (validSavedPosSatE && if (validSavedPosSatE &&
(gps0UnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) { (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3); VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
double timeDiffGpsMeas = gps0UnixSeconds - timeOfSavedPosSatE; double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE;
VectorOperations<double>::mulScalar(deltaDistance, 1 / timeDiffGpsMeas, gpsVelocityE, 3); VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3);
} }
savedPosSatE[0] = posSatE[0];
savedPosSatE[1] = posSatE[1];
savedPosSatE[2] = posSatE[2];
timeOfSavedPosSatE = gpsUnixSeconds;
validSavedPosSatE = true;
} }
{ {
PoolReadGuard pg(gpsDataProcessed); PoolReadGuard pg(gpsDataProcessed);
@ -594,15 +600,6 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
gpsDataProcessed->gcLatitude.value = 0.0; gpsDataProcessed->gcLatitude.value = 0.0;
} }
} }
savedPosSatE[0] = posSatE[0];
savedPosSatE[1] = posSatE[1];
savedPosSatE[2] = posSatE[2];
timeOfSavedPosSatE = gps0UnixSeconds;
validSavedPosSatE = true;
}
else {
validGcLatitude = false;
} }
} }
@ -614,9 +611,11 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
const AcsParameters *acsParameters) { const AcsParameters *acsParameters) {
sensorValues->update(); sensorValues->update();
processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value,
(sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() && (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
sensorValues->gpsSet.altitude.isValid()), sensorValues->gpsSet.altitude.isValid() && sensorValues->gpsSet.altitude.isValid() &&
gpsDataProcessed); sensorValues->gpsSet.unixSeconds.isValid()),
&acsParameters->gpsParameters, gpsDataProcessed);
processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value, processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value,
sensorValues->mgm0Lis3Set.fieldStrengths.isValid(), sensorValues->mgm0Lis3Set.fieldStrengths.isValid(),

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

@ -65,7 +65,9 @@ class SensorProcessing {
void processStr(); void processStr();
void processGps(const double gps0latitude, const double gps0longitude, const bool validGps, void processGps(const double gpsLatitude, const double gpsLongitude, const double gpsAltitude,
const double gpsUnixSeconds, const bool validGps,
const AcsParameters::GpsParameters *gpsParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed); acsctrl::GpsDataProcessed *gpsDataProcessed);
double savedMgmVecTot[3]; double savedMgmVecTot[3];
@ -73,7 +75,6 @@ class SensorProcessing {
double savedSusVecTot[3]; double savedSusVecTot[3];
timeval timeOfSavedSusDirEst; timeval timeOfSavedSusDirEst;
bool validMagField; bool validMagField;
bool validGcLatitude;
double savedPosSatE[3]; double savedPosSatE[3];
uint32_t timeOfSavedPosSatE; uint32_t timeOfSavedPosSatE;