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

fixed booleans in acs Parameters and enabled setting of parameters
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2022-12-16 13:59:49 +01:00
parent 3f2910c3a7
commit b3a2cc4367
3 changed files with 549 additions and 543 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

189
mission/controller/acs/AcsParameters.cpp
View File

@ -3,16 +3,15 @@
#include <fsfw/src/fsfw/globalfunctions/constants.h> #include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <stddef.h> #include <stddef.h>
AcsParameters::AcsParameters(){}; //(uint8_t parameterModuleId) : AcsParameters::AcsParameters() {}
// parameterModuleId(parameterModuleId) {}
AcsParameters::~AcsParameters() {} AcsParameters::~AcsParameters() {}
/*ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint16_t parameterId, ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
ParameterWrapper* parameterWrapper, ParameterWrapper* parameterWrapper,
const ParameterWrapper* newValues, const ParameterWrapper* newValues,
uint16_t startAtIndex) { uint16_t startAtIndex) {
if (domainId == parameterModuleId) { // if (domainId == parameterModuleId) {
switch (parameterId >> 8) { switch (parameterId >> 8) {
case 0x0: // direct members case 0x0: // direct members
switch (parameterId & 0xFF) { switch (parameterId & 0xFF) {
@ -35,7 +34,16 @@ AcsParameters::~AcsParameters() {}
parameterWrapper->set(inertiaEIVE.inertiaMatrix); parameterWrapper->set(inertiaEIVE.inertiaMatrix);
break; break;
case 0x1: case 0x1:
parameterWrapper->set(inertiaEIVE.inertiaMatrixInverse); parameterWrapper->set(inertiaEIVE.inertiaMatrixDeployed);
break;
case 0x2:
parameterWrapper->set(inertiaEIVE.inertiaMatrixUndeployed);
break;
case 0x3:
parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel1);
break;
case 0x4:
parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel3);
break; break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
@ -88,6 +96,15 @@ AcsParameters::~AcsParameters() {}
case 0xE: case 0xE:
parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse); parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse);
break; break;
case 0xF:
parameterWrapper->set(mgmHandlingParameters.mgm02variance);
break;
case 0x10:
parameterWrapper->set(mgmHandlingParameters.mgm13variance);
break;
case 0x11:
parameterWrapper->set(mgmHandlingParameters.mgm4variance);
break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
@ -227,7 +244,13 @@ AcsParameters::~AcsParameters() {}
parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix); parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix);
break; break;
case 0x4: case 0x4:
parameterWrapper->set(gyrHandlingParameters.gyrFusionWeight); parameterWrapper->set(gyrHandlingParameters.gyr02variance);
break;
case 0x5:
parameterWrapper->set(gyrHandlingParameters.gyr13variance);
break;
case 0x6:
parameterWrapper->set(gyrHandlingParameters.preferAdis);
break; break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
@ -253,6 +276,15 @@ AcsParameters::~AcsParameters() {}
case 0x5: case 0x5:
parameterWrapper->set(rwHandlingParameters.maxTrq); parameterWrapper->set(rwHandlingParameters.maxTrq);
break; break;
case 0x6:
parameterWrapper->set(rwHandlingParameters.stictionSpeed);
break;
case 0x7:
parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed);
break;
case 0x8:
parameterWrapper->set(rwHandlingParameters.stictionTorque);
break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
@ -277,6 +309,9 @@ AcsParameters::~AcsParameters() {}
case 0x5: case 0x5:
parameterWrapper->set(rwMatrices.without3); parameterWrapper->set(rwMatrices.without3);
break; break;
case 0x6:
parameterWrapper->set(rwMatrices.nullspace);
break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
@ -308,82 +343,73 @@ AcsParameters::~AcsParameters() {}
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
break; break;
case (0x9): // DetumbleCtrlParameters case (0x9): // PointingModeControllerParameters
switch (parameterId & 0xFF) { switch (parameterId & 0xFF) {
case 0x0: case 0x0:
parameterWrapper->set(detumbleCtrlParameters.gainD);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0xA): // PointingModeControllerParameters
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(targetModeControllerParameters.updtFlag);
break;
case 0x1:
parameterWrapper->set(targetModeControllerParameters.A_rw);
break;
case 0x2:
parameterWrapper->set(targetModeControllerParameters.refDirection); parameterWrapper->set(targetModeControllerParameters.refDirection);
break; break;
case 0x3: case 0x1:
parameterWrapper->set(targetModeControllerParameters.refRotRate); parameterWrapper->set(targetModeControllerParameters.refRotRate);
break; break;
case 0x4: case 0x2:
parameterWrapper->set(targetModeControllerParameters.quatRef); parameterWrapper->set(targetModeControllerParameters.quatRef);
break; break;
case 0x5: case 0x3:
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
break; break;
case 0x6: case 0x4:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
break; break;
case 0x7: case 0x5:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
break; break;
case 0x8: case 0x6:
parameterWrapper->set(targetModeControllerParameters.blindRotRate); parameterWrapper->set(targetModeControllerParameters.blindRotRate);
break; break;
case 0x9: case 0x7:
parameterWrapper->set(targetModeControllerParameters.zeta); parameterWrapper->set(targetModeControllerParameters.zeta);
break; break;
case 0xA: case 0x8:
parameterWrapper->set(targetModeControllerParameters.zetaLow);
break;
case 0xB:
parameterWrapper->set(targetModeControllerParameters.om); parameterWrapper->set(targetModeControllerParameters.om);
break; break;
case 0xC: case 0x9:
parameterWrapper->set(targetModeControllerParameters.omLow);
break;
case 0xD:
parameterWrapper->set(targetModeControllerParameters.omMax); parameterWrapper->set(targetModeControllerParameters.omMax);
break; break;
case 0xE: case 0xA:
parameterWrapper->set(targetModeControllerParameters.qiMin); parameterWrapper->set(targetModeControllerParameters.qiMin);
break; break;
case 0xF: case 0xB:
parameterWrapper->set(targetModeControllerParameters.gainNullspace); parameterWrapper->set(targetModeControllerParameters.gainNullspace);
break; break;
case 0x10: case 0xC:
parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
break; break;
case 0x11: case 0xD:
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
break; break;
case 0x12: case 0xE:
parameterWrapper->set(targetModeControllerParameters.desatOn); parameterWrapper->set(targetModeControllerParameters.desatOn);
break; break;
case 0x13: case 0xF:
parameterWrapper->set(targetModeControllerParameters.omegaEarth); parameterWrapper->set(targetModeControllerParameters.omegaEarth);
break; break;
case 0x10:
parameterWrapper->set(targetModeControllerParameters.nadirRefDirection);
break;
case 0x11:
parameterWrapper->set(targetModeControllerParameters.tgtQuatInertial);
break;
case 0x12:
parameterWrapper->set(targetModeControllerParameters.tgtRotRateInertial);
break;
case 0x13:
parameterWrapper->set(targetModeControllerParameters.nadirTimeElapsedMax);
break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
break; break;
case (0xB): // StrParameters case (0xA): // StrParameters
switch (parameterId & 0xFF) { switch (parameterId & 0xFF) {
case 0x0: case 0x0:
parameterWrapper->set(strParameters.exclusionAngle); parameterWrapper->set(strParameters.exclusionAngle);
@ -395,13 +421,16 @@ AcsParameters::~AcsParameters() {}
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
break; break;
case (0xC): // GpsParameters case (0xB): // GpsParameters
switch (parameterId & 0xFF) { switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(gpsParameters.timeDiffVelocityMax);
break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
break; break;
case (0xD): // GroundStationParameters case (0xC): // GroundStationParameters
switch (parameterId & 0xFF) { switch (parameterId & 0xFF) {
case 0x0: case 0x0:
parameterWrapper->set(groundStationParameters.latitudeGs); parameterWrapper->set(groundStationParameters.latitudeGs);
@ -412,90 +441,65 @@ AcsParameters::~AcsParameters() {}
case 0x2: case 0x2:
parameterWrapper->set(groundStationParameters.altitudeGs); parameterWrapper->set(groundStationParameters.altitudeGs);
break; break;
case 0x3:
parameterWrapper->set(groundStationParameters.earthRadiusEquat);
break;
case 0x4:
parameterWrapper->set(groundStationParameters.earthRadiusPolar);
break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
break; break;
case (0xE): // SunModelParameters case (0xD): // SunModelParameters
switch (parameterId & 0xFF) { switch (parameterId & 0xFF) {
case 0x0: case 0x0:
parameterWrapper->set(sunModelParameters.useSunModel);
break;
case 0x1:
parameterWrapper->set(sunModelParameters.domega); parameterWrapper->set(sunModelParameters.domega);
break; break;
case 0x2: case 0x1:
parameterWrapper->set(sunModelParameters.omega_0); parameterWrapper->set(sunModelParameters.omega_0);
break; break;
case 0x3: case 0x2:
parameterWrapper->set(sunModelParameters.m_0); parameterWrapper->set(sunModelParameters.m_0);
break; break;
case 0x4: case 0x3:
parameterWrapper->set(sunModelParameters.dm); parameterWrapper->set(sunModelParameters.dm);
break; break;
case 0x5: case 0x4:
parameterWrapper->set(sunModelParameters.e); parameterWrapper->set(sunModelParameters.e);
break; break;
case 0x6: case 0x5:
parameterWrapper->set(sunModelParameters.e1); parameterWrapper->set(sunModelParameters.e1);
break; break;
case 0x7: case 0x6:
parameterWrapper->set(sunModelParameters.p1); parameterWrapper->set(sunModelParameters.p1);
break; break;
case 0x8: case 0x7:
parameterWrapper->set(sunModelParameters.p2); parameterWrapper->set(sunModelParameters.p2);
break; break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
break; break;
case (0xF): // KalmanFilterParameters case (0xE): // KalmanFilterParameters
switch (parameterId & 0xFF) { switch (parameterId & 0xFF) {
case 0x0: case 0x0:
parameterWrapper->set(kalmanFilterParameters.activateKalmanFilter);
break;
case 0x1:
parameterWrapper->set(kalmanFilterParameters.requestResetFlag);
break;
case 0x2:
parameterWrapper->set(
kalmanFilterParameters.maxToleratedTimeBetweenKalmanFilterExecutionSteps);
break;
case 0x3:
parameterWrapper->set(kalmanFilterParameters.processNoiseOmega);
break;
case 0x4:
parameterWrapper->set(kalmanFilterParameters.processNoiseQuaternion);
break;
case 0x5:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR); parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR);
break; break;
case 0x6: case 0x1:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS); parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS);
break; break;
case 0x7: case 0x2:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG); parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG);
break; break;
case 0x8: case 0x3:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR); parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR);
break; break;
case 0x9: case 0x4:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR); parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR);
break; break;
case 0xA: case 0x5:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR); parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR);
break; break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
break; break;
case (0x10): // MagnetorquesParameter case (0xF): // MagnetorquesParameter
switch (parameterId & 0xFF) { switch (parameterId & 0xFF) {
case 0x0: case 0x0:
parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix); parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix);
@ -519,7 +523,7 @@ AcsParameters::~AcsParameters() {}
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
break; break;
case (0x11): // DetumbleParameter case (0x10): // DetumbleParameter
switch (parameterId & 0xFF) { switch (parameterId & 0xFF) {
case 0x0: case 0x0:
parameterWrapper->set(detumbleParameter.detumblecounter); parameterWrapper->set(detumbleParameter.detumblecounter);
@ -530,6 +534,9 @@ AcsParameters::~AcsParameters() {}
case 0x2: case 0x2:
parameterWrapper->set(detumbleParameter.omegaDetumbleEnd); parameterWrapper->set(detumbleParameter.omegaDetumbleEnd);
break; break;
case 0x3:
parameterWrapper->set(detumbleParameter.gainD);
break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
@ -538,7 +545,7 @@ AcsParameters::~AcsParameters() {}
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
return returnvalue::OK; return returnvalue::OK;
} else { // } else {
return INVALID_DOMAIN_ID; // return INVALID_DOMAIN_ID;
} // }
}*/ }

21
mission/controller/acs/AcsParameters.h
View File

@ -12,15 +12,15 @@
typedef unsigned char uint8_t; typedef unsigned char uint8_t;
class AcsParameters /*: public HasParametersIF*/ { class AcsParameters : public HasParametersIF {
public: public:
AcsParameters(); AcsParameters();
virtual ~AcsParameters(); virtual ~AcsParameters();
/*
virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId,
ParameterWrapper *parameterWrapper, ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues,
const ParameterWrapper *newValues, uint16_t startAtIndex); uint16_t startAtIndex) override;
*/
struct OnBoardParams { struct OnBoardParams {
double sampleTime = 0.4; // [s] double sampleTime = 0.4; // [s]
} onBoardParams; } onBoardParams;
@ -776,8 +776,7 @@ class AcsParameters /*: public HasParametersIF*/ {
pow(3.0e-3 * sqrt(2), 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms pow(3.0e-3 * sqrt(2), 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
pow(4.3e-3 * sqrt(2), 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms pow(4.3e-3 * sqrt(2), 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
enum PreferAdis { NO = 0, YES = 1 }; uint8_t preferAdis = true;
uint8_t preferAdis = PreferAdis::YES;
} gyrHandlingParameters; } gyrHandlingParameters;
struct RwHandlingParameters { struct RwHandlingParameters {
@ -834,7 +833,7 @@ class AcsParameters /*: public HasParametersIF*/ {
double refDirection[3] = {-1, 0, 0}; // Antenna double refDirection[3] = {-1, 0, 0}; // Antenna
double refRotRate[3] = {0, 0, 0}; double refRotRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 1}; double quatRef[4] = {0, 0, 0, 1};
bool avoidBlindStr = true; uint8_t avoidBlindStr = true;
double blindAvoidStart = 1.5; double blindAvoidStart = 1.5;
double blindAvoidStop = 2.5; double blindAvoidStop = 2.5;
double blindRotRate = 1 * M_PI / 180; double blindRotRate = 1 * M_PI / 180;
@ -847,8 +846,8 @@ class AcsParameters /*: public HasParametersIF*/ {
double desatMomentumRef[3] = {0, 0, 0}; double desatMomentumRef[3] = {0, 0, 0};
double deSatGainFactor = 1000; double deSatGainFactor = 1000;
bool desatOn = true; uint8_t desatOn = true;
bool enableAntiStiction = true; uint8_t enableAntiStiction = true;
double omegaEarth = 0.000072921158553; double omegaEarth = 0.000072921158553;

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

@ -530,7 +530,7 @@ void SensorProcessing::processGyr(
// take ADIS measurements, if both avail // take ADIS measurements, if both avail
// if just one ADIS measurement avail, perform sensor fusion // if just one ADIS measurement avail, perform sensor fusion
double gyrVecTot[3] = {0.0, 0.0, 0.0}; double gyrVecTot[3] = {0.0, 0.0, 0.0};
if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == gyrParameters->PreferAdis::YES) { if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == true) {
double gyr02ValuesSum[3]; double gyr02ValuesSum[3];
VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3); VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3);
VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3); VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3);