eive-obsw/mission/controller/acs/Navigation.cpp

82 lines
3.6 KiB
C++
Raw Normal View History

2022-09-20 14:15:55 +02:00
#include "Navigation.h"
2022-09-27 11:06:11 +02:00
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>
2022-09-27 11:06:11 +02:00
Navigation::Navigation() {}
2022-09-20 14:15:55 +02:00
Navigation::~Navigation() {}
ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed,
2023-11-27 10:37:40 +01:00
acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters) {
double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
2023-06-05 09:37:01 +02:00
bool quatIBValid = sensorValues->strSet.isTrustWorthy.value;
2023-02-23 11:26:43 +01:00
if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
mekfStatus = multiplicativeKalmanFilter.init(
mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData,
acsParameters);
return mekfStatus;
} else {
mekfStatus = multiplicativeKalmanFilter.mekfEst(
quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value,
gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value,
mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
mgmDataProcessed->magIgrfModel.isValid(), mekfData, acsParameters);
return mekfStatus;
}
2022-09-20 14:15:55 +02:00
}
2023-11-23 16:56:36 +01:00
void Navigation::resetMekf(acsctrl::AttitudeEstimationData *mekfData) {
mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
}
2023-08-07 11:23:10 +02:00
2023-08-09 13:41:25 +02:00
ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed) {
2023-08-07 11:23:10 +02:00
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) {
2023-11-27 10:37:40 +01:00
gpsDataProcessed->source = acs::gps::Source::SPG4;
2023-08-07 13:37:05 +02:00
gpsDataProcessed->source.setValid(true);
2023-08-07 11:23:10 +02:00
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) {
2023-11-27 10:37:40 +01:00
gpsDataProcessed->source = acs::gps::Source::NONE;
2023-08-07 13:37:05 +02:00
gpsDataProcessed->source.setValid(true);
2023-08-07 11:23:10 +02:00
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);
}
}
}
2023-08-09 13:41:25 +02:00
return result;
2023-08-07 11:23:10 +02:00
}
ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) {
return sgp4Propagator.initialize(line1, line2);
}