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

fixed GPS and STR inputs
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2022-10-12 15:06:24 +02:00
parent 2a9dc518a0
commit 84fc44fd5f
9 changed files with 434 additions and 438 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
mission/controller/AcsController.cpp
View File

@ -126,17 +126,19 @@ void AcsController::performPointingCtrl() {
double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
ptgCtrl.ptgGroundstation(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
double rwTrqNs[4] = {0, 0, 0, 0};
ptgCtrl.ptgNullspace(&(sensorValues.speedRw0), &(sensorValues.speedRw1), &(sensorValues.speedRw2),
&(sensorValues.speedRw3), rwTrqNs);
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.speedRw0), &(sensorValues.speedRw1),
&(sensorValues.speedRw2), &(sensorValues.speedRw3), torquePtgRws,
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.speedRw0),
&(sensorValues.speedRw1), &(sensorValues.speedRw2),
&(sensorValues.speedRw3), mgtDpDes);
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);
}

11
mission/controller/acs/ActuatorCmd.cpp
View File

@ -23,9 +23,9 @@ ActuatorCmd::~ActuatorCmd(){
}
void ActuatorCmd::cmdSpeedToRws(const double *speedRw0, const double *speedRw1, const double *speedRw2,
const double *speedRw3, const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed){
void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3,
const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed) {
using namespace Math;
// Scaling the commanded torque to a maximum value
double torque[4] = {0, 0, 0, 0};
@ -40,10 +40,8 @@ void ActuatorCmd::cmdSpeedToRws(const double *speedRw0, const double *speedRw1,
}
if (maxValue > maxTrq) {
double scalingFactor = maxTrq / maxValue;
VectorOperations<double>::mulScalar(torque, scalingFactor, torque, 4);
}
// Calculating the commanded speed in RPM for every reaction wheel
@ -56,9 +54,6 @@ void ActuatorCmd::cmdSpeedToRws(const double *speedRw0, const double *speedRw1,
double factor = commandTime / inertiaWheel * radToRpm;
VectorOperations<double>::mulScalar(torque, factor, deltaSpeed, 4);
VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 4);
}
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) {

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

@ -28,8 +28,9 @@ public:
* rwTrqNS Nullspace torque
* rwCmdSpeed output revolutions per minute for every reaction wheel
*/
void cmdSpeedToRws(const double *speedRw0, const double *speedRw1, const double *speedRw2, const double *speedRw3,
const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed);
void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3, const double *rwTrqIn,
const double *rwTrqNS, double *rwCmdSpeed);
/*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques

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

@ -5,39 +5,28 @@
* Author: Robin Marquardt
*/
#include "Guidance.h"
#include "string.h"
#include "util/MathOperations.h"
#include "util/CholeskyDecomposition.h"
#include <math.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>
Guidance::Guidance(AcsParameters *acsParameters_) {
acsParameters = *acsParameters_;
#include "string.h"
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
}
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; }
Guidance::~Guidance() {
}
Guidance::~Guidance() {}
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
for (int i = 0; i < 3; i++) {
sunTargetSafe[i] =
acsParameters.safeModeControllerParameters.sunTargetDir[i];
satRateSafe[i] =
acsParameters.safeModeControllerParameters.satRateRef[i];
sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i];
satRateSafe[i] = acsParameters.safeModeControllerParameters.satRateRef[i];
}
// memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24);
}
void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
@ -45,17 +34,20 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
//-------------------------------------------------------------------------------------
// Calculation of target quaternion to groundstation
//-------------------------------------------------------------------------------------
// Transform longitude, latitude and altitude of groundstation to cartesian coordiantes (earth fixed/centered frame)
// Transform longitude, latitude and altitude of groundstation to cartesian coordiantes (earth
// fixed/centered frame)
double groundStationCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs,
acsParameters.groundStationParameters.longitudeGs, acsParameters.groundStationParameters.altitudeGs,
acsParameters.groundStationParameters.longitudeGs,
acsParameters.groundStationParameters.altitudeGs,
groundStationCart);
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(sensorValues->gps0latitude, sensorValues->gps0longitude,
sensorValues->gps0altitude, posSatE);
MathOperations<double>::cartesianFromLatLongAlt(sensorValues->gpsSet.latitude.value,
sensorValues->gpsSet.longitude.value,
sensorValues->gpsSet.altitude.value, posSatE);
// Target direction in the ECEF frame
double targetDirE[3] = {0, 0, 0};
@ -74,7 +66,8 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
// 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(&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);
@ -118,11 +111,10 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
// Calculation of reference rotation rate
//-------------------------------------------------------------------------------------
double velSatE[3] = {0, 0, 0};
velSatE[0] = sensorValues->gps0Velocity[0];
velSatE[1] = sensorValues->gps0Velocity[1];
velSatE[2] = sensorValues->gps0Velocity[2];
double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0},
velSatBPart2[3] = {0, 0, 0};
velSatE[0] = 0.0; // sensorValues->gps0Velocity[0];
velSatE[1] = 0.0; // sensorValues->gps0Velocity[1];
velSatE[2] = 0.0; // sensorValues->gps0Velocity[2];
double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0};
// Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -142,12 +134,10 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
// Calculation of reference rotation rate in case of star tracker blinding
//-------------------------------------------------------------------------------------
if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
double sunDirJ[3] = {0, 0, 0};
double sunDirB[3] = {0, 0, 0};
if (outputValues->sunDirModelValid) {
sunDirJ[0] = outputValues->sunDirModel[0];
sunDirJ[1] = outputValues->sunDirModel[1];
sunDirJ[2] = outputValues->sunDirModel[2];
@ -158,17 +148,16 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
sunDirB[0] = outputValues->sunDirEst[0];
sunDirB[1] = outputValues->sunDirEst[1];
sunDirB[2] = outputValues->sunDirEst[2];
}
double exclAngle = acsParameters.strParameters.exclusionAngle,
blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop;
double sightAngleSun = VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB);
double sightAngleSun =
VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB);
if (!(strBlindAvoidFlag)) {
double critSightAngle = blindStart * exclAngle;
if (sightAngleSun < critSightAngle) {
@ -179,17 +168,14 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
else {
if (sightAngleSun < blindEnd * exclAngle) {
double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate;
double blindRefRate[3] = {0, 0, 0};
if (sunDirB[1] < 0) {
blindRefRate[0] = normBlindRefRate;
blindRefRate[1] = 0;
blindRefRate[2] = 0;
}
else {
} else {
blindRefRate[0] = -normBlindRefRate;
blindRefRate[1] = 0;
blindRefRate[2] = 0;
@ -197,17 +183,15 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
VectorOperations<double>::add(blindRefRate, refSatRate, refSatRate, 3);
}
else {
} else {
strBlindAvoidFlag = false;
}
}
}
}
void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] ) {
void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
double refSatRate[3], double quatError[3], double deltaRate[3]) {
double quatRef[4] = {0, 0, 0, 0};
quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0];
quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1];
@ -238,14 +222,11 @@ void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
// target flag in matlab, importance, does look like it only gives
// feedback if pointing control is under 150 arcsec ??
}
void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) {
if (sensorValues->validRw0 && sensorValues->validRw1 && sensorValues->validRw2 && sensorValues->validRw3) {
if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() && sensorValues->rw3Set.isValid() &&
sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
@ -261,8 +242,8 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *
}
else if (!(sensorValues->validRw0) && sensorValues->validRw1 && sensorValues->validRw2 && sensorValues->validRw3) {
else if (!(sensorValues->rw1Set.isValid()) && sensorValues->rw2Set.isValid() &&
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.without0[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without0[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without0[0][2];
@ -277,8 +258,8 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *
rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2];
}
else if ((sensorValues->validRw0) && !(sensorValues->validRw1) && sensorValues->validRw2 && sensorValues->validRw3) {
else if ((sensorValues->rw1Set.isValid()) && !(sensorValues->rw2Set.isValid()) &&
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without1[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without1[0][2];
@ -293,8 +274,8 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *
rwPseudoInv[11] = acsParameters.rwMatrices.without1[3][2];
}
else if ((sensorValues->validRw0) && (sensorValues->validRw1) && !(sensorValues->validRw2) && sensorValues->validRw3) {
else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) &&
!(sensorValues->rw3Set.isValid()) && sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.without2[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without2[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without2[0][2];
@ -309,8 +290,8 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *
rwPseudoInv[11] = acsParameters.rwMatrices.without2[3][2];
}
else if ((sensorValues->validRw0) && (sensorValues->validRw1) && (sensorValues->validRw2) && !(sensorValues->validRw3)) {
else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) &&
(sensorValues->rw3Set.isValid()) && !(sensorValues->rw4Set.isValid())) {
rwPseudoInv[0] = acsParameters.rwMatrices.without3[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without3[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without3[0][2];
@ -342,6 +323,5 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *
rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2];
}
}

4
mission/controller/acs/SensorProcessing.cpp
View File

@ -460,8 +460,8 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
sensorValues->mgm3Rm3100Set.fieldStrengths.value,
sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), sensorValues->imtqMgmSet.mtmRawNt.value,
sensorValues->imtqMgmSet.mtmRawNt.isValid(), now, &acsParameters->mgmHandlingParameters,
outputValues->gcLatitude, outputValues->gdLongitude, sensorValues->gps0altitude,
sensorValues->gps0Valid, outputValues->magFieldEst, &outputValues->magFieldEstValid,
outputValues->gcLatitude, outputValues->gdLongitude, sensorValues->gpsSet.altitude.value,
sensorValues->gpsSet.isValid(), outputValues->magFieldEst, &outputValues->magFieldEstValid,
outputValues->magFieldModel, &outputValues->magFieldModelValid,
outputValues->magneticFieldVectorDerivative,
&outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ?

17
mission/controller/acs/SensorValues.cpp
View File

@ -60,6 +60,23 @@ ReturnValue_t SensorValues::updateStr() {
return result;
}
ReturnValue_t SensorValues::updateGps() {
ReturnValue_t result;
PoolReadGuard pgGps(&gpsSet);
result = pgGps.getReadResult();
return result;
}
ReturnValue_t SensorValues::updateRw() {
ReturnValue_t result;
PoolReadGuard pgRw1(&rw1Set), pgRw2(&rw2Set), pgRw3(&rw3Set), pgRw4(&rw4Set);
result = (pgRw1.getReadResult() || pgRw2.getReadResult() || pgRw3.getReadResult() ||
pgRw4.getReadResult());
return result;
}
ReturnValue_t SensorValues::update() {
ReturnValue_t mgmUpdate = updateMgm();
ReturnValue_t susUpdate = updateSus();

44
mission/controller/acs/SensorValues.h
View File

@ -6,9 +6,11 @@
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
#include "mission/devices/devicedefinitions/GPSDefinitions.h"
#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
#include "mission/devices/devicedefinitions/GyroL3GD20Definitions.h"
#include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h"
#include "mission/devices/devicedefinitions/RwDefinitions.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h"
namespace ACS {
@ -22,7 +24,9 @@ class SensorValues {
ReturnValue_t updateMgm();
ReturnValue_t updateSus();
ReturnValue_t updateGyr();
ReturnValue_t updateGps();
ReturnValue_t updateStr();
ReturnValue_t updateRw();
MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set =
MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER);
@ -56,33 +60,27 @@ class SensorValues {
startracker::SolutionSet strSet = startracker::SolutionSet(objects::STAR_TRACKER);
// double quatJB[4]; // output star tracker. quaternion or dcm ? refrence to which KOS?
// bool quatJBValid;
// int strIntTime[2];
GpsPrimaryDataset gpsSet = GpsPrimaryDataset(objects::GPS_CONTROLLER);
double gps0latitude; // Reference is WGS84, so this one will probably be geodetic
double gps0longitude; // Should be geocentric for IGRF
double gps0altitude;
double gps0Velocity[3]; // speed over ground = ??
double gps0Time; // utc
// double gps0latitude; // Reference is WGS84, so this one will probably be geodetic
// double gps0longitude; // Should be geocentric for IGRF
// double gps0altitude;
// double gps0Velocity[3]; // speed over ground = ??
// double gps0Time; // utc
//
// // valid ids for gps values !
// int gps0TimeYear;
// int gps0TimeMonth;
// int gps0TimeHour; // should be double
// bool gps0Valid;
// valid ids for gps values !
int gps0TimeYear;
int gps0TimeMonth;
int gps0TimeHour; // should be double
bool gps0Valid;
// bool mgt0valid;
bool mgt0valid;
// Reaction wheel measurements
double speedRw0; // RPM [1/min]
double speedRw1; // RPM [1/min]
double speedRw2; // RPM [1/min]
double speedRw3; // RPM [1/min]
bool validRw0;
bool validRw1;
bool validRw2;
bool validRw3;
RwDefinitions::StatusSet rw1Set = RwDefinitions::StatusSet(objects::RW1);
RwDefinitions::StatusSet rw2Set = RwDefinitions::StatusSet(objects::RW2);
RwDefinitions::StatusSet rw3Set = RwDefinitions::StatusSet(objects::RW3);
RwDefinitions::StatusSet rw4Set = RwDefinitions::StatusSet(objects::RW4);
};
} /* namespace ACS */

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

@ -112,39 +112,39 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
}
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, double *speedRw0,
double *speedRw1, double *speedRw2, double *speedRw3, double *mgtDpDes) {
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
int32_t *speedRw3, double *mgtDpDes) {
if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) {
mgtDpDes[0] = 0;
mgtDpDes[1] = 0;
mgtDpDes[2] = 0;
return;
}
// calculating momentum of satellite and momentum of reaction wheels
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4);
MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, 1);
MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4,
1);
double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1);
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
// calculating momentum error
double deltaMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract(momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3);
VectorOperations<double>::subtract(
momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3);
// resulting magnetic dipole command
double crossMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
factor = (pointingModeControllerParameters->deSatGainFactor) / normMag;
VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
}
void PtgCtrl::ptgNullspace(const double *speedRw0, const double *speedRw1, const double *speedRw2, const double *speedRw3, double *rwTrqNs) {
void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
double wheelMomentum[4] = {0, 0, 0, 0};
double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60;
@ -153,13 +153,13 @@ void PtgCtrl::ptgNullspace(const double *speedRw0, const double *speedRw1, const
VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
double diffRwSpeed[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, wheelMomentum, 4);
VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel,
wheelMomentum, 4);
double gainNs = pointingModeControllerParameters->gainNullspace;
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, *nullSpaceMatrix, 4);
MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace,
*nullSpaceMatrix, 4);
MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
}

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

@ -4,8 +4,8 @@
* Created on: 17 Jul 2022
* Author: Robin Marquardt
*
* @brief: This class handles the pointing control mechanism. Calculation of an commanded torque
* for the reaction wheels, and magnetic Field strength for magnetorques for desaturation
* @brief: This class handles the pointing control mechanism. Calculation of an commanded
* torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation
*
* @note: A description of the used algorithms can be found in
* https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110
@ -14,16 +14,16 @@
#ifndef PTGCTRL_H_
#define PTGCTRL_H_
#include "../SensorValues.h"
#include "../OutputValues.h"
#include "../AcsParameters.h"
#include "../config/classIds.h"
#include <string.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
class PtgCtrl{
#include "../AcsParameters.h"
#include "../OutputValues.h"
#include "../SensorValues.h"
#include "../config/classIds.h"
class PtgCtrl {
public:
/* @brief: Constructor
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
@ -42,12 +42,15 @@ public:
/* @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,const double *rwPseudoInv, double *torqueRws);
void ptgGroundstation(const double mode, const double *qError, const double *deltaRate,
const double *rwPseudoInv, double *torqueRws);
void ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, double *speedRw0,
double *speedRw1, double *speedRw2, double *speedRw3, double *mgtDpDes);
void ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3,
double *mgtDpDes);
void ptgNullspace(const double *speedRw0, const double *speedRw1, const double *speedRw2, const double *speedRw3, double *rwTrqNs);
void ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
const int32_t *speedRw3, double *rwTrqNs);
private:
AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters;