added fdir for unrealistic gps altitudes
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
This commit is contained in:
parent
d8a07312f2
commit
fd436dbe8b
@ -588,6 +588,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
|
||||
// GPS Processed
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
|
||||
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0});
|
||||
|
@ -189,6 +189,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||
acsctrl::GpsDataProcessed gpsDataProcessed;
|
||||
PoolEntry<double> gcLatitude = PoolEntry<double>();
|
||||
PoolEntry<double> gdLongitude = PoolEntry<double>();
|
||||
PoolEntry<double> altitude = PoolEntry<double>();
|
||||
PoolEntry<double> gpsPosition = PoolEntry<double>(3);
|
||||
PoolEntry<double> gpsVelocity = PoolEntry<double>(3);
|
||||
|
||||
|
@ -591,6 +591,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
case 0x0:
|
||||
parameterWrapper->set(gpsParameters.timeDiffVelocityMax);
|
||||
break;
|
||||
case 0x1:
|
||||
parameterWrapper->set(gpsParameters.minimumFdirAltitude);
|
||||
break;
|
||||
case 0x2:
|
||||
parameterWrapper->set(gpsParameters.maximumFdirAltitude);
|
||||
break;
|
||||
case 0x3:
|
||||
parameterWrapper->set(gpsParameters.fdirAltitude);
|
||||
break;
|
||||
default:
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
}
|
||||
|
@ -772,7 +772,7 @@ class AcsParameters : public HasParametersIF {
|
||||
double gyr2bias[3] = {0.15039212820512823, 0.7094475589743591, -0.22298363589743594};
|
||||
double gyr3bias[3] = {0.0021730769230769217, -0.6655897435897435, 0.034096153846153845};
|
||||
|
||||
/* var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
|
||||
/* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
|
||||
* assumed to be equal for the same class of sensors */
|
||||
float gyr02variance[3] = {pow(3.0e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
|
||||
pow(3.0e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
|
||||
@ -886,7 +886,10 @@ class AcsParameters : public HasParametersIF {
|
||||
} strParameters;
|
||||
|
||||
struct GpsParameters {
|
||||
double timeDiffVelocityMax = 30; //[s]
|
||||
double timeDiffVelocityMax = 30; // [s]
|
||||
double minimumFdirAltitude = 475 * 1e3; // [m]
|
||||
double maximumFdirAltitude = 575 * 1e3; // [m]
|
||||
double fdirAltitude = 525 * 1e3; // [m]
|
||||
} gpsParameters;
|
||||
|
||||
struct SunModelParameters {
|
||||
|
@ -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);
|
||||
|
@ -1,17 +1,16 @@
|
||||
#ifndef SENSORVALUES_H_
|
||||
#define SENSORVALUES_H_
|
||||
|
||||
#include <fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h>
|
||||
#include <fsfw_hal/devicehandlers/MgmRM3100Handler.h>
|
||||
#include <linux/devices/devicedefinitions/StarTrackerDefinitions.h>
|
||||
#include <mission/devices/devicedefinitions/GPSDefinitions.h>
|
||||
#include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
|
||||
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
|
||||
#include <mission/devices/devicedefinitions/imtqHelpers.h>
|
||||
#include <mission/devices/devicedefinitions/rwHelpers.h>
|
||||
#include <mission/devices/devicedefinitions/susMax1227Helpers.h>
|
||||
|
||||
#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h"
|
||||
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
|
||||
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
|
||||
#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
|
||||
#include "mission/devices/devicedefinitions/GPSDefinitions.h"
|
||||
|
||||
namespace ACS {
|
||||
|
||||
class SensorValues {
|
||||
|
@ -86,6 +86,7 @@ enum PoolIds : lp_id_t {
|
||||
// GPS Processed
|
||||
GC_LATITUDE,
|
||||
GD_LONGITUDE,
|
||||
ALTITUDE,
|
||||
GPS_POSITION,
|
||||
GPS_VELOCITY,
|
||||
// MEKF
|
||||
@ -228,6 +229,7 @@ class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
|
||||
|
||||
lp_var_t<double> gcLatitude = lp_var_t<double>(sid.objectId, GC_LATITUDE, this);
|
||||
lp_var_t<double> gdLongitude = lp_var_t<double>(sid.objectId, GD_LONGITUDE, this);
|
||||
lp_var_t<double> altitude = lp_var_t<double>(sid.objectId, ALTITUDE, this);
|
||||
lp_vec_t<double, 3> gpsPosition = lp_vec_t<double, 3>(sid.objectId, GPS_POSITION, this);
|
||||
lp_vec_t<double, 3> gpsVelocity = lp_vec_t<double, 3>(sid.objectId, GPS_VELOCITY, this);
|
||||
|
||||
|
Loading…
x
Reference in New Issue
Block a user