diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 7d60a531..3b7d1cc1 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -1,6 +1,5 @@ #include "Navigation.h" -#include #include #include #include @@ -45,3 +44,35 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, void Navigation::resetMekf(acsctrl::MekfData *mekfData) { mekfStatus = multiplicativeKalmanFilter.reset(mekfData); } + +void Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed) { + double position[3] = {0, 0, 0}; + double velocity[3] = {0, 0, 0}; + ReturnValue_t result = sgp4Propagator.propagate(position, velocity, now, 0); + + if (result == returnvalue::OK) { + { + PoolReadGuard pg(gpsDataProcessed); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double)); + gpsDataProcessed->gpsPosition.setValid(true); + std::memcpy(gpsDataProcessed->gpsVelocity.value, velocity, 3 * sizeof(double)); + gpsDataProcessed->gpsVelocity.setValid(true); + } + } + } else { + { + PoolReadGuard pg(gpsDataProcessed); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double)); + gpsDataProcessed->gpsPosition.setValid(false); + std::memcpy(gpsDataProcessed->gpsVelocity.value, velocity, 3 * sizeof(double)); + gpsDataProcessed->gpsVelocity.setValid(false); + } + } + } +} + +ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) { + return sgp4Propagator.initialize(line1, line2); +} diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index b567fbdd..4dd2ceef 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -1,11 +1,12 @@ #ifndef NAVIGATION_H_ #define NAVIGATION_H_ -#include "../controllerdefinitions/AcsCtrlDefinitions.h" -#include "AcsParameters.h" -#include "MultiplicativeKalmanFilter.h" -#include "SensorProcessing.h" -#include "SensorValues.h" +#include +#include +#include +#include +#include +#include class Navigation { public: @@ -19,10 +20,14 @@ class Navigation { AcsParameters *acsParameters); void resetMekf(acsctrl::MekfData *mekfData); + void useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed); + ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2); + protected: private: MultiplicativeKalmanFilter multiplicativeKalmanFilter; ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::MEKF_UNINITIALIZED; + Sgp4Propagator sgp4Propagator; }; #endif /* ACS_NAVIGATION_H_ */