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

only one guidance function to calculate error quaternion

Browse Source
This commit is contained in:
Marius Eggert
2023-02-20 11:52:53 +01:00
parent 0dae3b04be
commit 700d7ced64
3 changed files with 53 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -148,7 +148,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta
}
void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3],
double quatIX[4], double refSatRate[3]) {
double quatIX[4], double targetSatRotRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for target pointing
//-------------------------------------------------------------------------------------
@ -210,7 +210,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
QuaternionOperations::fromDcm(dcmIX, quatIX);
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
refRotationRate(timeElapsedMax, now, quatIX, refSatRate);
targetRotationRate(timeElapsedMax, now, quatIX, targetSatRotRate);
}
void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]) {
@ -470,31 +470,41 @@ void Guidance::targetQuatPtgInertial(double targetQuat[4], double refSatRate[3])
3 * sizeof(double));
}
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, oldErrorQuat, newErrorQuatComplete, 4, 4, 1);
if (newErrorQuatComplete[3] < 0) {
VectorOperations<double>::mulScalar(newErrorQuatComplete, -1, newErrorQuatComplete, 4);
void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
double errorQuat[4], double errorSatRotRate[3], double errorAngle) {
// First calculate error quaternion between current and target orientation
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
// Last calculate add rotation from reference quaternion
QuaternionOperations::multiply(refQuat, errorQuat, errorQuat);
// Keep scalar part of quaternion positive
if (errorQuat[3] < 0) {
VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
}
// Calculate error angle
errorAngle = QuaternionOperations::getAngle(errorQuat, true);
// Only give back error satellite rotational rate if orientation has already been aquired
if (errorAngle < 2. / 180. * M_PI) {
// First combine the target and reference satellite rotational rates
double combinedRefSatRotRate[3] = {0, 0, 0};
VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
// Then substract the combined required satellite rotational rates from the actual rate
VectorOperations<double>::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate,
3);
} else {
// If orientation has not been aquired yet set satellite rotational rate to zero
errorSatRotRate = 0;
}
// target flag in matlab, importance, does look like it only gives feedback if pointing control is
// under 150 arcsec ??
}
void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
double *refSatRate) {
void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
double *refSatRate) {
//-------------------------------------------------------------------------------------
// Calculation of reference rotation rate
// Calculation of target rotation rate
//-------------------------------------------------------------------------------------
double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) -
(timeSavedQuaternion.tv_sec +
@ -533,12 +543,6 @@ 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());