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

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

@ -12,15 +12,15 @@
typedef unsigned char uint8_t;
class AcsParameters /*: public HasParametersIF*/ {
class AcsParameters : public HasParametersIF {
public:
AcsParameters();
virtual ~AcsParameters();
/*
virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId,
ParameterWrapper *parameterWrapper,
const ParameterWrapper *newValues, uint16_t startAtIndex);
*/
ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId,
ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues,
uint16_t startAtIndex) override;
struct OnBoardParams {
double sampleTime = 0.4; // [s]
} 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(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)};
enum PreferAdis { NO = 0, YES = 1 };
uint8_t preferAdis = PreferAdis::YES;
uint8_t preferAdis = true;
} gyrHandlingParameters;
struct RwHandlingParameters {
@ -834,7 +833,7 @@ class AcsParameters /*: public HasParametersIF*/ {
double refDirection[3] = {-1, 0, 0}; // Antenna
double refRotRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 1};
bool avoidBlindStr = true;
uint8_t avoidBlindStr = true;
double blindAvoidStart = 1.5;
double blindAvoidStop = 2.5;
double blindRotRate = 1 * M_PI / 180;
@ -847,8 +846,8 @@ class AcsParameters /*: public HasParametersIF*/ {
double desatMomentumRef[3] = {0, 0, 0};
double deSatGainFactor = 1000;
bool desatOn = true;
bool enableAntiStiction = true;
uint8_t desatOn = true;
uint8_t enableAntiStiction = true;
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
// if just one ADIS measurement avail, perform sensor fusion
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];
VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3);
VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3);