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

added Gps Velocity Calculation
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details

Browse Source
This commit is contained in:
Robin Marquardt
2022-10-21 14:23:31 +02:00
parent cc35db91cd
commit ffc7a55763
5 changed files with 39 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -454,7 +454,9 @@ void SensorProcessing::processGyr(
}
void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude,
const bool validGps, double *gcLatitude, double *gdLongitude) {
const double gps0altitude, const uint32_t gps0UnixSeconds,
const bool validGps, const AcsParameters::GpsParameters *gpsParameters,
double *gcLatitude, double *gdLongitude, double *gpsVelocityE) {
// name to convert not process
if (validGps) {
// Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
@ -464,6 +466,25 @@ void SensorProcessing::processGps(const double gps0latitude, const double gps0lo
double factor = 1 - pow(eccentricityWgs84, 2);
*gcLatitude = atan(factor * tan(latitudeRad));
validGcLatitude = true;
// Calculation of the satellite velocity in earth fixed frame
double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0};
// RADIANS OR DEGREE ? Function needs rad as input
MathOperations<double>::cartesianFromLatLongAlt(gps0latitude, gps0longitude, gps0altitude, posSatE);
if (validSavedPosSatE && (gps0UnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
double timeDiffGpsMeas = gps0UnixSeconds - timeOfSavedPosSatE;
VectorOperations<double>::mulScalar(deltaDistance, 1/timeDiffGpsMeas, gpsVelocityE, 3);
}
savedPosSatE[0] = posSatE[0];
savedPosSatE[1] = posSatE[1];
savedPosSatE[2] = posSatE[2];
timeOfSavedPosSatE = gps0UnixSeconds;
validSavedPosSatE = true;
}
else {
validGcLatitude = false;
}
}
@ -472,7 +493,10 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
const AcsParameters *acsParameters) {
sensorValues->update();
processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
sensorValues->gpsSet.isValid(), &outputValues->gcLatitude, &outputValues->gdLongitude);
sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value,
sensorValues->gpsSet.isValid(), &acsParameters->gpsParameters,
&outputValues->gcLatitude, &outputValues->gdLongitude,
&outputValues->gpsVelocity);
outputValues->mgmUpdated = processMgm(
sensorValues->mgm0Lis3Set.fieldStrengths.value,