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

added gpsPosition to gpsProcessedData
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-01-11 13:42:48 +01:00
parent ac83e66016
commit a2626afebb
4 changed files with 9 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
mission/controller/AcsController.cpp
View File

@ -421,6 +421,7 @@ void AcsController::performControlOperation() {
// GPS Processed // GPS Processed
localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0});
// MEKF // MEKF

1
mission/controller/AcsController.h
View File

@ -153,6 +153,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
acsctrl::GpsDataProcessed gpsDataProcessed; acsctrl::GpsDataProcessed gpsDataProcessed;
PoolEntry<double> gcLatitude = PoolEntry<double>(); PoolEntry<double> gcLatitude = PoolEntry<double>();
PoolEntry<double> gdLongitude = PoolEntry<double>(); PoolEntry<double> gdLongitude = PoolEntry<double>();
PoolEntry<double> gpsPosition = PoolEntry<double>(3);
PoolEntry<double> gpsVelocity = PoolEntry<double>(3); PoolEntry<double> gpsVelocity = PoolEntry<double>(3);
// MEKF // MEKF

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

@ -546,7 +546,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
const AcsParameters::GpsParameters *gpsParameters, 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 = 0, gcLatitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0};
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.;
@ -576,11 +576,9 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->gdLongitude.value = gdLongitude; gpsDataProcessed->gdLongitude.value = gdLongitude;
gpsDataProcessed->gcLatitude.value = gcLatitude; gpsDataProcessed->gcLatitude.value = gcLatitude;
gpsDataProcessed->setValidity(validGps, validGps); std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double));
if (!validGps) { std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double));
gpsDataProcessed->gdLongitude.value = 0.0; gpsDataProcessed->setValidity(validGps, true);
gpsDataProcessed->gcLatitude.value = 0.0;
}
} }
} }
} }

4
mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
View File

@ -86,6 +86,7 @@ enum PoolIds : lp_id_t {
// GPS Processed // GPS Processed
GC_LATITUDE, GC_LATITUDE,
GD_LONGITUDE, GD_LONGITUDE,
GPS_POSITION,
GPS_VELOCITY, GPS_VELOCITY,
// MEKF // MEKF
SAT_ROT_RATE_MEKF, SAT_ROT_RATE_MEKF,
@ -106,7 +107,7 @@ static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12;
static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15; static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4; static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5; static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 3; static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4;
static constexpr uint8_t MEKF_SET_ENTRIES = 2; static constexpr uint8_t MEKF_SET_ENTRIES = 2;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
@ -225,6 +226,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> 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> gdLongitude = lp_var_t<double>(sid.objectId, GD_LONGITUDE, 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); lp_vec_t<double, 3> gpsVelocity = lp_vec_t<double, 3>(sid.objectId, GPS_VELOCITY, this);
private: private: