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

renamed RMU related stuff to GYR. added GYR processing

Browse Source
This commit is contained in:
Marius Eggert
2022-10-11 15:01:32 +02:00
parent b2a715ef6a
commit 46dd2b92e5
4 changed files with 100 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -353,62 +353,79 @@ void SensorProcessing::processSus(
*sunVectorInertialValid = true;
}
void SensorProcessing::processRmu(const double rmu0Value[], bool rmu0valid,
const double rmu1Value[], bool rmu1valid,
const double rmu2Value[], bool rmu2valid,
timeval timeOfrmuMeasurement,
const AcsParameters::RmuHandlingParameters *rmuParameters,
double *satRatEst, bool *satRateEstValid) {
if (!rmu0valid && !rmu1valid && !rmu2valid) {
void SensorProcessing::processGyr(
const double gyr0axXvalue, bool gyr0axXvalid, const double gyr0axYvalue, bool gyr0axYvalid,
const double gyr0axZvalue, bool gyr0axZvalid, const double gyr1axXvalue, bool gyr1axXvalid,
const double gyr1axYvalue, bool gyr1axYvalid, const double gyr1axZvalue, bool gyr1axZvalid,
const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue, bool gyr2axYvalid,
const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid,
const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
timeval timeOfGyrMeasurement, const AcsParameters::GyrHandlingParameters *gyrParameters,
double *satRatEst, bool *satRateEstValid) {
if (!gyr0axXvalid && !gyr0axYvalid && !gyr0axZvalid && !gyr1axXvalid && !gyr1axYvalid &&
!gyr1axZvalid && !gyr2axXvalid && !gyr2axYvalid && !gyr2axZvalid && !gyr3axXvalid &&
!gyr3axYvalid && !gyr3axZvalid) {
*satRateEstValid = false;
return;
}
// Transforming Values to the Body Frame (actually it is the geometry frame atm)
double rmu0ValueBody[3] = {0, 0, 0}, rmu1ValueBody[3] = {0, 0, 0}, rmu2ValueBody[3] = {0, 0, 0};
double gyr0ValueBody[3] = {0, 0, 0}, gyr1ValueBody[3] = {0, 0, 0}, gyr2ValueBody[3] = {0, 0, 0},
gyr3ValueBody[3] = {0, 0, 0};
bool validUnit[3] = {false, false, false};
bool validUnit[4] = {false, false, false, false};
uint8_t validCount = 0;
if (rmu0valid) {
MatrixOperations<double>::multiply(rmuParameters->rmu0orientationMatrix[0], rmu0Value,
rmu0ValueBody, 3, 3, 1);
if (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid) {
const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
gyr0ValueBody, 3, 3, 1);
validCount += 1;
validUnit[0] = true;
}
if (rmu1valid) {
MatrixOperations<double>::multiply(rmuParameters->rmu1orientationMatrix[0], rmu1Value,
rmu1ValueBody, 3, 3, 1);
if (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid) {
const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
gyr1ValueBody, 3, 3, 1);
validCount += 1;
validUnit[1] = true;
}
if (rmu2valid) {
MatrixOperations<double>::multiply(rmuParameters->rmu2orientationMatrix[0], rmu2Value,
rmu2ValueBody, 3, 3, 1);
if (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid) {
const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
gyr2ValueBody, 3, 3, 1);
validCount += 1;
validUnit[2] = true;
}
if (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid) {
const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
gyr3ValueBody, 3, 3, 1);
validCount += 1;
validUnit[3] = true;
}
/* -------- SatRateEst: Middle Value ------- */
double rmuValues[3][3] = {{rmu0ValueBody[0], rmu1ValueBody[0], rmu2ValueBody[0]},
{rmu0ValueBody[1], rmu1ValueBody[1], rmu2ValueBody[1]},
{rmu0ValueBody[2], rmu1ValueBody[2], rmu2ValueBody[2]}};
double rmuValidValues[3][validCount];
double gyrValues[3][4] = {
{gyr0ValueBody[0], gyr1ValueBody[0], gyr2ValueBody[0], gyr3ValueBody[0]},
{gyr0ValueBody[1], gyr1ValueBody[1], gyr2ValueBody[1], gyr3ValueBody[1]},
{gyr0ValueBody[2], gyr1ValueBody[2], gyr2ValueBody[2], gyr3ValueBody[2]}};
double gyrValidValues[3][validCount];
uint8_t j = 0;
for (uint8_t i = 0; i < validCount; i++) {
if (validUnit[i]) {
rmuValidValues[0][j] = rmuValues[0][i];
rmuValidValues[1][j] = rmuValues[1][i];
rmuValidValues[2][j] = rmuValues[2][i];
gyrValidValues[0][j] = gyrValues[0][i];
gyrValidValues[1][j] = gyrValues[1][i];
gyrValidValues[2][j] = gyrValues[2][i];
j += 1;
}
}
// Selection Sort
double rmuValidValuesSort[3][validCount];
MathOperations<double>::selectionSort(*rmuValidValues, *rmuValidValuesSort, 3, validCount);
double gyrValidValuesSort[3][validCount];
MathOperations<double>::selectionSort(*gyrValidValues, *gyrValidValuesSort, 3, validCount);
uint8_t n = ceil(validCount / 2);
satRatEst[0] = rmuValidValuesSort[0][n];
satRatEst[1] = rmuValidValuesSort[1][n];
satRatEst[2] = rmuValidValuesSort[2][n];
satRatEst[0] = gyrValidValuesSort[0][n];
satRatEst[1] = gyrValidValuesSort[1][n];
satRatEst[2] = gyrValidValuesSort[2][n];
*satRateEstValid = true;
}
@ -467,8 +484,19 @@ void SensorProcessing::process(acsctrl::SusDataRaw *susData, timeval now,
&outputValues->sunVectorDerivativeValid);
// VALID outputs ?
// processRmu(sensorValues->rmu0, sensorValues->rmu0Valid, sensorValues->rmu1,
// sensorValues->rmu1Valid, sensorValues->rmu2, sensorValues->rmu2Valid, now,
// &acsParameters->rmuHandlingParameters, outputValues->satRateEst,
// &outputValues->satRateEstValid);
processGyr(
sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(),
sensorValues->gyr0AdisSet.angVelocY.value, sensorValues->gyr0AdisSet.angVelocY.isValid(),
sensorValues->gyr0AdisSet.angVelocZ.value, sensorValues->gyr0AdisSet.angVelocZ.isValid(),
sensorValues->gyr1L3gSet.angVelocX.value, sensorValues->gyr1L3gSet.angVelocX.isValid(),
sensorValues->gyr1L3gSet.angVelocY.value, sensorValues->gyr1L3gSet.angVelocY.isValid(),
sensorValues->gyr1L3gSet.angVelocZ.value, sensorValues->gyr1L3gSet.angVelocZ.isValid(),
sensorValues->gyr2AdisSet.angVelocX.value, sensorValues->gyr2AdisSet.angVelocX.isValid(),
sensorValues->gyr2AdisSet.angVelocY.value, sensorValues->gyr2AdisSet.angVelocY.isValid(),
sensorValues->gyr2AdisSet.angVelocZ.value, sensorValues->gyr2AdisSet.angVelocZ.isValid(),
sensorValues->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.isValid(),
sensorValues->gyr3L3gSet.angVelocY.value, sensorValues->gyr3L3gSet.angVelocY.isValid(),
sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now,
&acsParameters->gyrHandlingParameters, outputValues->satRateEst,
&outputValues->satRateEstValid);
}