eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 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
Show Stats 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);
}

14
mission/controller/acs/SensorProcessing.h
View File

@ -52,10 +52,16 @@ class SensorProcessing {
bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid,
double *sunVectorDerivative, bool *sunVectorDerivativeValid);
void processRmu(const double rmu0Value[], bool rmu0valid, // processRmu
const double rmu1Value[], bool rmu1valid, const double rmu2Value[],
bool rmu2valid, timeval timeOfrmuMeasurement,
const AcsParameters::RmuHandlingParameters *rmuParameters, double *satRatEst,
void 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);
void processStr();

31
mission/controller/acs/SensorValues.cpp
View File

@ -42,20 +42,25 @@ ReturnValue_t SensorValues::updateSus() {
return result;
}
ReturnValue_t SensorValues::updateGyr() {
ReturnValue_t result;
PoolReadGuard pgGyr0(&gyr0AdisSet), pgGyr1(&gyr1L3gSet), pgGyr2(&gyr2AdisSet),
pgGyr3(&gyr3L3gSet);
result = (pgGyr0.getReadResult() || pgGyr1.getReadResult() || pgGyr2.getReadResult() ||
pgGyr3.getReadResult());
return result;
}
ReturnValue_t SensorValues::update() {
updateSus();
updateMgm();
// lp_var_t<float> quaternion(objects::STAR_TRACKER, PoolIds::CALI_QW, nullptr,
// pool_rwm_t::VAR_READ);
// ReturnValue_t result = quaternion.read();
//
// if (result != RETURN_OK) {
// return RETURN_FAILED;
// }
// quatJB[3] = static_cast<double>(quaternion.value);
// quatJBValid = quaternion.isValid();
ReturnValue_t mgmUpdate = updateMgm();
ReturnValue_t susUpdate = updateSus();
ReturnValue_t gyrUpdate = updateGyr();
if ((mgmUpdate && susUpdate && gyrUpdate) == returnvalue::FAILED) {
return returnvalue::FAILED;
};
return returnvalue::OK;
}
} // namespace ACS
// namespace ACS

20
mission/controller/acs/SensorValues.h
View File

@ -1,16 +1,18 @@
#ifndef SENSORVALUES_H_
#define SENSORVALUES_H_
#include <commonObjects.h>
#include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
#include "mission/devices/devicedefinitions/GyroL3GD20Definitions.h"
#include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h"
namespace ACS {
class SensorValues{
class SensorValues {
public:
SensorValues();
virtual ~SensorValues();
@ -18,6 +20,7 @@ class SensorValues{
ReturnValue_t update();
ReturnValue_t updateMgm();
ReturnValue_t updateSus();
ReturnValue_t updateGyr();
MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set =
MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER);
@ -44,13 +47,10 @@ class SensorValues{
SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB),
};
double rmu0[3];
double rmu1[3];
double rmu2[3];
bool rmu0Valid;
bool rmu1Valid;
bool rmu2Valid;
AdisGyroPrimaryDataset gyr0AdisSet = AdisGyroPrimaryDataset(objects::GYRO_0_ADIS_HANDLER);
GyroPrimaryDataset gyr1L3gSet = GyroPrimaryDataset(objects::GYRO_1_L3G_HANDLER);
AdisGyroPrimaryDataset gyr2AdisSet = AdisGyroPrimaryDataset(objects::GYRO_2_ADIS_HANDLER);
GyroPrimaryDataset gyr3L3gSet = GyroPrimaryDataset(objects::GYRO_3_L3G_HANDLER);
double quatJB[4]; // output star tracker. quaternion or dcm ? refrence to which KOS?
bool quatJBValid;