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

PTG_TARGET should work now
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-02-17 15:57:07 +01:00
parent 0d32bc0c0a
commit 0dae3b04be
4 changed files with 43 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
mission/controller/acs/Guidance.cpp
View File

@ -470,27 +470,23 @@ void Guidance::targetQuatPtgInertial(double targetQuat[4], double refSatRate[3])
3 * sizeof(double));
}
void Guidance::comparePtg(double targetQuat[4], double quatRef[4], double refSatRate[3],
double quatErrorComplete[4], double quatError[3], double deltaRate[3]) {
double satRate[3] = {0, 0, 0};
std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));
VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);
// valid checks ?
void Guidance::comparePtg(double oldErrorQuat[4], double quatRef[4], double newErrorQuatComplete[4],
double satRotRate[3], double satRotRateGuidance[3],
double satRotRateRef[3], double satRotRateError[3]) {
double combinedRefSatRate[3] = {0, 0, 0};
VectorOperations<double>::add(satRotRateGuidance, satRotRateRef, combinedRefSatRate, 3);
VectorOperations<double>::subtract(satRotRate, combinedRefSatRate, satRotRateError, 3);
double quatErrorMtx[4][4] = {{quatRef[3], quatRef[2], -quatRef[1], -quatRef[0]},
{-quatRef[2], quatRef[3], quatRef[0], -quatRef[1]},
{quatRef[1], -quatRef[0], quatRef[3], -quatRef[2]},
{quatRef[0], -quatRef[1], quatRef[2], quatRef[3]}};
MatrixOperations<double>::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1);
MatrixOperations<double>::multiply(*quatErrorMtx, oldErrorQuat, newErrorQuatComplete, 4, 4, 1);
if (quatErrorComplete[3] < 0) {
quatErrorComplete[3] *= -1;
if (newErrorQuatComplete[3] < 0) {
VectorOperations<double>::mulScalar(newErrorQuatComplete, -1, newErrorQuatComplete, 4);
}
quatError[0] = quatErrorComplete[0];
quatError[1] = quatErrorComplete[1];
quatError[2] = quatErrorComplete[2];
// target flag in matlab, importance, does look like it only gives feedback if pointing control is
// under 150 arcsec ??
}
@ -537,6 +533,12 @@ void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatIn
savedQuaternion[3] = quatInertialTarget[3];
}
void Guidance::calculateErrorQuat(double targetQuat[4], double currentQuat[4], double errorQuat[4],
double errorAngle) {
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
errorAngle = 2 * acos(errorQuat[3]);
}
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
double *rwPseudoInv) {
bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());