diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1f9df068..362a5e1b 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -421,6 +421,7 @@ void AcsController::performControlOperation() { // GPS Processed localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); + localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition); localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); // MEKF diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 3ffcde85..64f5c09f 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -153,6 +153,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes acsctrl::GpsDataProcessed gpsDataProcessed; PoolEntry gcLatitude = PoolEntry(); PoolEntry gdLongitude = PoolEntry(); + PoolEntry gpsPosition = PoolEntry(3); PoolEntry gpsVelocity = PoolEntry(3); // MEKF diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 7e49fcdd..5d68bc44 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -546,7 +546,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong const AcsParameters::GpsParameters *gpsParameters, acsctrl::GpsDataProcessed *gpsDataProcessed) { // 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) { // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic gdLongitude = gpsLongitude * PI / 180.; @@ -576,11 +576,9 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong if (pg.getReadResult() == returnvalue::OK) { gpsDataProcessed->gdLongitude.value = gdLongitude; gpsDataProcessed->gcLatitude.value = gcLatitude; - gpsDataProcessed->setValidity(validGps, validGps); - if (!validGps) { - gpsDataProcessed->gdLongitude.value = 0.0; - gpsDataProcessed->gcLatitude.value = 0.0; - } + std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double)); + std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double)); + gpsDataProcessed->setValidity(validGps, true); } } } diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index ab5ed8da..0955581b 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -86,6 +86,7 @@ enum PoolIds : lp_id_t { // GPS Processed GC_LATITUDE, GD_LONGITUDE, + GPS_POSITION, GPS_VELOCITY, // 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 GYR_SET_RAW_ENTRIES = 4; 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 CTRL_VAL_SET_ENTRIES = 3; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; @@ -225,6 +226,7 @@ class GpsDataProcessed : public StaticLocalDataSet { lp_var_t gcLatitude = lp_var_t(sid.objectId, GC_LATITUDE, this); lp_var_t gdLongitude = lp_var_t(sid.objectId, GD_LONGITUDE, this); + lp_vec_t gpsPosition = lp_vec_t(sid.objectId, GPS_POSITION, this); lp_vec_t gpsVelocity = lp_vec_t(sid.objectId, GPS_VELOCITY, this); private: