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;
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
@ -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<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);
|
||||
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<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);
|
||||
}
|
||||
|
||||
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) {
|
||||
|
||||
@ -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
|
||||
|
||||
@ -5,343 +5,323 @@
|
||||
* Author: Robin Marquardt
|
||||
*/
|
||||
|
||||
|
||||
#include "Guidance.h"
|
||||
#include "string.h"
|
||||
#include "util/MathOperations.h"
|
||||
#include "util/CholeskyDecomposition.h"
|
||||
#include <math.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <math.h>
|
||||
|
||||
Guidance::Guidance(AcsParameters *acsParameters_) {
|
||||
acsParameters = *acsParameters_;
|
||||
#include "string.h"
|
||||
#include "util/CholeskyDecomposition.h"
|
||||
#include "util/MathOperations.h"
|
||||
|
||||
}
|
||||
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; }
|
||||
|
||||
Guidance::~Guidance() {
|
||||
|
||||
}
|
||||
Guidance::~Guidance() {}
|
||||
|
||||
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i];
|
||||
satRateSafe[i] = acsParameters.safeModeControllerParameters.satRateRef[i];
|
||||
}
|
||||
|
||||
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<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs,
|
||||
acsParameters.groundStationParameters.longitudeGs, acsParameters.groundStationParameters.altitudeGs,
|
||||
groundStationCart);
|
||||
MathOperations<double>::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<double>::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<double>::cartesianFromLatLongAlt(sensorValues->gpsSet.latitude.value,
|
||||
sensorValues->gpsSet.longitude.value,
|
||||
sensorValues->gpsSet.altitude.value, posSatE);
|
||||
|
||||
// Target direction in the ECEF frame
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
|
||||
// Target direction in the ECEF frame
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::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<double>::dcmEJ(now, *dcmEJ);
|
||||
MathOperations<double>::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<double>::dcmEJ(now, *dcmEJ);
|
||||
MathOperations<double>::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<double>::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<double>::multiply(&acsParameters.magnetorquesParameter.mtq0orientationMatrix,
|
||||
// dcmTEST, dcmTEST, 3, 3, 3);
|
||||
|
||||
MatrixOperations<double>::multiply(*dcmDot, *dcmEJ, *dcmEJDot, 3, 3, 3);
|
||||
MatrixOperations<double>::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
|
||||
MatrixOperations<double>::multiply(*dcmDot, *dcmEJ, *dcmEJDot, 3, 3, 3);
|
||||
MatrixOperations<double>::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3);
|
||||
MathOperations<double>::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<double>::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<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
|
||||
|
||||
// Target Direction in the body frame
|
||||
double targetDirB[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
|
||||
// Target Direction in the body frame
|
||||
double targetDirB[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::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<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
|
||||
VectorOperations<double>::normalize(refDir, refDir, 3);
|
||||
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
|
||||
double normRefDir = VectorOperations<double>::norm(refDir, 3);
|
||||
double crossDir[3] = {0, 0, 0};
|
||||
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
|
||||
VectorOperations<double>::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<double>::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<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
|
||||
VectorOperations<double>::normalize(refDir, refDir, 3);
|
||||
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
|
||||
double normRefDir = VectorOperations<double>::norm(refDir, 3);
|
||||
double crossDir[3] = {0, 0, 0};
|
||||
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
|
||||
VectorOperations<double>::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<double>::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<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
|
||||
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(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1);
|
||||
VectorOperations<double>::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<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
|
||||
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(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1);
|
||||
VectorOperations<double>::add(velSatBPart1, velSatBPart2, velSatB, 3);
|
||||
|
||||
double normVelSatB = VectorOperations<double>::norm(velSatB, 3);
|
||||
double normRefSatRate = normVelSatB / normTargetDirB;
|
||||
double normVelSatB = VectorOperations<double>::norm(velSatB, 3);
|
||||
double normRefSatRate = normVelSatB / normTargetDirB;
|
||||
|
||||
double satRateDir[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(velSatB, targetDirB, satRateDir);
|
||||
VectorOperations<double>::normalize(satRateDir, satRateDir, 3);
|
||||
VectorOperations<double>::mulScalar(satRateDir, normRefSatRate, refSatRate, 3);
|
||||
double satRateDir[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(velSatB, targetDirB, satRateDir);
|
||||
VectorOperations<double>::normalize(satRateDir, satRateDir, 3);
|
||||
VectorOperations<double>::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<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];
|
||||
sunDirJ[1] = outputValues->sunDirModel[1];
|
||||
sunDirJ[2] = outputValues->sunDirModel[2];
|
||||
MatrixOperations<double>::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<double>::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<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) {
|
||||
strBlindAvoidFlag = true;
|
||||
}
|
||||
VectorOperations<double>::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<double>::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<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};
|
||||
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<double>::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<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] }};
|
||||
if (quatErrorComplete[3] < 0) {
|
||||
quatErrorComplete[3] *= -1;
|
||||
}
|
||||
|
||||
double quatErrorComplete[4] = {0, 0, 0, 0};
|
||||
MatrixOperations<double>::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];
|
||||
}
|
||||
}
|
||||
|
||||
@ -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 ?
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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 */
|
||||
|
||||
|
||||
@ -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<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::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate,
|
||||
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
|
||||
int32_t *speedRw3, double *mgtDpDes) {
|
||||
if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) {
|
||||
mgtDpDes[0] = 0;
|
||||
mgtDpDes[1] = 0;
|
||||
mgtDpDes[2] = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// calculating momentum of satellite and momentum of reaction wheels
|
||||
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
|
||||
double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4);
|
||||
MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4,
|
||||
1);
|
||||
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) {
|
||||
|
||||
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<double>::mulScalar(rpmOffset, factor, rpmOffset, 4);
|
||||
VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
|
||||
double diffRwSpeed[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
|
||||
VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, wheelMomentum, 4);
|
||||
double gainNs = pointingModeControllerParameters->gainNullspace;
|
||||
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, *nullSpaceMatrix, 4);
|
||||
MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
|
||||
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
|
||||
VectorOperations<double>::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<double>::mulScalar(rpmOffset, factor, rpmOffset, 4);
|
||||
VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
|
||||
double diffRwSpeed[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
|
||||
VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel,
|
||||
wheelMomentum, 4);
|
||||
double gainNs = pointingModeControllerParameters->gainNullspace;
|
||||
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace,
|
||||
*nullSpaceMatrix, 4);
|
||||
MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
|
||||
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
|
||||
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
|
||||
}
|
||||
|
||||
@ -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 <string.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
class PtgCtrl{
|
||||
#include "../AcsParameters.h"
|
||||
#include "../OutputValues.h"
|
||||
#include "../SensorValues.h"
|
||||
#include "../config/classIds.h"
|
||||
|
||||
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_ */
|
||||
|
||||
Loading…
Reference in New Issue
Block a user