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

calc rot rate from STR
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-11-14 16:07:14 +01:00
parent c155f399b1
commit 74e7785c68
2 changed files with 39 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
mission/controller/acs/FusedRotationEstimation.cpp
View File

@ -7,13 +7,13 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_)
void FusedRotationEstimation::estimateFusedRotationRate( void FusedRotationEstimation::estimateFusedRotationRate(
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues,
acsctrl::FusedRotRateData *fusedRotRateData) { const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) {
if (sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and if (sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and
sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid()) { sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid()) {
double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, double quatNew[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};
if (VectorOperations<double>::norm(quatOld, 4) != 0) { if (VectorOperations<double>::norm(quatOld, 4) != 0 and timeDelta != 0) {
estimateFusedRotationRateStr(quatNew, fusedRotRateData); estimateFusedRotationRateStr(quatNew, timeDelta, fusedRotRateData);
} else { } else {
estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed, estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed,
fusedRotRateData); fusedRotRateData);
@ -27,7 +27,39 @@ void FusedRotationEstimation::estimateFusedRotationRate(
} }
void FusedRotationEstimation::estimateFusedRotationRateStr( void FusedRotationEstimation::estimateFusedRotationRateStr(
double *quatNew, acsctrl::FusedRotRateData *fusedRotRateData) {} double *quatNew, const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) {
double quatOldInv[4] = {0, 0, 0, 0};
double quatDelta[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatOld, quatOldInv);
QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta);
QuaternionOperations::normalize(quatDelta);
double rotVec[3] = {0, 0, 0};
double angle = QuaternionOperations::getAngle(quatDelta);
if (angle == 0.0) {
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(false);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);
}
}
VectorOperations<double>::normalize(quatDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(false);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateTotal.value, rotVec, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);
}
}
void FusedRotationEstimation::estimateFusedRotationRateSafe( void FusedRotationEstimation::estimateFusedRotationRateSafe(
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,

5
mission/controller/acs/FusedRotationEstimation.h
View File

@ -15,7 +15,7 @@ class FusedRotationEstimation {
void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed, void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
ACS::SensorValues *sensorValues, ACS::SensorValues *sensorValues, const double timeDelta,
acsctrl::FusedRotRateData *fusedRotRateData); acsctrl::FusedRotRateData *fusedRotRateData);
void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed, void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed,
@ -23,7 +23,8 @@ class FusedRotationEstimation {
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateData *fusedRotRateData); acsctrl::FusedRotRateData *fusedRotRateData);
void estimateFusedRotationRateStr(double *quatNew, acsctrl::FusedRotRateData *fusedRotRateData); void estimateFusedRotationRateStr(double *quatNew, const double timeDelta,
acsctrl::FusedRotRateData *fusedRotRateData);
protected: protected:
private: private: