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

updated AcsParameters. change DCM_EJ calc to with precission and nutation
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Robin Marquardt 2022-11-04 17:21:17 +01:00
parent ba541300ca
commit 75ab11fc35
7 changed files with 66 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
mission/controller/AcsController.cpp
View File

@ -45,6 +45,9 @@ void AcsController::performControlOperation() {
case SUBMODE_PTG_GS:
performPointingCtrl();
break;
case SUBMODE_PTG_SUN:
performPointingCtrlSun();
break;
}
}
break;
@ -124,7 +127,41 @@ void AcsController::performPointingCtrl() {
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
ptgCtrl.ptgGroundstation(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
double rwTrqNs[4] = {0, 0, 0, 0};
ptgCtrl.ptgNullspace(
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input
actuatorCmd.cmdSpeedToRws(
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws,
rwTrqNs, cmdSpeedRws);
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
ptgCtrl.ptgDesaturation(
outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
}
void AcsController::performPointingCtrlSun() {
ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
timeval now; // Übergabe ?
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate);
double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate);
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
double rwTrqNs[4] = {0, 0, 0, 0};
ptgCtrl.ptgNullspace(
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),

2
mission/controller/AcsController.h
View File

@ -27,12 +27,14 @@ class AcsController : public ExtendedControllerBase {
static const Submode_t SUBMODE_DETUMBLE = 3;
static const Submode_t SUBMODE_PTG_GS = 4;
static const Submode_t SUBMODE_PTG_NADIR = 5;
static const Submode_t SUBMODE_PTG_SUN = 6;
protected:
void performSafe();
void performDetumble();
void performPointingCtrl();
void performPointingCtrlSun();
private:
AcsParameters acsParameters;

14
mission/controller/acs/AcsParameters.h
View File

@ -29,7 +29,11 @@ class AcsParameters /*: public HasParametersIF*/ {
double inertiaMatrix[3][3] = {{0.1539829, -0.0001821456, -0.0050135},
{-0.0001821456, 0.1701302, 0.0004748963},
{-0.0050135, 0.0004748963, 0.08374296}}; //19.11.2021
double inertiaMatrixNoPanel[3][3] = {{0.122485, -0.0001798426, -0.005008},
// Possible inertia matrices
double inertiaMatrixDeployed[3][3] = {{0.1539829, -0.0001821456, -0.0050135},
{-0.0001821456, 0.1701302, 0.0004748963},
{-0.0050135, 0.0004748963, 0.08374296}}; //19.11.2021
double inertiaMatrixUndeployed[3][3] = {{0.122485, -0.0001798426, -0.005008},
{-0.0001798426, 0.162240, 0.000475596},
{-0.005008, 0.000475596, 0.060136}}; //19.11.2021
double inertiaMatrixPanel1[3][3] = {{0.13823347, -0.0001836122, -0.00501207},
@ -37,7 +41,7 @@ class AcsParameters /*: public HasParametersIF*/ {
{-0.00501207, 0.0083537, 0.07192588}}; //19.11.2021
double inertiaMatrixPanel3[3][3] = {{0.13823487, -0.000178376, -0.005009767},
{-0.000178376, 0.166172, -0.007403},
{-0.005009767, -0.007403, 0.07195314}}; //19.11.2021
{-0.005009767, -0.007403, 0.07195314}};
} inertiaEIVE;
struct MgmHandlingParameters {
@ -821,7 +825,7 @@ class AcsParameters /*: public HasParametersIF*/ {
// ToDo: mutiple structs for different pointing mode controllers?
struct PointingModeControllerParameters {
double refDirection[3] = {1, 0, 0}; //Antenna
double refDirection[3] = {-1, 0, 0}; //Antenna
double refRotRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 1};
bool avoidBlindStr = true;
@ -891,9 +895,9 @@ class AcsParameters /*: public HasParametersIF*/ {
} magnetorquesParameter;
struct DetumbleParameter {
uint8_t detumblecounter = 75;
uint8_t detumblecounter = 75; // 30 s
double omegaDetumbleStart = 2 * M_PI / 180;
double omegaDetumbleEnd = 0.2 * M_PI / 180;
double omegaDetumbleEnd = 0.4 * M_PI / 180;
double gainD = pow(10.0, -3.3);
} detumbleParameter;
};

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

@ -57,21 +57,11 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::dcmEJ(now, *dcmEJ);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
double omegaEarth = acsParameters.targetModeControllerParameters.omegaEarth;
// TEST SECTION !
// double dcmTEST[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
// MatrixOperations<double>::multiply(&acsParameters.magnetorquesParameter.mtq0orientationMatrix,
// dcmTEST, dcmTEST, 3, 3, 3);
MatrixOperations<double>::multiply(*dcmDot, *dcmEJ, *dcmEJDot, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3);
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Transformation between ECEF and Body frame

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

@ -31,7 +31,7 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_){
rwMatrices =&(acsParameters_->rwMatrices);
}
void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){
void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){
//------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix

2
mission/controller/acs/control/PtgCtrl.h
View File

@ -42,7 +42,7 @@ class PtgCtrl {
/* @brief: Calculates the needed torque for the pointing control mechanism
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
void ptgGroundstation(const double mode, const double *qError, const double *deltaRate,
void ptgLaw(const double mode, const double *qError, const double *deltaRate,
const double *rwPseudoInv, double *torqueRws);
void ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate,

13
mission/controller/acs/util/MathOperations.h
View File

@ -124,10 +124,11 @@ public:
}
static void dcmEJ(timeval time, T1 * outputDcmEJ){
static void dcmEJ(timeval time, T1 * outputDcmEJ, T1 * outputDotDcmEJ){
/* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
* @param: time Current time
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
* outputDotDcmEJ Derivative of transformation matrix [3][3]
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
* Landis Markley and John L. Crassidis*/
double JD2000Floor = 0;
@ -162,6 +163,16 @@ public:
outputDcmEJ[7] = 0;
outputDcmEJ[8] = 1;
// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]},
{outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]},
{outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}};
double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
double omegaEarth = 0.000072921158553;
double dotDcmEJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
MatrixOperations<double>::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3);
}
/* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame