eive/eive-obsw
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
mission/controller/acs/AcsParameters.h
View File

@ -833,6 +833,7 @@ class AcsParameters /*: public HasParametersIF*/ {
} strParameters;
struct GpsParameters {
double timeDiffVelocityMax = 30; //[s]
} gpsParameters;
struct GroundStationParameters {

6
mission/controller/acs/Guidance.cpp
View File

@ -111,9 +111,9 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
// Calculation of reference rotation rate
//-------------------------------------------------------------------------------------
double velSatE[3] = {0, 0, 0};
velSatE[0] = 0.0; // sensorValues->gps0Velocity[0];
velSatE[1] = 0.0; // sensorValues->gps0Velocity[1];
velSatE[2] = 0.0; // sensorValues->gps0Velocity[2];
velSatE[0] = outputValues->gpsVelocity[0]; // sensorValues->gps0Velocity[0];
velSatE[1] = outputValues->gpsVelocity[1]; // sensorValues->gps0Velocity[1];
velSatE[2] = outputValues->gpsVelocity[2]; // sensorValues->gps0Velocity[2];
double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0};
// Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);

1
mission/controller/acs/OutputValues.h
View File

@ -44,6 +44,7 @@ public:
double gcLatitude; // geocentric latitude, radian
double gdLongitude; // Radian longitude
double gpsVelocity[3]; //earth fixed frame
};
}

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,

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

@ -65,8 +65,10 @@ class SensorProcessing {
void processStr();
void processGps(const double gps0latitude, const double gps0longitude, const bool validGps,
double *gcLatitude, double *gdLongitude);
void processGps(const double gps0latitude, const double gps0longitude,
const double gps0altitude, const uint32_t gps0UnixSeconds,
const bool validGps, const AcsParameters::GpsParameters *gpsParameters,
double *gcLatitude, double *gdLongitude, double *gpsVelocityE);
double savedMagFieldEst[3];
timeval timeOfSavedMagFieldEst;
@ -75,6 +77,10 @@ class SensorProcessing {
bool validMagField;
bool validGcLatitude;
double savedPosSatE[3];
uint32_t timeOfSavedPosSatE;
bool validSavedPosSatE;
SusConverter susConverter;
AcsParameters acsParameters;
};