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

added inertial pointing
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-14 17:16:47 +01:00
parent 20936faec6
commit 55dec574c5
7 changed files with 38 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
mission/controller/AcsController.cpp
View File

@ -43,7 +43,7 @@ void AcsController::performControlOperation() {
performDetumble(); performDetumble();
break; break;
case SUBMODE_PTG_GS: case SUBMODE_PTG_TARGET:
performPointingCtrl(); performPointingCtrl();
break; break;
@ -113,7 +113,8 @@ void AcsController::performSafe() {
sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid); sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid);
} }
double dipolCmdUnits[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
} }
void AcsController::performDetumble() { void AcsController::performDetumble() {
@ -154,7 +155,8 @@ void AcsController::performPointingCtrl() {
ACS::SensorValues sensorValues; ACS::SensorValues sensorValues;
ACS::OutputValues outputValues; ACS::OutputValues outputValues;
timeval now; // Übergabe ? timeval now;
Clock::getClock_timeval(&now);
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
ReturnValue_t validMekf; ReturnValue_t validMekf;
@ -162,14 +164,17 @@ void AcsController::performPointingCtrl() {
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
switch (submode) { switch (submode) {
case SUBMODE_PTG_GS: case SUBMODE_PTG_TARGET:
guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
break; break;
case SUBMODE_PTG_SUN: case SUBMODE_PTG_SUN:
guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate); guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate);
break; break;
case SUBMODE_PTG_NADIR: case SUBMODE_PTG_NADIR:
guidance.targetQuatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); guidance.quatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
break;
case SUBMODE_PTG_INERTIAL:
guidance.inertialQuatPtg(targetQuat, refSatRate);
break; break;
} }
double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
@ -188,8 +193,8 @@ void AcsController::performPointingCtrl() {
if (acsParameters.pointingModeControllerParameters.enableAntiStiction) { if (acsParameters.pointingModeControllerParameters.enableAntiStiction) {
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
double rwSpeed[4] = {&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), int32_t rwSpeed[4] = {(sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value)}; (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)};
ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
} }

3
mission/controller/AcsController.h
View File

@ -26,9 +26,10 @@ class AcsController : public ExtendedControllerBase {
static const Submode_t SUBMODE_SAFE = 2; static const Submode_t SUBMODE_SAFE = 2;
static const Submode_t SUBMODE_DETUMBLE = 3; static const Submode_t SUBMODE_DETUMBLE = 3;
static const Submode_t SUBMODE_PTG_GS = 4; static const Submode_t SUBMODE_PTG_TARGET = 4;
static const Submode_t SUBMODE_PTG_NADIR = 5; static const Submode_t SUBMODE_PTG_NADIR = 5;
static const Submode_t SUBMODE_PTG_SUN = 6; static const Submode_t SUBMODE_PTG_SUN = 6;
static const Submode_t SUBMODE_PTG_INERTIAL = 7;
protected: protected:

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

@ -45,10 +45,10 @@ class AcsParameters /*: public HasParametersIF*/ {
} inertiaEIVE; } inertiaEIVE;
struct MgmHandlingParameters { struct MgmHandlingParameters {
float mgm0orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; float mgm0orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; // Documentation not consistent
float mgm1orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; float mgm1orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; // Documentation not consistent
float mgm2orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; float mgm2orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; // Documentation not consistent
float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; // Documentation not consistent
float mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; float mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
float mgm0hardIronOffset[3] = {19.89364, -29.94111, -31.07508}; float mgm0hardIronOffset[3] = {19.89364, -29.94111, -31.07508};
@ -850,6 +850,8 @@ class AcsParameters /*: public HasParametersIF*/ {
double omegaEarth = 0.000072921158553; double omegaEarth = 0.000072921158553;
double nadirRefDirection[3] = {-1, 0, 0}; //Camera double nadirRefDirection[3] = {-1, 0, 0}; //Camera
double refQuatInertial[4] = {0, 0, 0, 1};
double refRotRateInertial[3] = {0, 0, 0};
} pointingModeControllerParameters, inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; } pointingModeControllerParameters, inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters;
struct StrParameters { struct StrParameters {

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

@ -32,9 +32,9 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3
void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
timeval now, double targetQuat[4], double refSatRate[3]) { timeval now, double targetQuat[4], double refSatRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion to groundstation // Calculation of target quaternion to groundstation or given latitude, longitude and altitude
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Transform longitude, latitude and altitude of groundstation to cartesian coordiantes (earth // Transform longitude, latitude and altitude to cartesian coordiantes (earth
// fixed/centered frame) // fixed/centered frame)
double groundStationCart[3] = {0, 0, 0}; double groundStationCart[3] = {0, 0, 0};
@ -234,7 +234,7 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou
refSatRate[2] = 0; refSatRate[2] = 0;
} }
void Guidance::targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
double targetQuat[4], double refSatRate[3]) { double targetQuat[4], double refSatRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing // Calculation of target quaternion for Nadir pointing
@ -301,6 +301,15 @@ void Guidance::targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputVa
} }
void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
for (int i = 0; i < 4; i++) {
targetQuat[i] = acsParameters.inertialModeControllerParameters.refQuatInertial[i];
}
for (int i = 0; i < 3; i++) {
refSatRate[i] = acsParameters.inertialModeControllerParameters.refRotRateInertial[i];
}
}
void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues, void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
double refSatRate[3], double quatError[3], double deltaRate[3]) { double refSatRate[3], double quatError[3], double deltaRate[3]) {
double quatRef[4] = {0, 0, 0, 0}; double quatRef[4] = {0, 0, 0, 0};

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

@ -31,9 +31,12 @@ public:
double targetQuat[4], double refSatRate[3]); double targetQuat[4], double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate from gps position for Nadir pointing // Function to get the target quaternion and refence rotation rate from gps position for Nadir pointing
void targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, void quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
double targetQuat[4], double refSatRate[3]); double targetQuat[4], double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate from parameters for inertial pointing
void inertialQuatPtg(double targetQuat[4], double refSatRate[3]);
// @note: compares target Quaternion and reference quaternion, also actual satellite rate and desired // @note: compares target Quaternion and reference quaternion, also actual satellite rate and desired
void comparePtg( double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] ); void comparePtg( double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] );

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

@ -165,7 +165,7 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4); VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
} }
void PtgCtrl::rwAntistiction(const bool* rwAvailable, const double* omegaRw, void PtgCtrl::rwAntistiction(const bool* rwAvailable, const int32_t* omegaRw,
double* torqueCommand) { double* torqueCommand) {
for (uint8_t i = 0; i < 4; i++) { for (uint8_t i = 0; i < 4; i++) {
if (rwAvailable[i]) { if (rwAvailable[i]) {
@ -196,4 +196,3 @@ void PtgCtrl::rwAntistiction(const bool* rwAvailable, const double* omegaRw,
omegaMemory[i] = omegaRw[i]; omegaMemory[i] = omegaRw[i];
} }
} }
}

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

@ -57,7 +57,7 @@ class PtgCtrl {
* omegaRw current wheel speed of reaction wheels * omegaRw current wheel speed of reaction wheels
* torqueCommand modified torque after antistiction * torqueCommand modified torque after antistiction
*/ */
void rwAntistiction(const bool* rwAvailable, const double* omegaRw, void rwAntistiction(const bool* rwAvailable, const int32_t* omegaRw,
double* torqueCommand); double* torqueCommand);
private: private: