eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
mission/controller/AcsController.cpp
View File

@ -246,7 +246,7 @@ void AcsController::performPointingCtrl() {
// Variables required for guidance
double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0},
errorQuatInterim[4] = {0, 0, 0, 1}, errorQuat[4] = {0, 0, 0, 1}, errorAngle = 0,
satRotRate[3] = {0, 0, 0}, satRotRateError[3] = {0, 0, 0};
satRotRate[3] = {0, 0, 0}, errorSatRotRate[3] = {0, 0, 0};
switch (submode) {
case acs::PTG_IDLE:
guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
@ -279,17 +279,11 @@ void AcsController::performPointingCtrl() {
guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat,
targetSatRotRate);
guidance.calculateErrorQuat(targetQuat, mekfData.quatMekf.value, errorQuatInterim,
errorAngle);
if (mekfData.satRotRateMekf.isValid()) {
std::memcpy(satRotRate, mekfData.satRotRateMekf.value, 3 * sizeof(double));
} else {
std::memcpy(satRotRate, gyrDataProcessed.gyrVecTot.value, 3 * sizeof(double));
}
guidance.comparePtg(errorQuatInterim, acsParameters.targetModeControllerParameters.quatRef,
errorQuat, satRotRate, targetSatRotRate,
acsParameters.targetModeControllerParameters.refRotRate, satRotRateError);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, satRotRateError,
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef,
acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),

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());

35
mission/controller/acs/Guidance.h
View File

@ -16,37 +16,34 @@ class Guidance {
// Function to get the target quaternion and refence rotation rate from gps position and
// position of the ground station
void targetQuatPtgSingleAxis(timeval now, double targetQuat[4], double refSatRate[3]);
void targetQuatPtgSingleAxis(timeval now, double targetQuat[4], double targetSatRotRate[3]);
void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4],
double refSatRate[3]);
void targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]);
double targetSatRotRate[3]);
void targetQuatPtgGs(timeval now, double targetQuat[4], double targetSatRotRate[3]);
// Function to get the target quaternion and refence rotation rate for sun pointing after ground
// station
void targetQuatPtgSun(timeval now, double targetQuat[4], double refSatRate[3]);
void targetQuatPtgSun(timeval now, double targetQuat[4], double targetSatRotRate[3]);
// Function to get the target quaternion and refence rotation rate from gps position for Nadir
// pointing
void targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double refSatRate[3]);
void targetQuatPtgNadirThreeAxes(timeval now, double targetQuat[4], double refSatRate[3]);
void targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double targetSatRotRate[3]);
void targetQuatPtgNadirThreeAxes(timeval now, double targetQuat[4], double targetSatRotRate[3]);
// Function to get the target quaternion and refence rotation rate from parameters for inertial
// pointing
void targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]);
void targetQuatPtgInertial(double targetQuat[4], double targetSatRotRate[3]);
// @note: compares the target Quaternion in the ECI to the current orientation in ECI to compute
// the error quaternion and error angle
void calculateErrorQuat(double targetQuat[4], double currentQuat[4], double errorQuat[4],
double errorAngle);
// @note: Calculates the error quaternion between the current orientation and the target
// quaternion, considering a reference quaternion. Additionally the difference between the actual
// and a desired satellite rotational rate is calculated, again considering a reference rotational
// rate. Lastly gives back the error angle of the error quaternion.
void 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);
// @note: compares target Quaternion and reference quaternion, also actual and desired satellite
// rate
void comparePtg(double oldErrorQuat[4], double quatRef[4], double newErrorQuatComplete[4],
double satRotRate[3], double satRotRateGuidance[3], double satRotRateRef[3],
double satRotRateError[3]);
void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
double *refSatRate);
void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
double *targetSatRotRate);
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
// reation wheel maybe can be done in "commanding.h"