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

20
mission/controller/AcsController.cpp
View File

@ -126,17 +126,19 @@ void AcsController::performPointingCtrl() {
double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0; double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
ptgCtrl.ptgGroundstation(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgGroundstation(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
double rwTrqNs[4] = {0, 0, 0, 0}; double rwTrqNs[4] = {0, 0, 0, 0};
ptgCtrl.ptgNullspace(&(sensorValues.speedRw0), &(sensorValues.speedRw1), &(sensorValues.speedRw2), ptgCtrl.ptgNullspace(
&(sensorValues.speedRw3), rwTrqNs); &(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 double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input
actuatorCmd.cmdSpeedToRws(&(sensorValues.speedRw0), &(sensorValues.speedRw1), actuatorCmd.cmdSpeedToRws(
&(sensorValues.speedRw2), &(sensorValues.speedRw3), torquePtgRws, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
rwTrqNs, cmdSpeedRws); &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws,
rwTrqNs, cmdSpeedRws);
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
ptgCtrl.ptgDesaturation(outputValues.magFieldEst, &outputValues.magFieldEstValid, ptgCtrl.ptgDesaturation(
outputValues.satRateMekf, &(sensorValues.speedRw0), outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf,
&(sensorValues.speedRw1), &(sensorValues.speedRw2), &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.speedRw3), mgtDpDes); &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
} }

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

@ -23,42 +23,37 @@ ActuatorCmd::~ActuatorCmd(){
} }
void ActuatorCmd::cmdSpeedToRws(const double *speedRw0, const double *speedRw1, const double *speedRw2, void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
const double *speedRw3, const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed){ const int32_t *speedRw2, const int32_t *speedRw3,
const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed) {
using namespace Math; using namespace Math;
// Scaling the commanded torque to a maximum value // Scaling the commanded torque to a maximum value
double torque[4] = {0, 0, 0, 0}; double torque[4] = {0, 0, 0, 0};
double maxTrq = acsParameters.rwHandlingParameters.maxTrq; double maxTrq = acsParameters.rwHandlingParameters.maxTrq;
VectorOperations<double>::add(rwTrqIn, rwTrqNS, torque, 4); VectorOperations<double>::add(rwTrqIn, rwTrqNS, torque, 4);
double maxValue = 0;
for (int i = 0; i < 4; i++) { //size of torque, always 4 ?
if (abs(torque[i]) > maxValue) {
maxValue = abs(torque[i]);
}
}
if (maxValue > maxTrq) {
double scalingFactor = maxTrq / maxValue;
VectorOperations<double>::mulScalar(torque, scalingFactor, torque, 4);
}
// Calculating the commanded speed in RPM for every reaction wheel
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
double deltaSpeed[4] = {0, 0, 0, 0};
double commandTime = acsParameters.onBoardParams.sampleTime,
inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel;
double radToRpm = 60 / (2 * PI); // factor for conversion to RPM
// W_RW = Torque_RW / I_RW * delta t [rad/s]
double factor = commandTime / inertiaWheel * radToRpm;
VectorOperations<double>::mulScalar(torque, factor, deltaSpeed, 4);
VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 4);
double maxValue = 0;
for (int i = 0; i < 4; i++) { // size of torque, always 4 ?
if (abs(torque[i]) > maxValue) {
maxValue = abs(torque[i]);
}
}
if (maxValue > maxTrq) {
double scalingFactor = maxTrq / maxValue;
VectorOperations<double>::mulScalar(torque, scalingFactor, torque, 4);
}
// Calculating the commanded speed in RPM for every reaction wheel
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
double deltaSpeed[4] = {0, 0, 0, 0};
double commandTime = acsParameters.onBoardParams.sampleTime,
inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel;
double radToRpm = 60 / (2 * PI); // factor for conversion to RPM
// W_RW = Torque_RW / I_RW * delta t [rad/s]
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) { void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) {

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

@ -28,10 +28,11 @@ public:
* rwTrqNS Nullspace torque * rwTrqNS Nullspace torque
* rwCmdSpeed output revolutions per minute for every reaction wheel * rwCmdSpeed output revolutions per minute for every reaction wheel
*/ */
void cmdSpeedToRws(const double *speedRw0, const double *speedRw1, const double *speedRw2, const double *speedRw3, void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed); 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 * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques
* *
* @param: dipolMoment given dipol moment in spacecraft frame * @param: dipolMoment given dipol moment in spacecraft frame

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

@ -5,343 +5,323 @@
* Author: Robin Marquardt * Author: Robin Marquardt
*/ */
#include "Guidance.h" #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/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h> #include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>
Guidance::Guidance(AcsParameters *acsParameters_) { #include "string.h"
acsParameters = *acsParameters_; #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]) { 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];
}
for (int i = 0; i < 3; i++) { // memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24);
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, 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
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// 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
double groundStationCart[3] = {0, 0, 0}; // fixed/centered frame)
double groundStationCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs, MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs,
acsParameters.groundStationParameters.longitudeGs, acsParameters.groundStationParameters.altitudeGs, acsParameters.groundStationParameters.longitudeGs,
groundStationCart); acsParameters.groundStationParameters.altitudeGs,
groundStationCart);
// Position of the satellite in the earth/fixed frame via GPS // Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0}; double posSatE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(sensorValues->gps0latitude, sensorValues->gps0longitude, MathOperations<double>::cartesianFromLatLongAlt(sensorValues->gpsSet.latitude.value,
sensorValues->gps0altitude, posSatE); sensorValues->gpsSet.longitude.value,
sensorValues->gpsSet.altitude.value, posSatE);
// Target direction in the ECEF frame // Target direction in the ECEF frame
double targetDirE[3] = {0, 0, 0}; double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3); VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
// Transformation between ECEF and IJK frame // Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; 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}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::dcmEJ(now, *dcmEJ); MathOperations<double>::dcmEJ(now, *dcmEJ);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE); MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION // Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; 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 dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
double omegaEarth = acsParameters.targetModeControllerParameters.omegaEarth; double omegaEarth = acsParameters.targetModeControllerParameters.omegaEarth;
// TEST SECTION ! // TEST SECTION !
//double dcmTEST[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // 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>::multiply(*dcmDot, *dcmEJ, *dcmEJDot, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3); MatrixOperations<double>::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3);
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Transformation between ECEF and Body frame // Transformation between ECEF and Body frame
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double quatBJ[4] = {0, 0, 0, 0}; double quatBJ[4] = {0, 0, 0, 0};
quatBJ[0] = outputValues->quatMekfBJ[0]; quatBJ[0] = outputValues->quatMekfBJ[0];
quatBJ[1] = outputValues->quatMekfBJ[1]; quatBJ[1] = outputValues->quatMekfBJ[1];
quatBJ[2] = outputValues->quatMekfBJ[2]; quatBJ[2] = outputValues->quatMekfBJ[2];
quatBJ[3] = outputValues->quatMekfBJ[3]; quatBJ[3] = outputValues->quatMekfBJ[3];
QuaternionOperations::toDcm(quatBJ, dcmBJ); QuaternionOperations::toDcm(quatBJ, dcmBJ);
MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
// Target Direction in the body frame // Target Direction in the body frame
double targetDirB[3] = {0, 0, 0}; double targetDirB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
// rotation quaternion from two vectors // rotation quaternion from two vectors
double refDir[3] = {0, 0, 0}; double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0]; refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0];
refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1]; refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1];
refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2]; refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2];
double noramlizedTargetDirB[3] = {0, 0, 0}; double noramlizedTargetDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3); VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
VectorOperations<double>::normalize(refDir, refDir, 3); VectorOperations<double>::normalize(refDir, refDir, 3);
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3); double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
double normRefDir = VectorOperations<double>::norm(refDir, 3); double normRefDir = VectorOperations<double>::norm(refDir, 3);
double crossDir[3] = {0, 0, 0}; double crossDir[3] = {0, 0, 0};
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir); double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir); VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
targetQuat[0] = crossDir[0]; targetQuat[0] = crossDir[0];
targetQuat[1] = crossDir[1]; targetQuat[1] = crossDir[1];
targetQuat[2] = crossDir[2]; targetQuat[2] = crossDir[2];
targetQuat[3] = sqrt(pow(normTargetDirB,2) * pow(normRefDir,2) + dotDirections); targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
VectorOperations<double>::normalize(targetQuat, targetQuat, 4); VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of reference rotation rate // Calculation of reference rotation rate
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
double velSatE[3] = {0, 0, 0}; double velSatE[3] = {0, 0, 0};
velSatE[0] = sensorValues->gps0Velocity[0]; velSatE[0] = 0.0; // sensorValues->gps0Velocity[0];
velSatE[1] = sensorValues->gps0Velocity[1]; velSatE[1] = 0.0; // sensorValues->gps0Velocity[1];
velSatE[2] = sensorValues->gps0Velocity[2]; velSatE[2] = 0.0; // sensorValues->gps0Velocity[2];
double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0};
velSatBPart2[3] = {0, 0, 0}; // Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
// Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1); double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MatrixOperations<double>::multiply(*dcmBJ, *dcmJEDot, *dcmBEDot, 3, 3, 3);
MatrixOperations<double>::multiply(*dcmBJ, *dcmJEDot, *dcmBEDot, 3, 3, 3); MatrixOperations<double>::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1); VectorOperations<double>::add(velSatBPart1, velSatBPart2, velSatB, 3);
VectorOperations<double>::add(velSatBPart1, velSatBPart2, velSatB, 3);
double normVelSatB = VectorOperations<double>::norm(velSatB, 3); double normVelSatB = VectorOperations<double>::norm(velSatB, 3);
double normRefSatRate = normVelSatB / normTargetDirB; double normRefSatRate = normVelSatB / normTargetDirB;
double satRateDir[3] = {0, 0, 0}; double satRateDir[3] = {0, 0, 0};
VectorOperations<double>::cross(velSatB, targetDirB, satRateDir); VectorOperations<double>::cross(velSatB, targetDirB, satRateDir);
VectorOperations<double>::normalize(satRateDir, satRateDir, 3); VectorOperations<double>::normalize(satRateDir, satRateDir, 3);
VectorOperations<double>::mulScalar(satRateDir, normRefSatRate, refSatRate, 3); VectorOperations<double>::mulScalar(satRateDir, normRefSatRate, refSatRate, 3);
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of reference rotation rate in case of star tracker blinding // Calculation of reference rotation rate in case of star tracker blinding
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
if ( acsParameters.targetModeControllerParameters.avoidBlindStr ) { if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
double sunDirJ[3] = {0, 0, 0};
double sunDirB[3] = {0, 0, 0};
double sunDirJ[3] = {0, 0, 0}; if (outputValues->sunDirModelValid) {
double sunDirB[3] = {0, 0, 0}; sunDirJ[0] = outputValues->sunDirModel[0];
sunDirJ[1] = outputValues->sunDirModel[1];
sunDirJ[2] = outputValues->sunDirModel[2];
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
}
if ( outputValues->sunDirModelValid ) { else {
sunDirB[0] = outputValues->sunDirEst[0];
sunDirB[1] = outputValues->sunDirEst[1];
sunDirB[2] = outputValues->sunDirEst[2];
}
sunDirJ[0] = outputValues->sunDirModel[0]; double exclAngle = acsParameters.strParameters.exclusionAngle,
sunDirJ[1] = outputValues->sunDirModel[1]; blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
sunDirJ[2] = outputValues->sunDirModel[2]; blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop;
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
}
else { double sightAngleSun =
sunDirB[0] = outputValues->sunDirEst[0]; VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB);
sunDirB[1] = outputValues->sunDirEst[1];
sunDirB[2] = outputValues->sunDirEst[2];
} if (!(strBlindAvoidFlag)) {
double critSightAngle = blindStart * exclAngle;
double exclAngle = acsParameters.strParameters.exclusionAngle, if (sightAngleSun < critSightAngle) {
blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, strBlindAvoidFlag = true;
blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop; }
double sightAngleSun = VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB); }
if ( !(strBlindAvoidFlag) ) { else {
if (sightAngleSun < blindEnd * exclAngle) {
double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate;
double blindRefRate[3] = {0, 0, 0};
double critSightAngle = blindStart * exclAngle; if (sunDirB[1] < 0) {
blindRefRate[0] = normBlindRefRate;
blindRefRate[1] = 0;
blindRefRate[2] = 0;
} else {
blindRefRate[0] = -normBlindRefRate;
blindRefRate[1] = 0;
blindRefRate[2] = 0;
}
if ( sightAngleSun < critSightAngle) { VectorOperations<double>::add(blindRefRate, refSatRate, refSatRate, 3);
strBlindAvoidFlag = true;
}
} } else {
strBlindAvoidFlag = false;
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 {
blindRefRate[0] = -normBlindRefRate;
blindRefRate[1] = 0;
blindRefRate[2] = 0;
}
VectorOperations<double>::add(blindRefRate, refSatRate, refSatRate, 3);
}
else {
strBlindAvoidFlag = false;
}
}
}
} }
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];
quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2];
quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3];
void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] ) { double satRate[3] = {0, 0, 0};
satRate[0] = outputValues->satRateMekf[0];
satRate[1] = outputValues->satRateMekf[1];
satRate[2] = outputValues->satRateMekf[2];
VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);
// valid checks ?
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]}};
double quatRef[4] = {0, 0, 0, 0}; double quatErrorComplete[4] = {0, 0, 0, 0};
quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0]; MatrixOperations<double>::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1);
quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1];
quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2];
quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3];
double satRate[3] = {0, 0, 0}; if (quatErrorComplete[3] < 0) {
satRate[0] = outputValues->satRateMekf[0]; quatErrorComplete[3] *= -1;
satRate[1] = outputValues->satRateMekf[1]; }
satRate[2] = outputValues->satRateMekf[2];
VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);
// valid checks ?
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] }};
double quatErrorComplete[4] = {0, 0, 0, 0}; quatError[0] = quatErrorComplete[0];
MatrixOperations<double>::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1); quatError[1] = quatErrorComplete[1];
quatError[2] = quatErrorComplete[2];
if (quatErrorComplete[3] < 0) {
quatErrorComplete[3] *= -1;
}
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 ??
// 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->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];
rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2];
void Guidance::getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *rwPseudoInv) { }
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];
rwPseudoInv[3] = acsParameters.rwMatrices.without0[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without0[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without0[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without0[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without0[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without0[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without0[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without0[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2];
}
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; else if ((sensorValues->rw1Set.isValid()) && !(sensorValues->rw2Set.isValid()) &&
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0];
rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0]; rwPseudoInv[1] = acsParameters.rwMatrices.without1[0][1];
rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1]; rwPseudoInv[2] = acsParameters.rwMatrices.without1[0][2];
rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2]; rwPseudoInv[3] = acsParameters.rwMatrices.without1[1][0];
rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0]; rwPseudoInv[4] = acsParameters.rwMatrices.without1[1][1];
rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1]; rwPseudoInv[5] = acsParameters.rwMatrices.without1[1][2];
rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2]; rwPseudoInv[6] = acsParameters.rwMatrices.without1[2][0];
rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0]; rwPseudoInv[7] = acsParameters.rwMatrices.without1[2][1];
rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1]; rwPseudoInv[8] = acsParameters.rwMatrices.without1[2][2];
rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2]; rwPseudoInv[9] = acsParameters.rwMatrices.without1[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without1[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without1[3][2];
}
} 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];
rwPseudoInv[3] = acsParameters.rwMatrices.without2[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without2[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without2[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without2[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without2[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without2[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without2[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without2[3][1];
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];
rwPseudoInv[3] = acsParameters.rwMatrices.without3[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without3[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without3[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without3[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without3[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without3[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without3[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without3[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without3[3][2];
}
rwPseudoInv[0] = acsParameters.rwMatrices.without0[0][0]; else {
rwPseudoInv[1] = acsParameters.rwMatrices.without0[0][1]; // @note: This one takes the normal pseudoInverse of all four raction wheels valid.
rwPseudoInv[2] = acsParameters.rwMatrices.without0[0][2]; // Does not make sense, but is implemented that way in MATLAB ?!
rwPseudoInv[3] = acsParameters.rwMatrices.without0[1][0]; // Thought: It does not really play a role, because in case there are more then one
rwPseudoInv[4] = acsParameters.rwMatrices.without0[1][1]; // reaction wheel the pointing control is destined to fail.
rwPseudoInv[5] = acsParameters.rwMatrices.without0[1][2]; rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
rwPseudoInv[6] = acsParameters.rwMatrices.without0[2][0]; rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
rwPseudoInv[7] = acsParameters.rwMatrices.without0[2][1]; rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
rwPseudoInv[8] = acsParameters.rwMatrices.without0[2][2]; rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0];
rwPseudoInv[9] = acsParameters.rwMatrices.without0[3][0]; rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1];
rwPseudoInv[10] = acsParameters.rwMatrices.without0[3][1]; rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2];
rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2]; rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0];
} rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2];
else if ((sensorValues->validRw0) && !(sensorValues->validRw1) && sensorValues->validRw2 && sensorValues->validRw3) { rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1];
rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0]; rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2];
rwPseudoInv[1] = acsParameters.rwMatrices.without1[0][1]; }
rwPseudoInv[2] = acsParameters.rwMatrices.without1[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.without1[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without1[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without1[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without1[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without1[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without1[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without1[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without1[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without1[3][2];
}
else if ((sensorValues->validRw0) && (sensorValues->validRw1) && !(sensorValues->validRw2) && sensorValues->validRw3) {
rwPseudoInv[0] = acsParameters.rwMatrices.without2[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without2[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without2[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.without2[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without2[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without2[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without2[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without2[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without2[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without2[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without2[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without2[3][2];
}
else if ((sensorValues->validRw0) && (sensorValues->validRw1) && (sensorValues->validRw2) && !(sensorValues->validRw3)) {
rwPseudoInv[0] = acsParameters.rwMatrices.without3[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without3[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without3[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.without3[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without3[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without3[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without3[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without3[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without3[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without3[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without3[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without3[3][2];
}
else {
// @note: This one takes the normal pseudoInverse of all four raction wheels valid.
// Does not make sense, but is implemented that way in MATLAB ?!
// Thought: It does not really play a role, because in case there are more then one
// reaction wheel the pointing control is destined to fail.
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2];
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.value,
sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), sensorValues->imtqMgmSet.mtmRawNt.value,
sensorValues->imtqMgmSet.mtmRawNt.isValid(), now, &acsParameters->mgmHandlingParameters, sensorValues->imtqMgmSet.mtmRawNt.isValid(), now, &acsParameters->mgmHandlingParameters,
outputValues->gcLatitude, outputValues->gdLongitude, sensorValues->gps0altitude, outputValues->gcLatitude, outputValues->gdLongitude, sensorValues->gpsSet.altitude.value,
sensorValues->gps0Valid, outputValues->magFieldEst, &outputValues->magFieldEstValid, sensorValues->gpsSet.isValid(), outputValues->magFieldEst, &outputValues->magFieldEstValid,
outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->magFieldModel, &outputValues->magFieldModelValid,
outputValues->magneticFieldVectorDerivative, outputValues->magneticFieldVectorDerivative,
&outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ? &outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ?

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

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

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

@ -112,54 +112,54 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
} }
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, double *speedRw0, void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate,
double *speedRw1, double *speedRw2, double *speedRw3, double *mgtDpDes) { int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
if ( !(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) { int32_t *speedRw3, double *mgtDpDes) {
if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) {
mgtDpDes[0] = 0; mgtDpDes[0] = 0;
mgtDpDes[1] = 0; mgtDpDes[1] = 0;
mgtDpDes[2] = 0; mgtDpDes[2] = 0;
return; 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);
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);
// 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);
// 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);
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);
// 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 speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
double wheelMomentum[4] = {0, 0, 0, 0}; double wheelMomentum[4] = {0, 0, 0, 0};
double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60;
//Conversion to [rad/s] for further calculations // Conversion to [rad/s] for further calculations
VectorOperations<double>::mulScalar(rpmOffset, factor, rpmOffset, 4); VectorOperations<double>::mulScalar(rpmOffset, factor, rpmOffset, 4);
VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4); VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
double diffRwSpeed[4] = {0, 0, 0, 0}; double diffRwSpeed[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4); VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, wheelMomentum, 4); VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel,
double gainNs = pointingModeControllerParameters->gainNullspace; wheelMomentum, 4);
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double gainNs = pointingModeControllerParameters->gainNullspace;
MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, *nullSpaceMatrix, 4); double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace,
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); *nullSpaceMatrix, 4);
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4); MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
} }

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

@ -4,8 +4,8 @@
* Created on: 17 Jul 2022 * Created on: 17 Jul 2022
* Author: Robin Marquardt * Author: Robin Marquardt
* *
* @brief: This class handles the pointing control mechanism. Calculation of an commanded torque * @brief: This class handles the pointing control mechanism. Calculation of an commanded
* for the reaction wheels, and magnetic Field strength for magnetorques for desaturation * torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation
* *
* @note: A description of the used algorithms can be found in * @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 * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110
@ -14,46 +14,49 @@
#ifndef PTGCTRL_H_ #ifndef PTGCTRL_H_
#define 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 <stdio.h>
#include <string.h>
#include <time.h> #include <time.h>
class PtgCtrl{ #include "../AcsParameters.h"
#include "../OutputValues.h"
#include "../SensorValues.h"
#include "../config/classIds.h"
public: class PtgCtrl {
/* @brief: Constructor public:
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters /* @brief: Constructor
*/ * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
PtgCtrl(AcsParameters *acsParameters_); */
virtual ~PtgCtrl(); PtgCtrl(AcsParameters *acsParameters_);
virtual ~PtgCtrl();
static const uint8_t INTERFACE_ID = CLASS_ID::PTG; static const uint8_t INTERFACE_ID = CLASS_ID::PTG;
static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
/* @brief: Load AcsParameters für this class /* @brief: Load AcsParameters für this class
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/ */
void loadAcsParameters(AcsParameters *acsParameters_); void loadAcsParameters(AcsParameters *acsParameters_);
/* @brief: Calculates the needed torque for the pointing control mechanism /* @brief: Calculates the needed torque for the pointing control mechanism
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters * @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, void ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate,
double *speedRw1, double *speedRw2, double *speedRw3, double *mgtDpDes); 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: private:
AcsParameters::PointingModeControllerParameters* pointingModeControllerParameters; AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters;
AcsParameters::RwHandlingParameters* rwHandlingParameters; AcsParameters::RwHandlingParameters *rwHandlingParameters;
AcsParameters::InertiaEIVE* inertiaEIVE; AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters::RwMatrices* rwMatrices; AcsParameters::RwMatrices *rwMatrices;
}; };
#endif /* ACS_CONTROL_PTGCTRL_H_ */ #endif /* ACS_CONTROL_PTGCTRL_H_ */