74 lines
3.1 KiB
C++
74 lines
3.1 KiB
C++
#include "Navigation.h"
|
|
|
|
Navigation::Navigation(AcsParameters *acsParameters) : multiplicativeKalmanFilter(acsParameters) {}
|
|
|
|
Navigation::~Navigation() {}
|
|
|
|
ReturnValue_t Navigation::useMekf(const ACS::SensorValues *sensorValues,
|
|
const acsctrl::GyrDataProcessed *gyrDataProcessed,
|
|
const acsctrl::MgmDataProcessed *mgmDataProcessed,
|
|
const acsctrl::SusDataProcessed *susDataProcessed,
|
|
const double timeDelta,
|
|
acsctrl::AttitudeEstimationData *attitudeEstimationData,
|
|
const bool allowStr) {
|
|
multiplicativeKalmanFilter.setStrData(
|
|
sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
|
|
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value,
|
|
sensorValues->strSet.caliQx.isValid(), allowStr);
|
|
|
|
if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
|
|
mekfStatus = multiplicativeKalmanFilter.init(susDataProcessed, mgmDataProcessed,
|
|
gyrDataProcessed, attitudeEstimationData);
|
|
return mekfStatus;
|
|
} else {
|
|
mekfStatus = multiplicativeKalmanFilter.mekfEst(
|
|
susDataProcessed, mgmDataProcessed, gyrDataProcessed, timeDelta, attitudeEstimationData);
|
|
return mekfStatus;
|
|
}
|
|
}
|
|
|
|
void Navigation::resetMekf(acsctrl::AttitudeEstimationData *mekfData) {
|
|
mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
|
|
}
|
|
|
|
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);
|
|
|
|
if (result == returnvalue::OK) {
|
|
{
|
|
PoolReadGuard pg(gpsDataProcessed);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
gpsDataProcessed->source = acs::gps::Source::SPG4;
|
|
gpsDataProcessed->source.setValid(true);
|
|
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) {
|
|
gpsDataProcessed->source = acs::gps::Source::NONE;
|
|
gpsDataProcessed->source.setValid(true);
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) {
|
|
return sgp4Propagator.initialize(line1, line2);
|
|
}
|
|
|
|
void Navigation::updateMekfStandardDeviations(const AcsParameters *acsParameters) {
|
|
multiplicativeKalmanFilter.updateStandardDeviations(acsParameters);
|
|
}
|