From 84fc44fd5fcf9d7794f2c5bf222be52fda130430 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 12 Oct 2022 15:06:24 +0200 Subject: [PATCH] fixed GPS and STR inputs --- mission/controller/AcsController.cpp | 20 +- mission/controller/acs/ActuatorCmd.cpp | 61 +-- mission/controller/acs/ActuatorCmd.h | 7 +- mission/controller/acs/Guidance.cpp | 558 ++++++++++---------- mission/controller/acs/SensorProcessing.cpp | 4 +- mission/controller/acs/SensorValues.cpp | 17 + mission/controller/acs/SensorValues.h | 44 +- mission/controller/acs/control/PtgCtrl.cpp | 94 ++-- mission/controller/acs/control/PtgCtrl.h | 67 +-- 9 files changed, 434 insertions(+), 438 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 84a7682f..856a0034 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -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, - rwTrqNs, cmdSpeedRws); + 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); } diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 261277a0..920989ed 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -23,42 +23,37 @@ 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){ - - using namespace Math; - // Scaling the commanded torque to a maximum value - double torque[4] = {0, 0, 0, 0}; - double maxTrq = acsParameters.rwHandlingParameters.maxTrq; - VectorOperations::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::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::mulScalar(torque, factor, deltaSpeed, 4); - VectorOperations::add(speedRws, deltaSpeed, rwCmdSpeed, 4); +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}; + double maxTrq = acsParameters.rwHandlingParameters.maxTrq; + VectorOperations::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::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::mulScalar(torque, factor, deltaSpeed, 4); + VectorOperations::add(speedRws, deltaSpeed, rwCmdSpeed, 4); } void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) { diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 820f1f00..1da2c89a 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -28,10 +28,11 @@ 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 * * @param: dipolMoment given dipol moment in spacecraft frame diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 7d9cd8df..f6ebee5f 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -5,343 +5,323 @@ * Author: Robin Marquardt */ - #include "Guidance.h" -#include "string.h" -#include "util/MathOperations.h" -#include "util/CholeskyDecomposition.h" -#include -#include + #include #include +#include +#include -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]; + } - for (int i = 0; i < 3; i++) { - - sunTargetSafe[i] = - acsParameters.safeModeControllerParameters.sunTargetDir[i]; - satRateSafe[i] = - acsParameters.safeModeControllerParameters.satRateRef[i]; - - } - - -// memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24); - + // memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24); } -void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, - timeval now, double targetQuat[4], double refSatRate[3]){ -//------------------------------------------------------------------------------------- -// Calculation of target quaternion to groundstation -//------------------------------------------------------------------------------------- -// Transform longitude, latitude and altitude of groundstation to cartesian coordiantes (earth fixed/centered frame) - double groundStationCart[3] = {0, 0, 0}; +void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + timeval now, double targetQuat[4], double refSatRate[3]) { + //------------------------------------------------------------------------------------- + // Calculation of target quaternion to groundstation + //------------------------------------------------------------------------------------- + // Transform longitude, latitude and altitude of groundstation to cartesian coordiantes (earth + // fixed/centered frame) + double groundStationCart[3] = {0, 0, 0}; - MathOperations::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs, - acsParameters.groundStationParameters.longitudeGs, acsParameters.groundStationParameters.altitudeGs, - groundStationCart); + MathOperations::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs, + 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::cartesianFromLatLongAlt(sensorValues->gps0latitude, sensorValues->gps0longitude, - sensorValues->gps0altitude, posSatE); + // Position of the satellite in the earth/fixed frame via GPS + double posSatE[3] = {0, 0, 0}; + MathOperations::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}; - VectorOperations::subtract(groundStationCart, posSatE, targetDirE, 3); + // Target direction in the ECEF frame + double targetDirE[3] = {0, 0, 0}; + VectorOperations::subtract(groundStationCart, posSatE, targetDirE, 3); -// 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::dcmEJ(now, *dcmEJ); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); -// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION - 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 dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; - double omegaEarth = acsParameters.targetModeControllerParameters.omegaEarth; + // 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::dcmEJ(now, *dcmEJ); + MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + // Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION + 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 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::multiply(&acsParameters.magnetorquesParameter.mtq0orientationMatrix, dcmTEST, dcmTEST, 3, 3, 3); + // TEST SECTION ! + // double dcmTEST[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + // MatrixOperations::multiply(&acsParameters.magnetorquesParameter.mtq0orientationMatrix, + // dcmTEST, dcmTEST, 3, 3, 3); - MatrixOperations::multiply(*dcmDot, *dcmEJ, *dcmEJDot, 3, 3, 3); - MatrixOperations::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3); - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + MatrixOperations::multiply(*dcmDot, *dcmEJ, *dcmEJDot, 3, 3, 3); + MatrixOperations::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3); + MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); -// Transformation between ECEF and Body frame - 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 quatBJ[4] = {0, 0, 0, 0}; - quatBJ[0] = outputValues->quatMekfBJ[0]; - quatBJ[1] = outputValues->quatMekfBJ[1]; - quatBJ[2] = outputValues->quatMekfBJ[2]; - quatBJ[3] = outputValues->quatMekfBJ[3]; - QuaternionOperations::toDcm(quatBJ, dcmBJ); - MatrixOperations::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); + // Transformation between ECEF and Body frame + 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 quatBJ[4] = {0, 0, 0, 0}; + quatBJ[0] = outputValues->quatMekfBJ[0]; + quatBJ[1] = outputValues->quatMekfBJ[1]; + quatBJ[2] = outputValues->quatMekfBJ[2]; + quatBJ[3] = outputValues->quatMekfBJ[3]; + QuaternionOperations::toDcm(quatBJ, dcmBJ); + MatrixOperations::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); -// Target Direction in the body frame - double targetDirB[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); + // Target Direction in the body frame + double targetDirB[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); -// rotation quaternion from two vectors - double refDir[3] = {0, 0, 0}; - refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0]; - refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1]; - refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2]; - double noramlizedTargetDirB[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); - VectorOperations::normalize(refDir, refDir, 3); - double normTargetDirB = VectorOperations::norm(noramlizedTargetDirB, 3); - double normRefDir = VectorOperations::norm(refDir, 3); - double crossDir[3] = {0, 0, 0}; - double dotDirections = VectorOperations::dot(noramlizedTargetDirB, refDir); - VectorOperations::cross(noramlizedTargetDirB, refDir, crossDir); - targetQuat[0] = crossDir[0]; - targetQuat[1] = crossDir[1]; - targetQuat[2] = crossDir[2]; - targetQuat[3] = sqrt(pow(normTargetDirB,2) * pow(normRefDir,2) + dotDirections); - VectorOperations::normalize(targetQuat, targetQuat, 4); + // rotation quaternion from two vectors + double refDir[3] = {0, 0, 0}; + refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0]; + refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1]; + refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2]; + double noramlizedTargetDirB[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); + VectorOperations::normalize(refDir, refDir, 3); + double normTargetDirB = VectorOperations::norm(noramlizedTargetDirB, 3); + double normRefDir = VectorOperations::norm(refDir, 3); + double crossDir[3] = {0, 0, 0}; + double dotDirections = VectorOperations::dot(noramlizedTargetDirB, refDir); + VectorOperations::cross(noramlizedTargetDirB, refDir, crossDir); + targetQuat[0] = crossDir[0]; + targetQuat[1] = crossDir[1]; + targetQuat[2] = crossDir[2]; + targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections); + VectorOperations::normalize(targetQuat, targetQuat, 4); -//------------------------------------------------------------------------------------- -// 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}; -// Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E - MatrixOperations::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1); - double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::multiply(*dcmBJ, *dcmJEDot, *dcmBEDot, 3, 3, 3); - MatrixOperations::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1); - VectorOperations::add(velSatBPart1, velSatBPart2, velSatB, 3); + //------------------------------------------------------------------------------------- + // Calculation of reference rotation rate + //------------------------------------------------------------------------------------- + double velSatE[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::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1); + double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(*dcmBJ, *dcmJEDot, *dcmBEDot, 3, 3, 3); + MatrixOperations::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1); + VectorOperations::add(velSatBPart1, velSatBPart2, velSatB, 3); - double normVelSatB = VectorOperations::norm(velSatB, 3); - double normRefSatRate = normVelSatB / normTargetDirB; + double normVelSatB = VectorOperations::norm(velSatB, 3); + double normRefSatRate = normVelSatB / normTargetDirB; - double satRateDir[3] = {0, 0, 0}; - VectorOperations::cross(velSatB, targetDirB, satRateDir); - VectorOperations::normalize(satRateDir, satRateDir, 3); - VectorOperations::mulScalar(satRateDir, normRefSatRate, refSatRate, 3); + double satRateDir[3] = {0, 0, 0}; + VectorOperations::cross(velSatB, targetDirB, satRateDir); + VectorOperations::normalize(satRateDir, satRateDir, 3); + VectorOperations::mulScalar(satRateDir, normRefSatRate, refSatRate, 3); -//------------------------------------------------------------------------------------- -// Calculation of reference rotation rate in case of star tracker blinding -//------------------------------------------------------------------------------------- - if ( acsParameters.targetModeControllerParameters.avoidBlindStr ) { + //------------------------------------------------------------------------------------- + // 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}; - 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]; + MatrixOperations::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]; - sunDirJ[1] = outputValues->sunDirModel[1]; - sunDirJ[2] = outputValues->sunDirModel[2]; - MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); - } + double exclAngle = acsParameters.strParameters.exclusionAngle, + blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, + blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop; - else { - sunDirB[0] = outputValues->sunDirEst[0]; - sunDirB[1] = outputValues->sunDirEst[1]; - sunDirB[2] = outputValues->sunDirEst[2]; + double sightAngleSun = + VectorOperations::dot(acsParameters.strParameters.boresightAxis, sunDirB); - } + if (!(strBlindAvoidFlag)) { + double critSightAngle = blindStart * exclAngle; - double exclAngle = acsParameters.strParameters.exclusionAngle, - blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, - blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop; + if (sightAngleSun < critSightAngle) { + strBlindAvoidFlag = true; + } - double sightAngleSun = VectorOperations::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) { - strBlindAvoidFlag = true; - } + VectorOperations::add(blindRefRate, refSatRate, refSatRate, 3); - } - - 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::add(blindRefRate, refSatRate, refSatRate, 3); - - } - else { - strBlindAvoidFlag = false; - } - } - } + } 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::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}; - quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0]; - quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1]; - quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2]; - quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3]; + double quatErrorComplete[4] = {0, 0, 0, 0}; + MatrixOperations::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1); - double satRate[3] = {0, 0, 0}; - satRate[0] = outputValues->satRateMekf[0]; - satRate[1] = outputValues->satRateMekf[1]; - satRate[2] = outputValues->satRateMekf[2]; - VectorOperations::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] }}; + if (quatErrorComplete[3] < 0) { + quatErrorComplete[3] *= -1; + } - double quatErrorComplete[4] = {0, 0, 0, 0}; - MatrixOperations::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1); - - 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 ?? + 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 ?? } +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]; - 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]; + 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]; + 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->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]; - 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]; - } - - else if ((sensorValues->validRw0) && !(sensorValues->validRw1) && sensorValues->validRw2 && sensorValues->validRw3) { - - rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0]; - 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]; - - } + 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]; + } } diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index bf9894e2..93b9eb47 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -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 ? diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index b38a35b1..1492ad6e 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -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(); diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index 4e3a215f..08fe5177 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -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 */ diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 999df912..6201a32a 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -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, - double *speedRw1, double *speedRw2, double *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::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4); - MatrixOperations::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, 1); - double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0}; - MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1); - VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); -// calculating momentum error - double deltaMomentum[3] = {0, 0, 0}; - VectorOperations::subtract(momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3); -// resulting magnetic dipole command - double crossMomentumMagField[3] = {0, 0, 0}; - VectorOperations::cross(deltaMomentum, magFieldEst, crossMomentumMagField); - double normMag = VectorOperations::norm(magFieldEst, 3), factor = 0; - factor = (pointingModeControllerParameters->deSatGainFactor) / normMag; - VectorOperations::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); +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::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4); + MatrixOperations::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, + 1); + double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0}; + MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1); + VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); + // calculating momentum error + double deltaMomentum[3] = {0, 0, 0}; + VectorOperations::subtract( + momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3); + // resulting magnetic dipole command + double crossMomentumMagField[3] = {0, 0, 0}; + VectorOperations::cross(deltaMomentum, magFieldEst, crossMomentumMagField); + double normMag = VectorOperations::norm(magFieldEst, 3), factor = 0; + factor = (pointingModeControllerParameters->deSatGainFactor) / normMag; + VectorOperations::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); } -void PtgCtrl::ptgNullspace(const double *speedRw0, const double *speedRw1, const double *speedRw2, const double *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; - //Conversion to [rad/s] for further calculations - VectorOperations::mulScalar(rpmOffset, factor, rpmOffset, 4); - VectorOperations::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4); - double diffRwSpeed[4] = {0, 0, 0, 0}; - VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); - VectorOperations::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::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, *nullSpaceMatrix, 4); - MatrixOperations::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); - VectorOperations::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); - VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); - - +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; + // Conversion to [rad/s] for further calculations + VectorOperations::mulScalar(rpmOffset, factor, rpmOffset, 4); + VectorOperations::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4); + double diffRwSpeed[4] = {0, 0, 0, 0}; + VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); + VectorOperations::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::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, + *nullSpaceMatrix, 4); + MatrixOperations::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); + VectorOperations::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); + VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); } diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 87a66f2d..be67187d 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -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,46 +14,49 @@ #ifndef PTGCTRL_H_ #define PTGCTRL_H_ -#include "../SensorValues.h" -#include "../OutputValues.h" -#include "../AcsParameters.h" -#include "../config/classIds.h" -#include #include +#include #include -class PtgCtrl{ +#include "../AcsParameters.h" +#include "../OutputValues.h" +#include "../SensorValues.h" +#include "../config/classIds.h" -public: - /* @brief: Constructor - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters - */ - PtgCtrl(AcsParameters *acsParameters_); - virtual ~PtgCtrl(); +class PtgCtrl { + public: + /* @brief: Constructor + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + PtgCtrl(AcsParameters *acsParameters_); + virtual ~PtgCtrl(); - static const uint8_t INTERFACE_ID = CLASS_ID::PTG; - static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); + static const uint8_t INTERFACE_ID = CLASS_ID::PTG; + static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); - /* @brief: Load AcsParameters für this class - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters - */ - void loadAcsParameters(AcsParameters *acsParameters_); + /* @brief: Load AcsParameters für this class + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + void loadAcsParameters(AcsParameters *acsParameters_); - /* @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); + /* @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 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; - AcsParameters::RwHandlingParameters* rwHandlingParameters; - AcsParameters::InertiaEIVE* inertiaEIVE; - AcsParameters::RwMatrices* rwMatrices; + private: + AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters; + AcsParameters::RwHandlingParameters *rwHandlingParameters; + AcsParameters::InertiaEIVE *inertiaEIVE; + AcsParameters::RwMatrices *rwMatrices; }; #endif /* ACS_CONTROL_PTGCTRL_H_ */