oh my poggers
This commit is contained in:
parent
f0247a9ab3
commit
2f3335403b
@ -45,20 +45,23 @@ enum ControlModeStrategy : uint8_t {
|
|||||||
PTGCTRL_STR = 101,
|
PTGCTRL_STR = 101,
|
||||||
PTGCTRL_QUEST = 102,
|
PTGCTRL_QUEST = 102,
|
||||||
};
|
};
|
||||||
|
namespace gps {
|
||||||
enum GpsSource : uint8_t {
|
enum Source : uint8_t {
|
||||||
NONE,
|
NONE,
|
||||||
GPS,
|
GPS,
|
||||||
GPS_EXTRAPOLATED,
|
GPS_EXTRAPOLATED,
|
||||||
SPG4,
|
SPG4,
|
||||||
};
|
};
|
||||||
|
}
|
||||||
|
|
||||||
enum RotRateSource : uint8_t {
|
namespace rotrate {
|
||||||
ALL_INVALID,
|
enum Source : uint8_t {
|
||||||
|
NONE,
|
||||||
SUSMGM,
|
SUSMGM,
|
||||||
QUEST,
|
QUEST,
|
||||||
STR,
|
STR,
|
||||||
};
|
};
|
||||||
|
}
|
||||||
|
|
||||||
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
|
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
|
||||||
//! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated.
|
//! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated.
|
||||||
|
@ -431,6 +431,10 @@ void AcsController::performPointingCtrl() {
|
|||||||
std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI));
|
std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI));
|
||||||
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
|
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
|
||||||
break;
|
break;
|
||||||
|
default:
|
||||||
|
sif::error << "AcsController: Invalid pointing mode strategy for performDetumble"
|
||||||
|
<< std::endl;
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t enableAntiStiction = true;
|
uint8_t enableAntiStiction = true;
|
||||||
|
@ -26,7 +26,7 @@ void FusedRotationEstimation::estimateFusedRotationRate(
|
|||||||
std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
|
std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
|
||||||
fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double));
|
fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double));
|
||||||
fusedRotRateData->rotRateOrthogonal.setValid(true);
|
fusedRotRateData->rotRateOrthogonal.setValid(true);
|
||||||
fusedRotRateData->rotRateSource.value = acs::RotRateSource::STR;
|
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR;
|
||||||
fusedRotRateData->rotRateSource.setValid(true);
|
fusedRotRateData->rotRateSource.setValid(true);
|
||||||
}
|
}
|
||||||
} else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and
|
} else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and
|
||||||
@ -40,7 +40,7 @@ void FusedRotationEstimation::estimateFusedRotationRate(
|
|||||||
std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
|
std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
|
||||||
fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double));
|
fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double));
|
||||||
fusedRotRateData->rotRateOrthogonal.setValid(true);
|
fusedRotRateData->rotRateOrthogonal.setValid(true);
|
||||||
fusedRotRateData->rotRateSource.value = acs::RotRateSource::QUEST;
|
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::QUEST;
|
||||||
fusedRotRateData->rotRateSource.setValid(true);
|
fusedRotRateData->rotRateSource.setValid(true);
|
||||||
}
|
}
|
||||||
} else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) {
|
} else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) {
|
||||||
@ -55,7 +55,7 @@ void FusedRotationEstimation::estimateFusedRotationRate(
|
|||||||
std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
|
std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
|
||||||
fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double));
|
fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double));
|
||||||
fusedRotRateData->rotRateOrthogonal.setValid(true);
|
fusedRotRateData->rotRateOrthogonal.setValid(true);
|
||||||
fusedRotRateData->rotRateSource.value = acs::RotRateSource::SUSMGM;
|
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::SUSMGM;
|
||||||
fusedRotRateData->rotRateSource.setValid(true);
|
fusedRotRateData->rotRateSource.setValid(true);
|
||||||
} else {
|
} else {
|
||||||
PoolReadGuard pg(fusedRotRateData);
|
PoolReadGuard pg(fusedRotRateData);
|
||||||
@ -63,7 +63,7 @@ void FusedRotationEstimation::estimateFusedRotationRate(
|
|||||||
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
|
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
|
||||||
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
|
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
|
||||||
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
|
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
|
||||||
fusedRotRateData->rotRateSource.value = acs::RotRateSource::ALL_INVALID;
|
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE;
|
||||||
fusedRotRateData->setValidity(false, true);
|
fusedRotRateData->setValidity(false, true);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -16,7 +16,8 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
|
|||||||
acsctrl::GyrDataProcessed *gyrDataProcessed,
|
acsctrl::GyrDataProcessed *gyrDataProcessed,
|
||||||
acsctrl::MgmDataProcessed *mgmDataProcessed,
|
acsctrl::MgmDataProcessed *mgmDataProcessed,
|
||||||
acsctrl::SusDataProcessed *susDataProcessed,
|
acsctrl::SusDataProcessed *susDataProcessed,
|
||||||
acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters) {
|
acsctrl::AttitudeEstimationData *mekfData,
|
||||||
|
AcsParameters *acsParameters) {
|
||||||
double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
|
double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
|
||||||
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
|
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
|
||||||
bool quatIBValid = sensorValues->strSet.isTrustWorthy.value;
|
bool quatIBValid = sensorValues->strSet.isTrustWorthy.value;
|
||||||
@ -54,7 +55,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
|
|||||||
{
|
{
|
||||||
PoolReadGuard pg(gpsDataProcessed);
|
PoolReadGuard pg(gpsDataProcessed);
|
||||||
if (pg.getReadResult() == returnvalue::OK) {
|
if (pg.getReadResult() == returnvalue::OK) {
|
||||||
gpsDataProcessed->source = acs::GpsSource::SPG4;
|
gpsDataProcessed->source = acs::gps::Source::SPG4;
|
||||||
gpsDataProcessed->source.setValid(true);
|
gpsDataProcessed->source.setValid(true);
|
||||||
std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
|
std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
|
||||||
gpsDataProcessed->gpsPosition.setValid(true);
|
gpsDataProcessed->gpsPosition.setValid(true);
|
||||||
@ -66,7 +67,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
|
|||||||
{
|
{
|
||||||
PoolReadGuard pg(gpsDataProcessed);
|
PoolReadGuard pg(gpsDataProcessed);
|
||||||
if (pg.getReadResult() == returnvalue::OK) {
|
if (pg.getReadResult() == returnvalue::OK) {
|
||||||
gpsDataProcessed->source = acs::GpsSource::NONE;
|
gpsDataProcessed->source = acs::gps::Source::NONE;
|
||||||
gpsDataProcessed->source.setValid(true);
|
gpsDataProcessed->source.setValid(true);
|
||||||
std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
|
std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
|
||||||
gpsDataProcessed->gpsPosition.setValid(false);
|
gpsDataProcessed->gpsPosition.setValid(false);
|
||||||
|
@ -15,7 +15,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
|||||||
// ------------------------------------------------
|
// ------------------------------------------------
|
||||||
double magIgrfModel[3] = {0.0, 0.0, 0.0};
|
double magIgrfModel[3] = {0.0, 0.0, 0.0};
|
||||||
bool gpsValid = false;
|
bool gpsValid = false;
|
||||||
if (gpsDataProcessed->source.value != acs::GpsSource::NONE) {
|
if (gpsDataProcessed->source.value != acs::gps::Source::NONE) {
|
||||||
Igrf13Model igrf13;
|
Igrf13Model igrf13;
|
||||||
igrf13.schmidtNormalization();
|
igrf13.schmidtNormalization();
|
||||||
igrf13.updateCoeffGH(timeAbsolute);
|
igrf13.updateCoeffGH(timeAbsolute);
|
||||||
@ -526,9 +526,9 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
|
|||||||
// init variables
|
// init variables
|
||||||
double gdLongitude = 0, gdLatitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0},
|
double gdLongitude = 0, gdLatitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0},
|
||||||
gpsVelocityE[3] = {0, 0, 0};
|
gpsVelocityE[3] = {0, 0, 0};
|
||||||
uint8_t gpsSource = acs::GpsSource::NONE;
|
uint8_t gpsSource = acs::gps::Source::NONE;
|
||||||
// We do not trust the GPS and therefore it shall die here if SPG4 is running
|
// We do not trust the GPS and therefore it shall die here if SPG4 is running
|
||||||
if (gpsDataProcessed->source.value == acs::GpsSource::SPG4 and gpsParameters->useSpg4) {
|
if (gpsDataProcessed->source.value == acs::gps::Source::SPG4 and gpsParameters->useSpg4) {
|
||||||
MathOperations<double>::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude,
|
MathOperations<double>::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude,
|
||||||
gdLongitude, altitude);
|
gdLongitude, altitude);
|
||||||
double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
|
double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
|
||||||
@ -571,7 +571,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
|
|||||||
|
|
||||||
validSavedPosSatE = true;
|
validSavedPosSatE = true;
|
||||||
|
|
||||||
gpsSource = acs::GpsSource::GPS;
|
gpsSource = acs::gps::Source::GPS;
|
||||||
}
|
}
|
||||||
{
|
{
|
||||||
PoolReadGuard pg(gpsDataProcessed);
|
PoolReadGuard pg(gpsDataProcessed);
|
||||||
|
@ -17,9 +17,9 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
|
|||||||
return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
|
return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
|
||||||
} else if (mekfEnabled and mekfValid) {
|
} else if (mekfEnabled and mekfValid) {
|
||||||
return acs::ControlModeStrategy::PTGCTRL_MEKF;
|
return acs::ControlModeStrategy::PTGCTRL_MEKF;
|
||||||
} else if (strValid and fusedRateValid and rotRateSource > acs::RotRateSource::SUSMGM) {
|
} else if (strValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
|
||||||
return acs::ControlModeStrategy::PTGCTRL_STR;
|
return acs::ControlModeStrategy::PTGCTRL_STR;
|
||||||
} else if (questValid and fusedRateValid and rotRateSource > acs::RotRateSource::SUSMGM) {
|
} else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
|
||||||
return acs::ControlModeStrategy::PTGCTRL_QUEST;
|
return acs::ControlModeStrategy::PTGCTRL_QUEST;
|
||||||
} else {
|
} else {
|
||||||
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
|
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
|
||||||
|
Loading…
Reference in New Issue
Block a user