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

First Version of ACS Controller #329

Merged
muellerr merged 106 commits from acs-ctrl-v1 into develop 2022-12-02 16:21:58 +01:00
Conversation 0 Commits 106 Files Changed 50 +100 -61
4 changed files with 100 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 46dd2b92e5 - Show all commits

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

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

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

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

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

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