eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues4 Pull Requests1 Releases121 Wiki Activity

added fdir for unrealistic gps altitudes

Browse Source
This commit is contained in:
Marius Eggert 2023-03-07 11:10:47 +01:00
parent d8a07312f2
commit fd436dbe8b
7 changed files with 35 additions and 11 deletions

1
mission/controller/AcsController.cpp

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

1
mission/controller/AcsController.h

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

9
mission/controller/acs/AcsParameters.cpp

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

7
mission/controller/acs/AcsParameters.h

@ -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 {

15
mission/controller/acs/SensorProcessing.cpp

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

11
mission/controller/acs/SensorValues.h

@ -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 {

2
mission/controller/controllerdefinitions/AcsCtrlDefinitions.h

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