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

Use SGP4 Propagator for GPS #770

Merged
muellerr merged 33 commits from use-sgp4-propagator into main 2023-08-14 15:47:33 +02:00
Conversation 0 Commits 33 Files Changed 14 +34 -11
5 changed files with 34 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit e2cce1cb51 - Show all commits

2
mission/acs/defs.h
View File

@ -71,6 +71,8 @@ static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(6, severity::HIG
//! failed.
//! P1: Missing information about magnetic field, P2: Missing information about rotational rate
static constexpr Event SAFE_MODE_CONTROLLER_FAILURE = MAKE_EVENT(7, severity::HIGH);
//! [EXPORT] : [COMMENT] The TLE for the SPG4 Propagator has become too old.
static constexpr Event TLE_TOO_OLD = MAKE_EVENT(8, severity::INFO);
extern const char* getModeStr(AcsMode mode);

36
mission/controller/AcsController.cpp
View File

@ -167,13 +167,19 @@ void AcsController::performSafe() {
timeval now;
Clock::getClock_timeval(&now);
navigation.useSpg4(now, &gpsDataProcessed);
ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
triggerEvent(acs::TLE_TOO_OLD);
tleTooOldFlag = true;
} else if (result != Sgp4Propagator::TLE_TOO_OLD) {
tleTooOldFlag = false;
}
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
&gyrDataProcessed, &fusedRotRateData);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
@ -293,13 +299,19 @@ void AcsController::performDetumble() {
timeval now;
Clock::getClock_timeval(&now);
navigation.useSpg4(now, &gpsDataProcessed);
ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
triggerEvent(acs::TLE_TOO_OLD);
tleTooOldFlag = true;
} else {
tleTooOldFlag = false;
}
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
&gyrDataProcessed, &fusedRotRateData);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
@ -380,11 +392,17 @@ void AcsController::performPointingCtrl() {
timeval now;
Clock::getClock_timeval(&now);
navigation.useSpg4(now, &gpsDataProcessed);
ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
triggerEvent(acs::TLE_TOO_OLD);
tleTooOldFlag = true;
} else {
tleTooOldFlag = false;
}
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
mekfInvalidCounter++;

2
mission/controller/AcsController.h
View File

@ -3,6 +3,7 @@
#include <eive/objects.h>
#include <fsfw/controller/ExtendedControllerBase.h>
#include <fsfw/coordinates/Sgp4Propagator.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/health/HealthTable.h>
#include <fsfw/parameters/ParameterHelper.h>
@ -61,6 +62,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
ParameterHelper parameterHelper;
bool tleTooOldFlag = false;
uint8_t detumbleCounter = 0;
uint8_t multipleRwUnavailableCounter = 0;
bool mekfInvalidFlag = false;

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

@ -45,7 +45,7 @@ void Navigation::resetMekf(acsctrl::MekfData *mekfData) {
mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
}
void Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed) {
ReturnValue_t 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);
@ -75,6 +75,7 @@ void Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcesse
}
}
}
return result;
}
ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) {

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

@ -21,7 +21,7 @@ class Navigation {
AcsParameters *acsParameters);
void resetMekf(acsctrl::MekfData *mekfData);
void useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);
ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);
ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2);
protected: