fixed GPS and STR inputs
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
This commit is contained in:
parent
2a9dc518a0
commit
84fc44fd5f
@ -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),
|
||||||
|
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws,
|
||||||
rwTrqNs, cmdSpeedRws);
|
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);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -23,9 +23,9 @@ 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};
|
||||||
@ -40,10 +40,8 @@ void ActuatorCmd::cmdSpeedToRws(const double *speedRw0, const double *speedRw1,
|
|||||||
}
|
}
|
||||||
|
|
||||||
if (maxValue > maxTrq) {
|
if (maxValue > maxTrq) {
|
||||||
|
|
||||||
double scalingFactor = maxTrq / maxValue;
|
double scalingFactor = maxTrq / maxValue;
|
||||||
VectorOperations<double>::mulScalar(torque, scalingFactor, torque, 4);
|
VectorOperations<double>::mulScalar(torque, scalingFactor, torque, 4);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Calculating the commanded speed in RPM for every reaction wheel
|
// Calculating the commanded speed in RPM for every reaction wheel
|
||||||
@ -56,9 +54,6 @@ void ActuatorCmd::cmdSpeedToRws(const double *speedRw0, const double *speedRw1,
|
|||||||
double factor = commandTime / inertiaWheel * radToRpm;
|
double factor = commandTime / inertiaWheel * radToRpm;
|
||||||
VectorOperations<double>::mulScalar(torque, factor, deltaSpeed, 4);
|
VectorOperations<double>::mulScalar(torque, factor, deltaSpeed, 4);
|
||||||
VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 4);
|
VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 4);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) {
|
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) {
|
||||||
|
@ -28,8 +28,9 @@ 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
|
||||||
|
@ -5,39 +5,28 @@
|
|||||||
* 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++) {
|
for (int i = 0; i < 3; i++) {
|
||||||
|
sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i];
|
||||||
sunTargetSafe[i] =
|
satRateSafe[i] = acsParameters.safeModeControllerParameters.satRateRef[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,
|
void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
|
||||||
@ -45,17 +34,20 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
|
|||||||
//-------------------------------------------------------------------------------------
|
//-------------------------------------------------------------------------------------
|
||||||
// Calculation of target quaternion to groundstation
|
// Calculation of target quaternion to groundstation
|
||||||
//-------------------------------------------------------------------------------------
|
//-------------------------------------------------------------------------------------
|
||||||
// Transform longitude, latitude and altitude of groundstation to cartesian coordiantes (earth fixed/centered frame)
|
// Transform longitude, latitude and altitude of groundstation to cartesian coordiantes (earth
|
||||||
|
// fixed/centered frame)
|
||||||
double groundStationCart[3] = {0, 0, 0};
|
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,
|
||||||
|
acsParameters.groundStationParameters.altitudeGs,
|
||||||
groundStationCart);
|
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};
|
||||||
@ -74,7 +66,8 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
|
|||||||
|
|
||||||
// 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);
|
||||||
@ -118,11 +111,10 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
|
|||||||
// 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}};
|
||||||
@ -142,12 +134,10 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
|
|||||||
// 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 sunDirJ[3] = {0, 0, 0};
|
||||||
double sunDirB[3] = {0, 0, 0};
|
double sunDirB[3] = {0, 0, 0};
|
||||||
|
|
||||||
if (outputValues->sunDirModelValid) {
|
if (outputValues->sunDirModelValid) {
|
||||||
|
|
||||||
sunDirJ[0] = outputValues->sunDirModel[0];
|
sunDirJ[0] = outputValues->sunDirModel[0];
|
||||||
sunDirJ[1] = outputValues->sunDirModel[1];
|
sunDirJ[1] = outputValues->sunDirModel[1];
|
||||||
sunDirJ[2] = outputValues->sunDirModel[2];
|
sunDirJ[2] = outputValues->sunDirModel[2];
|
||||||
@ -158,17 +148,16 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
|
|||||||
sunDirB[0] = outputValues->sunDirEst[0];
|
sunDirB[0] = outputValues->sunDirEst[0];
|
||||||
sunDirB[1] = outputValues->sunDirEst[1];
|
sunDirB[1] = outputValues->sunDirEst[1];
|
||||||
sunDirB[2] = outputValues->sunDirEst[2];
|
sunDirB[2] = outputValues->sunDirEst[2];
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
double exclAngle = acsParameters.strParameters.exclusionAngle,
|
double exclAngle = acsParameters.strParameters.exclusionAngle,
|
||||||
blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
|
blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
|
||||||
blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop;
|
blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop;
|
||||||
|
|
||||||
double sightAngleSun = VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB);
|
double sightAngleSun =
|
||||||
|
VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB);
|
||||||
|
|
||||||
if (!(strBlindAvoidFlag)) {
|
if (!(strBlindAvoidFlag)) {
|
||||||
|
|
||||||
double critSightAngle = blindStart * exclAngle;
|
double critSightAngle = blindStart * exclAngle;
|
||||||
|
|
||||||
if (sightAngleSun < critSightAngle) {
|
if (sightAngleSun < critSightAngle) {
|
||||||
@ -179,17 +168,14 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
|
|||||||
|
|
||||||
else {
|
else {
|
||||||
if (sightAngleSun < blindEnd * exclAngle) {
|
if (sightAngleSun < blindEnd * exclAngle) {
|
||||||
|
|
||||||
double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate;
|
double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate;
|
||||||
double blindRefRate[3] = {0, 0, 0};
|
double blindRefRate[3] = {0, 0, 0};
|
||||||
|
|
||||||
|
|
||||||
if (sunDirB[1] < 0) {
|
if (sunDirB[1] < 0) {
|
||||||
blindRefRate[0] = normBlindRefRate;
|
blindRefRate[0] = normBlindRefRate;
|
||||||
blindRefRate[1] = 0;
|
blindRefRate[1] = 0;
|
||||||
blindRefRate[2] = 0;
|
blindRefRate[2] = 0;
|
||||||
}
|
} else {
|
||||||
else {
|
|
||||||
blindRefRate[0] = -normBlindRefRate;
|
blindRefRate[0] = -normBlindRefRate;
|
||||||
blindRefRate[1] = 0;
|
blindRefRate[1] = 0;
|
||||||
blindRefRate[2] = 0;
|
blindRefRate[2] = 0;
|
||||||
@ -197,17 +183,15 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues
|
|||||||
|
|
||||||
VectorOperations<double>::add(blindRefRate, refSatRate, refSatRate, 3);
|
VectorOperations<double>::add(blindRefRate, refSatRate, refSatRate, 3);
|
||||||
|
|
||||||
}
|
} else {
|
||||||
else {
|
|
||||||
strBlindAvoidFlag = false;
|
strBlindAvoidFlag = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
|
||||||
void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] ) {
|
double refSatRate[3], double quatError[3], double deltaRate[3]) {
|
||||||
|
|
||||||
double quatRef[4] = {0, 0, 0, 0};
|
double quatRef[4] = {0, 0, 0, 0};
|
||||||
quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0];
|
quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0];
|
||||||
quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1];
|
quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1];
|
||||||
@ -238,14 +222,11 @@ void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
|
|||||||
|
|
||||||
// target flag in matlab, importance, does look like it only gives
|
// target flag in matlab, importance, does look like it only gives
|
||||||
// feedback if pointing control is under 150 arcsec ??
|
// feedback if pointing control is under 150 arcsec ??
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) {
|
void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) {
|
||||||
|
if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() && sensorValues->rw3Set.isValid() &&
|
||||||
if (sensorValues->validRw0 && sensorValues->validRw1 && sensorValues->validRw2 && sensorValues->validRw3) {
|
sensorValues->rw4Set.isValid()) {
|
||||||
|
|
||||||
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
|
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
|
||||||
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
|
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
|
||||||
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
|
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
|
||||||
@ -261,8 +242,8 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
else if (!(sensorValues->validRw0) && sensorValues->validRw1 && sensorValues->validRw2 && sensorValues->validRw3) {
|
else if (!(sensorValues->rw1Set.isValid()) && sensorValues->rw2Set.isValid() &&
|
||||||
|
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
|
||||||
rwPseudoInv[0] = acsParameters.rwMatrices.without0[0][0];
|
rwPseudoInv[0] = acsParameters.rwMatrices.without0[0][0];
|
||||||
rwPseudoInv[1] = acsParameters.rwMatrices.without0[0][1];
|
rwPseudoInv[1] = acsParameters.rwMatrices.without0[0][1];
|
||||||
rwPseudoInv[2] = acsParameters.rwMatrices.without0[0][2];
|
rwPseudoInv[2] = acsParameters.rwMatrices.without0[0][2];
|
||||||
@ -277,8 +258,8 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *
|
|||||||
rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2];
|
rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2];
|
||||||
}
|
}
|
||||||
|
|
||||||
else if ((sensorValues->validRw0) && !(sensorValues->validRw1) && sensorValues->validRw2 && sensorValues->validRw3) {
|
else if ((sensorValues->rw1Set.isValid()) && !(sensorValues->rw2Set.isValid()) &&
|
||||||
|
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
|
||||||
rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0];
|
rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0];
|
||||||
rwPseudoInv[1] = acsParameters.rwMatrices.without1[0][1];
|
rwPseudoInv[1] = acsParameters.rwMatrices.without1[0][1];
|
||||||
rwPseudoInv[2] = acsParameters.rwMatrices.without1[0][2];
|
rwPseudoInv[2] = acsParameters.rwMatrices.without1[0][2];
|
||||||
@ -293,8 +274,8 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *
|
|||||||
rwPseudoInv[11] = acsParameters.rwMatrices.without1[3][2];
|
rwPseudoInv[11] = acsParameters.rwMatrices.without1[3][2];
|
||||||
}
|
}
|
||||||
|
|
||||||
else if ((sensorValues->validRw0) && (sensorValues->validRw1) && !(sensorValues->validRw2) && sensorValues->validRw3) {
|
else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) &&
|
||||||
|
!(sensorValues->rw3Set.isValid()) && sensorValues->rw4Set.isValid()) {
|
||||||
rwPseudoInv[0] = acsParameters.rwMatrices.without2[0][0];
|
rwPseudoInv[0] = acsParameters.rwMatrices.without2[0][0];
|
||||||
rwPseudoInv[1] = acsParameters.rwMatrices.without2[0][1];
|
rwPseudoInv[1] = acsParameters.rwMatrices.without2[0][1];
|
||||||
rwPseudoInv[2] = acsParameters.rwMatrices.without2[0][2];
|
rwPseudoInv[2] = acsParameters.rwMatrices.without2[0][2];
|
||||||
@ -309,8 +290,8 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *
|
|||||||
rwPseudoInv[11] = acsParameters.rwMatrices.without2[3][2];
|
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[0] = acsParameters.rwMatrices.without3[0][0];
|
||||||
rwPseudoInv[1] = acsParameters.rwMatrices.without3[0][1];
|
rwPseudoInv[1] = acsParameters.rwMatrices.without3[0][1];
|
||||||
rwPseudoInv[2] = acsParameters.rwMatrices.without3[0][2];
|
rwPseudoInv[2] = acsParameters.rwMatrices.without3[0][2];
|
||||||
@ -342,6 +323,5 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *
|
|||||||
rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0];
|
rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0];
|
||||||
rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1];
|
rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1];
|
||||||
rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2];
|
rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2];
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -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 ?
|
||||||
|
@ -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();
|
||||||
|
@ -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 */
|
||||||
|
|
||||||
|
@ -112,39 +112,39 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, double *speedRw0,
|
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,
|
||||||
|
int32_t *speedRw3, double *mgtDpDes) {
|
||||||
if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) {
|
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
|
// calculating momentum of satellite and momentum of reaction wheels
|
||||||
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
|
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
|
||||||
double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
|
double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
|
||||||
VectorOperations<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4);
|
VectorOperations<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4);
|
||||||
MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, 1);
|
MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4,
|
||||||
|
1);
|
||||||
double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0};
|
double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0};
|
||||||
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1);
|
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1);
|
||||||
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
|
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
|
||||||
// calculating momentum error
|
// calculating momentum error
|
||||||
double deltaMomentum[3] = {0, 0, 0};
|
double deltaMomentum[3] = {0, 0, 0};
|
||||||
VectorOperations<double>::subtract(momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3);
|
VectorOperations<double>::subtract(
|
||||||
|
momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3);
|
||||||
// resulting magnetic dipole command
|
// resulting magnetic dipole command
|
||||||
double crossMomentumMagField[3] = {0, 0, 0};
|
double crossMomentumMagField[3] = {0, 0, 0};
|
||||||
VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
|
VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
|
||||||
double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
|
double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
|
||||||
factor = (pointingModeControllerParameters->deSatGainFactor) / normMag;
|
factor = (pointingModeControllerParameters->deSatGainFactor) / normMag;
|
||||||
VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
|
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;
|
||||||
@ -153,13 +153,13 @@ void PtgCtrl::ptgNullspace(const double *speedRw0, const double *speedRw1, const
|
|||||||
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,
|
||||||
|
wheelMomentum, 4);
|
||||||
double gainNs = pointingModeControllerParameters->gainNullspace;
|
double gainNs = pointingModeControllerParameters->gainNullspace;
|
||||||
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||||
MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, *nullSpaceMatrix, 4);
|
MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace,
|
||||||
|
*nullSpaceMatrix, 4);
|
||||||
MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
|
MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
|
||||||
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
|
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
|
||||||
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
|
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
@ -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,16 +14,16 @@
|
|||||||
#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"
|
||||||
|
|
||||||
|
class PtgCtrl {
|
||||||
public:
|
public:
|
||||||
/* @brief: Constructor
|
/* @brief: Constructor
|
||||||
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
|
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
|
||||||
@ -42,12 +42,15 @@ public:
|
|||||||
/* @brief: Calculates the needed torque for the pointing control mechanism
|
/* @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;
|
||||||
|
Loading…
Reference in New Issue
Block a user