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

spg4 into navigation
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-08-07 11:23:10 +02:00
parent f9befaebd0
commit 2a0b139f70
2 changed files with 42 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
mission/controller/acs/Navigation.cpp
View File

@ -1,6 +1,5 @@
#include "Navigation.h"
#include <fsfw/coordinates/Sgp4Propagator.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
@ -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);
}

15
mission/controller/acs/Navigation.h
View File

@ -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 <fsfw/coordinates/Sgp4Propagator.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/MultiplicativeKalmanFilter.h>
#include <mission/controller/acs/SensorProcessing.h>
#include <mission/controller/acs/SensorValues.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
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_ */