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

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

@ -34,15 +34,21 @@ class Guidance {
// pointing
void targetQuatPtgInertial(double targetQuat[4], double refSatRate[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: compares target Quaternion and reference quaternion, also actual and desired satellite
// rate
void comparePtg(double targetQuat[4], double quatRef[4], double refSatRate[3],
double quatErrorComplete[4], double quatError[3], double deltaRate[3]);
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);
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
// reation wheel maybe can be done in "commanding.h"
ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);

4
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -27,7 +27,7 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
}
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *qError, const double *deltaRate, const double *rwPseudoInv,
const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
double *torqueRws) {
//------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix
@ -37,6 +37,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
double qErrorMin = pointingLawParameters->qiMin;
double omMax = pointingLawParameters->omMax;
double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]};
double cInt = 2 * om * zeta;
double kInt = 2 * pow(om, 2);