# Conflicts: # mission/controller/AcsController.cpp # mission/controller/AcsController.h # mission/controller/acs/AcsParameters.h # mission/controller/acs/ActuatorCmd.h # mission/controller/acs/Guidance.cpp # mission/controller/acs/Guidance.h # mission/controller/acs/MultiplicativeKalmanFilter.cpp # mission/controller/acs/OutputValues.h # mission/controller/acs/SensorProcessing.cpp # mission/controller/acs/SensorProcessing.h # mission/controller/acs/control/Detumble.cpp # mission/controller/acs/control/Detumble.h # mission/controller/acs/control/PtgCtrl.cpp # mission/controller/acs/util/MathOperations.h
This commit is contained in:
CHANGELOG.mdCMakeLists.txtREADME.md
bsp_egse
bsp_hosted
CMakeLists.txtInitMission.hOBSWConfig.h.inObjectFactory.cpp
fsfwconfig
main.cppscheduling.cppscheduling.hbsp_linux_board
CMakeLists.txtInitMission.cppOBSWConfig.h.inObjectFactory.cppRPiSdCardManager.cppRPiSdCardManager.hdefinitions.h
bsp_q7s
CMakeLists.txtOBSWConfig.h.in
boardconfig
boardtest
callbacks
core
CMakeLists.txtCoreController.cppCoreController.hObjectFactory.cppObjectFactory.hscheduling.cppscheduling.h
em
fmObjectFactory.cppfs
CMakeLists.txtFilesystemHelper.cppFilesystemHelper.hSdCardManager.cppSdCardManager.hhelpers.cpphelpers.h
memory
obsw.cppbsp_te0720_1cfa
cmake/scripts
common/config
dummies
CMakeLists.txtCoreControllerDummy.cppGpsDummy.cppGyroL3GD20Dummy.cppImtqDummy.cppSaDeploymentDummy.cppSaDeploymentDummy.hSusDummy.cppSusDummy.hTemperatureSensorsDummy.cpphelpers.cpphelpers.h
fsfwgenerators
linux
CMakeLists.txtObjectFactory.cppObjectFactory.h
boardtest
devices
CMakeLists.txtGPSHyperionLinuxController.cppGPSHyperionLinuxController.hMax31865RtdLowlevelHandler.cppMax31865RtdLowlevelHandler.hScexDleParser.cppScexDleParser.hScexHelper.cppScexHelper.hScexUartReader.cppScexUartReader.h
devicedefinitions
ploc
CMakeLists.txtPlocMPSoCHandler.cppPlocMPSoCHandler.hPlocMPSoCHelper.cppPlocMPSoCHelper.hPlocMemoryDumper.hPlocSupervisorHandler.cppPlocSupervisorHandler.hPlocSupvHelper.cppPlocSupvUartMan.cppPlocSupvUartMan.h
startracker
fsfwconfig
devices
events
objects
pollingsequence
returnvalues
ipcore
AxiPtmeConfig.cppAxiPtmeConfig.hCMakeLists.txtPapbVcInterface.cppPapbVcInterface.hPdecConfig.cppPdecConfig.hPdecHandler.cppPdecHandler.hPtme.cppPtme.hPtmeConfig.cppPtmeConfig.hPtmeIF.hVcInterfaceIF.hpdec.h
scheduling.cppscheduling.hutility
misc/eclipse
mission
CMakeLists.txt
q7s-env-em.shq7s-env.shcfdp
controller
AcsController.cppAcsController.hCMakeLists.txtThermalController.cppThermalController.h
acs
AcsParameters.cppAcsParameters.hActuatorCmd.cppActuatorCmd.hCMakeLists.txtGuidance.cppGuidance.hIgrf13Model.cppIgrf13Model.hMultiplicativeKalmanFilter.cppMultiplicativeKalmanFilter.hNavigation.cppNavigation.hOutputValues.cppOutputValues.hSensorProcessing.cppSensorProcessing.hSensorValues.cppSensorValues.hSusConverter.cppSusConverter.h
config
control
util
controllerdefinitions
core
devices
BpxBatteryHandler.cppCMakeLists.txtGomspaceDeviceHandler.cppGyroADIS1650XHandler.cppImtqHandler.cppImtqHandler.hMax31865EiveHandler.cppMax31865PT1000Handler.cppP60DockHandler.hPCDUHandler.cppPayloadPcduHandler.cppPayloadPcduHandler.hRwHandler.cppScexDeviceHandler.cppScexDeviceHandler.hSolarArrayDeploymentHandler.cppSolarArrayDeploymentHandler.hSusHandler.cppSyrlinksHkHandler.cppTmp1075Handler.cppTmp1075Handler.h
devicedefinitions
CMakeLists.txtGPSDefinitions.hGomspaceDefinitions.hSCEXDefinitions.hScexDefinitions.cppScexDefinitions.hSpBase.hSyrlinksDefinitions.hgomspaceDefines.himtqHandlerDefinitions.hpayloadPcduDefinitions.hpowerDefinitions.h
torquer.cpptorquer.hmemory
system
AcsSubsystem.cppCMakeLists.txtComSubsystem.cppEiveSystem.cppPayloadSubsystem.cpp
fdir
objects
AcsBoardAssembly.cppAcsBoardAssembly.hAcsSubsystem.cppAcsSubsystem.hCMakeLists.txtCamSwitcher.cppCamSwitcher.hComSubsystem.cppComSubsystem.hDualLaneAssemblyBase.cppDualLaneAssemblyBase.hDualLanePowerStateMachine.cppDualLanePowerStateMachine.hEiveSystem.cppEiveSystem.hPayloadSubsystem.cppPayloadSubsystem.hPowerStateMachineBase.cppPowerStateMachineBase.hRwAssembly.cppRwAssembly.hSusAssembly.cppSusAssembly.hTcsBoardAssembly.cppTcsBoardAssembly.hdefinitions.h
tree
tmtc
CMakeLists.txtCcsdsIpCoreHandler.cppCcsdsIpCoreHandler.hCfdpTmFunnel.cppCfdpTmFunnel.hPusTmFunnel.cppPusTmFunnel.hTmFunnel.cppTmFunnel.hTmFunnelBase.cppTmFunnelBase.hTmFunnelHandler.cppTmFunnelHandler.hVirtualChannel.cppVirtualChannel.h
utility
scripts
thirdparty
tmtcunittest
@ -7,6 +7,7 @@
|
||||
|
||||
#include "SensorProcessing.h"
|
||||
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
#include <fsfw/globalfunctions/constants.h>
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
@ -20,27 +21,35 @@
|
||||
|
||||
using namespace Math;
|
||||
|
||||
SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) : savedMagFieldEst{0, 0, 0} {
|
||||
validMagField = false;
|
||||
validGcLatitude = false;
|
||||
}
|
||||
SensorProcessing::SensorProcessing(AcsParameters *acsParameters_)
|
||||
: savedMgmVecTot{0, 0, 0}, validMagField(false), validGcLatitude(false) {}
|
||||
|
||||
SensorProcessing::~SensorProcessing() {}
|
||||
|
||||
bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value,
|
||||
void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value,
|
||||
bool mgm1valid, const float *mgm2Value, bool mgm2valid,
|
||||
const float *mgm3Value, bool mgm3valid, const float *mgm4Value,
|
||||
bool mgm4valid, timeval timeOfMgmMeasurement,
|
||||
const AcsParameters::MgmHandlingParameters *mgmParameters,
|
||||
const double gpsLatitude, const double gpsLongitude,
|
||||
const double gpsAltitude, bool gpsValid, double *magFieldEst,
|
||||
bool *outputValid, double *magFieldModel,
|
||||
bool *magFieldModelValid, double *magneticFieldVectorDerivative,
|
||||
bool *magneticFieldVectorDerivativeValid) {
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed,
|
||||
const double gpsAltitude, bool gpsValid,
|
||||
acsctrl::MgmDataProcessed *mgmDataProcessed) {
|
||||
if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) {
|
||||
*outputValid = false;
|
||||
validMagField = false;
|
||||
return false;
|
||||
{
|
||||
PoolReadGuard pg(mgmDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->magIgrfModel.value, zeroVector, 3 * sizeof(double));
|
||||
mgmDataProcessed->setValidity(false, true);
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0},
|
||||
mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0},
|
||||
@ -49,9 +58,8 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
||||
mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
|
||||
float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0},
|
||||
mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0};
|
||||
float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
|
||||
|
||||
bool validUnit[5] = {false, false, false, false, false};
|
||||
uint8_t validCount = 0;
|
||||
if (mgm0valid) {
|
||||
VectorOperations<float>::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias,
|
||||
3);
|
||||
@ -59,8 +67,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
||||
mgm0ValueCalib, 3, 3, 1);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib,
|
||||
mgm0ValueBody, 3, 3, 1);
|
||||
validCount += 1;
|
||||
validUnit[0] = true;
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
sensorFusionNumerator[i] += mgm0ValueBody[i] / mgmParameters->mgm02variance[i];
|
||||
sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
|
||||
}
|
||||
}
|
||||
if (mgm1valid) {
|
||||
VectorOperations<float>::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias,
|
||||
@ -69,8 +79,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
||||
mgm1ValueCalib, 3, 3, 1);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib,
|
||||
mgm1ValueBody, 3, 3, 1);
|
||||
validCount += 1;
|
||||
validUnit[1] = true;
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
sensorFusionNumerator[i] += mgm1ValueBody[i] / mgmParameters->mgm13variance[i];
|
||||
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
|
||||
}
|
||||
}
|
||||
if (mgm2valid) {
|
||||
VectorOperations<float>::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias,
|
||||
@ -79,8 +91,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
||||
mgm2ValueCalib, 3, 3, 1);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib,
|
||||
mgm2ValueBody, 3, 3, 1);
|
||||
validCount += 1;
|
||||
validUnit[2] = true;
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
sensorFusionNumerator[i] += mgm2ValueBody[i] / mgmParameters->mgm02variance[i];
|
||||
sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
|
||||
}
|
||||
}
|
||||
if (mgm3valid) {
|
||||
VectorOperations<float>::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias,
|
||||
@ -89,81 +103,79 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
||||
mgm3ValueCalib, 3, 3, 1);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib,
|
||||
mgm3ValueBody, 3, 3, 1);
|
||||
validCount += 1;
|
||||
validUnit[3] = true;
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
sensorFusionNumerator[i] += mgm3ValueBody[i] / mgmParameters->mgm13variance[i];
|
||||
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
|
||||
}
|
||||
}
|
||||
if (mgm4valid) {
|
||||
VectorOperations<float>::subtract(mgm4Value, mgmParameters->mgm4hardIronOffset, mgm4ValueNoBias,
|
||||
3);
|
||||
float mgm4ValueNT[3];
|
||||
VectorOperations<float>::mulScalar(mgm4Value, 1e3, mgm4ValueNT, 3); // uT to nT
|
||||
VectorOperations<float>::subtract(mgm4ValueNT, mgmParameters->mgm4hardIronOffset,
|
||||
mgm4ValueNoBias, 3);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm4softIronInverse[0], mgm4ValueNoBias,
|
||||
mgm4ValueCalib, 3, 3, 1);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib,
|
||||
mgm4ValueBody, 3, 3, 1);
|
||||
validCount += 1;
|
||||
validUnit[4] = true;
|
||||
}
|
||||
|
||||
/* -------- MagFieldEst: Middle Value ------- */
|
||||
float mgmValues[3][5] = {
|
||||
{mgm0ValueBody[0], mgm1ValueBody[0], mgm2ValueBody[0], mgm3ValueBody[0], mgm4ValueBody[0]},
|
||||
{mgm0ValueBody[1], mgm1ValueBody[1], mgm2ValueBody[1], mgm3ValueBody[1], mgm4ValueBody[1]},
|
||||
{mgm0ValueBody[2], mgm1ValueBody[2], mgm2ValueBody[2], mgm3ValueBody[2], mgm4ValueBody[2]}};
|
||||
double mgmValidValues[3][validCount];
|
||||
uint8_t j = 0;
|
||||
for (uint8_t i = 0; i < validCount; i++) {
|
||||
if (validUnit[i]) {
|
||||
mgmValidValues[0][j] = mgmValues[0][i];
|
||||
mgmValidValues[1][j] = mgmValues[1][i];
|
||||
mgmValidValues[2][j] = mgmValues[2][i];
|
||||
j += 1;
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
sensorFusionNumerator[i] += mgm4ValueBody[i] / mgmParameters->mgm4variance[i];
|
||||
sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i];
|
||||
}
|
||||
}
|
||||
// Selection Sort
|
||||
double mgmValidValuesSort[3][validCount];
|
||||
MathOperations<double>::selectionSort(*mgmValidValues, *mgmValidValuesSort, 3, validCount);
|
||||
|
||||
uint8_t n = ceil(validCount / 2);
|
||||
magFieldEst[0] = mgmValidValuesSort[0][n];
|
||||
magFieldEst[1] = mgmValidValuesSort[1][n];
|
||||
magFieldEst[2] = mgmValidValuesSort[2][n];
|
||||
validMagField = true;
|
||||
|
||||
//-----------------------Mag Rate Computation ---------------------------------------------------
|
||||
double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
|
||||
double mgmVecTot[3] = {0.0, 0.0, 0.0};
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
magneticFieldVectorDerivative[i] = (magFieldEst[i] - savedMagFieldEst[i]) / timeDiff;
|
||||
savedMagFieldEst[i] = magFieldEst[i];
|
||||
mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
|
||||
}
|
||||
|
||||
*magneticFieldVectorDerivativeValid = true;
|
||||
if (timeOfSavedMagFieldEst.tv_sec == 0) {
|
||||
magneticFieldVectorDerivative[0] = 0;
|
||||
magneticFieldVectorDerivative[1] = 0;
|
||||
magneticFieldVectorDerivative[2] = 0;
|
||||
*magneticFieldVectorDerivativeValid = false;
|
||||
//-----------------------Mgm Rate Computation ---------------------------------------------------
|
||||
double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
|
||||
bool mgmVecTotDerivativeValid = false;
|
||||
double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
|
||||
if (timeOfSavedMagFieldEst.tv_sec != 0) {
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff;
|
||||
savedMgmVecTot[i] = mgmVecTot[i];
|
||||
}
|
||||
}
|
||||
|
||||
timeOfSavedMagFieldEst = timeOfMgmMeasurement;
|
||||
|
||||
*outputValid = true;
|
||||
|
||||
// ---------------- IGRF- 13 Implementation here ------------------------------------------------
|
||||
if (!gpsValid) {
|
||||
*magFieldModelValid = false;
|
||||
} else {
|
||||
double magIgrfModel[3] = {0.0, 0.0, 0.0};
|
||||
if (gpsValid) {
|
||||
// Should be existing class object which will be called and modified here.
|
||||
Igrf13Model igrf13;
|
||||
// So the line above should not be done here. Update: Can be done here as long updated coffs
|
||||
// stored in acsParameters ?
|
||||
igrf13.schmidtNormalization();
|
||||
igrf13.updateCoeffGH(timeOfMgmMeasurement);
|
||||
// maybe put a condition here, to only update after a full day, this
|
||||
// class function has around 700 steps to perform
|
||||
igrf13.magFieldComp(gpsLongitude, gpsLatitude, gpsAltitude, timeOfMgmMeasurement,
|
||||
magFieldModel);
|
||||
*magFieldModelValid = false;
|
||||
igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
|
||||
gpsAltitude, timeOfMgmMeasurement, magIgrfModel);
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(mgmDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueBody, 3 * sizeof(float));
|
||||
mgmDataProcessed->mgm0vec.setValid(mgm0valid);
|
||||
std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueBody, 3 * sizeof(float));
|
||||
mgmDataProcessed->mgm1vec.setValid(mgm1valid);
|
||||
std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueBody, 3 * sizeof(float));
|
||||
mgmDataProcessed->mgm2vec.setValid(mgm2valid);
|
||||
std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueBody, 3 * sizeof(float));
|
||||
mgmDataProcessed->mgm3vec.setValid(mgm3valid);
|
||||
std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueBody, 3 * sizeof(float));
|
||||
mgmDataProcessed->mgm4vec.setValid(mgm4valid);
|
||||
std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(double));
|
||||
mgmDataProcessed->mgmVecTot.setValid(true);
|
||||
std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, mgmVecTotDerivative,
|
||||
3 * sizeof(double));
|
||||
mgmDataProcessed->mgmVecTotDerivative.setValid(mgmVecTotDerivativeValid);
|
||||
std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double));
|
||||
mgmDataProcessed->magIgrfModel.setValid(gpsValid);
|
||||
mgmDataProcessed->setValidity(true, false);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void SensorProcessing::processSus(
|
||||
@ -174,9 +186,8 @@ void SensorProcessing::processSus(
|
||||
const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid,
|
||||
const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
|
||||
timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters,
|
||||
const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst,
|
||||
bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid,
|
||||
double *sunVectorDerivative, bool *sunVectorDerivativeValid) {
|
||||
const AcsParameters::SunModelParameters *sunModelParameters,
|
||||
acsctrl::SusDataProcessed *susDataProcessed) {
|
||||
if (sus0valid) {
|
||||
sus0valid = susConverter.checkSunSensorData(sus0Value);
|
||||
}
|
||||
@ -216,142 +227,176 @@ void SensorProcessing::processSus(
|
||||
|
||||
if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid &&
|
||||
!sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) {
|
||||
*sunDirEstValid = false;
|
||||
return;
|
||||
} else {
|
||||
// WARNING: NOT TRANSFORMED IN BODY FRAME YET
|
||||
// Transformation into Geomtry Frame
|
||||
float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0},
|
||||
sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0},
|
||||
sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0},
|
||||
sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0};
|
||||
|
||||
if (sus0valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus0orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha,
|
||||
susParameters->sus0coeffBeta),
|
||||
sus0VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus1valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus1orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha,
|
||||
susParameters->sus1coeffBeta),
|
||||
sus1VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus2valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus2orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha,
|
||||
susParameters->sus2coeffBeta),
|
||||
sus2VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus3valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus3orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha,
|
||||
susParameters->sus3coeffBeta),
|
||||
sus3VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus4valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus4orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha,
|
||||
susParameters->sus4coeffBeta),
|
||||
sus4VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus5valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus5orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha,
|
||||
susParameters->sus5coeffBeta),
|
||||
sus5VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus6valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus6orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha,
|
||||
susParameters->sus6coeffBeta),
|
||||
sus6VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus7valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus7orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha,
|
||||
susParameters->sus7coeffBeta),
|
||||
sus7VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus8valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus8orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha,
|
||||
susParameters->sus8coeffBeta),
|
||||
sus8VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus9valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus9orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha,
|
||||
susParameters->sus9coeffBeta),
|
||||
sus9VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus10valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus10orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha,
|
||||
susParameters->sus10coeffBeta),
|
||||
sus10VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus11valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus11orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha,
|
||||
susParameters->sus11coeffBeta),
|
||||
sus11VecBody, 3, 3, 1);
|
||||
}
|
||||
|
||||
/* ------ Mean Value: susDirEst ------ */
|
||||
bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid,
|
||||
sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid};
|
||||
float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0],
|
||||
sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0],
|
||||
sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]},
|
||||
{sus0VecBody[1], sus1VecBody[1], sus2VecBody[1], sus3VecBody[1],
|
||||
sus4VecBody[1], sus5VecBody[1], sus6VecBody[1], sus7VecBody[1],
|
||||
sus8VecBody[1], sus9VecBody[1], sus10VecBody[1], sus11VecBody[1]},
|
||||
{sus0VecBody[2], sus1VecBody[2], sus2VecBody[2], sus3VecBody[2],
|
||||
sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2],
|
||||
sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}};
|
||||
|
||||
double susMeanValue[3] = {0, 0, 0};
|
||||
for (uint8_t i = 0; i < 12; i++) {
|
||||
if (validIds[i]) {
|
||||
susMeanValue[0] += susVecBody[0][i];
|
||||
susMeanValue[1] += susVecBody[1][i];
|
||||
susMeanValue[2] += susVecBody[2][i];
|
||||
{
|
||||
PoolReadGuard pg(susDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus2vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus3vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus4vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus5vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus6vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus7vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus8vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus9vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus10vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus11vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->susVecTot.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sunIjkModel.value, zeroVector, 3 * sizeof(double));
|
||||
susDataProcessed->setValidity(false, true);
|
||||
}
|
||||
}
|
||||
VectorOperations<double>::normalize(susMeanValue, sunDirEst, 3);
|
||||
*sunDirEstValid = true;
|
||||
return;
|
||||
}
|
||||
// WARNING: NOT TRANSFORMED IN BODY FRAME YET
|
||||
// Transformation into Geomtry Frame
|
||||
float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0},
|
||||
sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0},
|
||||
sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0},
|
||||
sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0};
|
||||
|
||||
if (sus0valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus0orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha,
|
||||
susParameters->sus0coeffBeta),
|
||||
sus0VecBody, 3, 3, 1);
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(susDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus0vec.setValid(sus0valid);
|
||||
if (!sus0valid) {
|
||||
std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float));
|
||||
}
|
||||
}
|
||||
}
|
||||
if (sus1valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus1orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha,
|
||||
susParameters->sus1coeffBeta),
|
||||
sus1VecBody, 3, 3, 1);
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(susDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus1vec.setValid(sus1valid);
|
||||
if (!sus1valid) {
|
||||
std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float));
|
||||
}
|
||||
}
|
||||
}
|
||||
if (sus2valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus2orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha,
|
||||
susParameters->sus2coeffBeta),
|
||||
sus2VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus3valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus3orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha,
|
||||
susParameters->sus3coeffBeta),
|
||||
sus3VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus4valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus4orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha,
|
||||
susParameters->sus4coeffBeta),
|
||||
sus4VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus5valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus5orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha,
|
||||
susParameters->sus5coeffBeta),
|
||||
sus5VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus6valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus6orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha,
|
||||
susParameters->sus6coeffBeta),
|
||||
sus6VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus7valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus7orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha,
|
||||
susParameters->sus7coeffBeta),
|
||||
sus7VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus8valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus8orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha,
|
||||
susParameters->sus8coeffBeta),
|
||||
sus8VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus9valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus9orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha,
|
||||
susParameters->sus9coeffBeta),
|
||||
sus9VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus10valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus10orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha,
|
||||
susParameters->sus10coeffBeta),
|
||||
sus10VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus11valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus11orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha,
|
||||
susParameters->sus11coeffBeta),
|
||||
sus11VecBody, 3, 3, 1);
|
||||
}
|
||||
|
||||
/* ------ Mean Value: susDirEst ------ */
|
||||
bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid,
|
||||
sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid};
|
||||
float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0],
|
||||
sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0],
|
||||
sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]},
|
||||
{sus0VecBody[1], sus1VecBody[1], sus2VecBody[1], sus3VecBody[1],
|
||||
sus4VecBody[1], sus5VecBody[1], sus6VecBody[1], sus7VecBody[1],
|
||||
sus8VecBody[1], sus9VecBody[1], sus10VecBody[1], sus11VecBody[1]},
|
||||
{sus0VecBody[2], sus1VecBody[2], sus2VecBody[2], sus3VecBody[2],
|
||||
sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2],
|
||||
sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}};
|
||||
|
||||
double susMeanValue[3] = {0, 0, 0};
|
||||
for (uint8_t i = 0; i < 12; i++) {
|
||||
if (validIds[i]) {
|
||||
susMeanValue[0] += susVecBody[0][i];
|
||||
susMeanValue[1] += susVecBody[1][i];
|
||||
susMeanValue[2] += susVecBody[2][i];
|
||||
}
|
||||
}
|
||||
double susVecTot[3] = {0.0, 0.0, 0.0};
|
||||
VectorOperations<double>::normalize(susMeanValue, susVecTot, 3);
|
||||
|
||||
/* -------- Sun Derivatiative --------------------- */
|
||||
|
||||
double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
|
||||
bool susVecTotDerivativeValid = false;
|
||||
double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
sunVectorDerivative[i] = (sunDirEst[i] - savedSunVector[i]) / timeDiff;
|
||||
savedSunVector[i] = sunDirEst[i];
|
||||
if (timeOfSavedSusDirEst.tv_sec != 0) {
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff;
|
||||
savedSusVecTot[i] = susVecTot[i];
|
||||
}
|
||||
}
|
||||
|
||||
*sunVectorDerivativeValid = true;
|
||||
if (timeOfSavedSusDirEst.tv_sec == 0) {
|
||||
sunVectorDerivative[0] = 0;
|
||||
sunVectorDerivative[1] = 0;
|
||||
sunVectorDerivative[2] = 0;
|
||||
*sunVectorDerivativeValid = false;
|
||||
}
|
||||
|
||||
timeOfSavedSusDirEst = timeOfSusMeasurement;
|
||||
|
||||
/* -------- Sun Model Direction (IJK frame) ------- */
|
||||
@ -359,10 +404,11 @@ void SensorProcessing::processSus(
|
||||
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);
|
||||
|
||||
// Julean Centuries
|
||||
double JC2000 = JD2000 / 36525;
|
||||
double sunIjkModel[3] = {0.0, 0.0, 0.0};
|
||||
double JC2000 = JD2000 / 36525.;
|
||||
|
||||
double meanLongitude =
|
||||
(sunModelParameters->omega_0 + (sunModelParameters->domega) * JC2000) * PI / 180;
|
||||
sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * PI / 180.;
|
||||
double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.;
|
||||
|
||||
double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) +
|
||||
@ -370,11 +416,46 @@ void SensorProcessing::processSus(
|
||||
|
||||
double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000;
|
||||
|
||||
sunVectorInertial[0] = cos(eclipticLongitude);
|
||||
sunVectorInertial[1] = sin(eclipticLongitude) * cos(epsilon);
|
||||
sunVectorInertial[2] = sin(eclipticLongitude) * sin(epsilon);
|
||||
|
||||
*sunVectorInertialValid = true;
|
||||
sunIjkModel[0] = cos(eclipticLongitude);
|
||||
sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
|
||||
sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
|
||||
{
|
||||
PoolReadGuard pg(susDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus0vec.setValid(sus0valid);
|
||||
std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus1vec.setValid(sus1valid);
|
||||
std::memcpy(susDataProcessed->sus2vec.value, sus2VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus2vec.setValid(sus2valid);
|
||||
std::memcpy(susDataProcessed->sus3vec.value, sus3VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus3vec.setValid(sus3valid);
|
||||
std::memcpy(susDataProcessed->sus4vec.value, sus4VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus4vec.setValid(sus4valid);
|
||||
std::memcpy(susDataProcessed->sus5vec.value, sus5VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus5vec.setValid(sus5valid);
|
||||
std::memcpy(susDataProcessed->sus6vec.value, sus6VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus6vec.setValid(sus6valid);
|
||||
std::memcpy(susDataProcessed->sus7vec.value, sus7VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus7vec.setValid(sus7valid);
|
||||
std::memcpy(susDataProcessed->sus8vec.value, sus8VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus8vec.setValid(sus8valid);
|
||||
std::memcpy(susDataProcessed->sus9vec.value, sus9VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus9vec.setValid(sus9valid);
|
||||
std::memcpy(susDataProcessed->sus10vec.value, sus10VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus10vec.setValid(sus10valid);
|
||||
std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus11vec.setValid(sus11valid);
|
||||
std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(double));
|
||||
susDataProcessed->susVecTot.setValid(true);
|
||||
std::memcpy(susDataProcessed->susVecTotDerivative.value, susVecTotDerivative,
|
||||
3 * sizeof(double));
|
||||
susDataProcessed->susVecTotDerivative.setValid(susVecTotDerivativeValid);
|
||||
std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
|
||||
susDataProcessed->sunIjkModel.setValid(true);
|
||||
susDataProcessed->setValidity(true, false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SensorProcessing::processGyr(
|
||||
@ -385,87 +466,110 @@ void SensorProcessing::processGyr(
|
||||
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;
|
||||
acsctrl::GyrDataProcessed *gyrDataProcessed) {
|
||||
bool gyr0valid = (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid);
|
||||
bool gyr1valid = (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid);
|
||||
bool gyr2valid = (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid);
|
||||
bool gyr3valid = (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid);
|
||||
if (!gyr0valid && !gyr1valid && !gyr2valid && !gyr3valid) {
|
||||
{
|
||||
PoolReadGuard pg(gyrDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(gyrDataProcessed->gyr0vec.value, zeroVector, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyr2vec.value, zeroVector, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyr3vec.value, zeroVector, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyrVecTot.value, zeroVector, 3 * sizeof(double));
|
||||
gyrDataProcessed->setValidity(false, true);
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
// Transforming Values to the Body Frame (actually it is the geometry frame atm)
|
||||
double gyr0ValueBody[3] = {0, 0, 0}, gyr1ValueBody[3] = {0, 0, 0}, gyr2ValueBody[3] = {0, 0, 0},
|
||||
gyr3ValueBody[3] = {0, 0, 0};
|
||||
float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
|
||||
|
||||
bool validUnit[4] = {false, false, false, false};
|
||||
uint8_t validCount = 0;
|
||||
if (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid) {
|
||||
if (gyr0valid) {
|
||||
const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
|
||||
MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
|
||||
gyr0ValueBody, 3, 3, 1);
|
||||
validCount += 1;
|
||||
validUnit[0] = true;
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i];
|
||||
sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
|
||||
}
|
||||
}
|
||||
if (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid) {
|
||||
if (gyr1valid) {
|
||||
const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
|
||||
MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
|
||||
gyr1ValueBody, 3, 3, 1);
|
||||
validCount += 1;
|
||||
validUnit[1] = true;
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i];
|
||||
sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
|
||||
}
|
||||
}
|
||||
if (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid) {
|
||||
if (gyr2valid) {
|
||||
const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
|
||||
MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
|
||||
gyr2ValueBody, 3, 3, 1);
|
||||
validCount += 1;
|
||||
validUnit[2] = true;
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i];
|
||||
sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
|
||||
}
|
||||
}
|
||||
if (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid) {
|
||||
if (gyr3valid) {
|
||||
const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
|
||||
MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
|
||||
gyr3ValueBody, 3, 3, 1);
|
||||
validCount += 1;
|
||||
validUnit[3] = true;
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i];
|
||||
sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
|
||||
}
|
||||
}
|
||||
|
||||
/* -------- SatRateEst: Middle Value ------- */
|
||||
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]) {
|
||||
gyrValidValues[0][j] = gyrValues[0][i];
|
||||
gyrValidValues[1][j] = gyrValues[1][i];
|
||||
gyrValidValues[2][j] = gyrValues[2][i];
|
||||
j += 1;
|
||||
// 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) {
|
||||
double gyr02ValuesSum[3];
|
||||
VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3);
|
||||
VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3);
|
||||
} else {
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
gyrVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(gyrDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(gyrDataProcessed->gyr0vec.value, gyr0ValueBody, 3 * sizeof(double));
|
||||
gyrDataProcessed->gyr0vec.setValid(gyr0valid);
|
||||
std::memcpy(gyrDataProcessed->gyr1vec.value, gyr1ValueBody, 3 * sizeof(double));
|
||||
gyrDataProcessed->gyr1vec.setValid(gyr1valid);
|
||||
std::memcpy(gyrDataProcessed->gyr2vec.value, gyr2ValueBody, 3 * sizeof(double));
|
||||
gyrDataProcessed->gyr2vec.setValid(gyr2valid);
|
||||
std::memcpy(gyrDataProcessed->gyr3vec.value, gyr3ValueBody, 3 * sizeof(double));
|
||||
gyrDataProcessed->gyr3vec.setValid(gyr3valid);
|
||||
std::memcpy(gyrDataProcessed->gyrVecTot.value, gyrVecTot, 3 * sizeof(double));
|
||||
gyrDataProcessed->gyrVecTot.setValid(true);
|
||||
gyrDataProcessed->setValidity(true, false);
|
||||
}
|
||||
}
|
||||
// Selection Sort
|
||||
double gyrValidValuesSort[3][validCount];
|
||||
MathOperations<double>::selectionSort(*gyrValidValues, *gyrValidValuesSort, 3, validCount);
|
||||
|
||||
uint8_t n = ceil(validCount / 2);
|
||||
satRatEst[0] = gyrValidValuesSort[0][n];
|
||||
satRatEst[1] = gyrValidValuesSort[1][n];
|
||||
satRatEst[2] = gyrValidValuesSort[2][n];
|
||||
*satRateEstValid = true;
|
||||
}
|
||||
|
||||
void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude,
|
||||
const double gps0altitude, const uint32_t gps0UnixSeconds,
|
||||
const bool validGps, const AcsParameters::GpsParameters *gpsParameters,
|
||||
double *gcLatitude, double *gdLongitude, double *gpsVelocityE) {
|
||||
void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude,
|
||||
const bool validGps,
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed) {
|
||||
// name to convert not process
|
||||
double gdLongitude, gcLatitude;
|
||||
if (validGps) {
|
||||
// Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
|
||||
*gdLongitude = gps0longitude * PI / 180;
|
||||
double latitudeRad = gps0latitude * PI / 180;
|
||||
gdLongitude = gpsLongitude * PI / 180;
|
||||
double latitudeRad = gpsLatitude * PI / 180;
|
||||
double eccentricityWgs84 = 0.0818195;
|
||||
double factor = 1 - pow(eccentricityWgs84, 2);
|
||||
*gcLatitude = atan(factor * tan(latitudeRad));
|
||||
validGcLatitude = true;
|
||||
gcLatitude = atan(factor * tan(latitudeRad));
|
||||
|
||||
// Calculation of the satellite velocity in earth fixed frame
|
||||
double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0};
|
||||
@ -476,6 +580,18 @@ void SensorProcessing::processGps(const double gps0latitude, const double gps0lo
|
||||
double timeDiffGpsMeas = gps0UnixSeconds - timeOfSavedPosSatE;
|
||||
VectorOperations<double>::mulScalar(deltaDistance, 1/timeDiffGpsMeas, gpsVelocityE, 3);
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(gpsDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
gpsDataProcessed->gdLongitude.value = gdLongitude;
|
||||
gpsDataProcessed->gcLatitude.value = gcLatitude;
|
||||
gpsDataProcessed->setValidity(validGps, validGps);
|
||||
if (!validGps) {
|
||||
gpsDataProcessed->gdLongitude.value = 0.0;
|
||||
gpsDataProcessed->gcLatitude.value = 0.0;
|
||||
}
|
||||
}
|
||||
savedPosSatE[0] = posSatE[0];
|
||||
savedPosSatE[1] = posSatE[1];
|
||||
savedPosSatE[2] = posSatE[2];
|
||||
@ -489,30 +605,31 @@ void SensorProcessing::processGps(const double gps0latitude, const double gps0lo
|
||||
}
|
||||
|
||||
void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
|
||||
ACS::OutputValues *outputValues,
|
||||
acsctrl::MgmDataProcessed *mgmDataProcessed,
|
||||
acsctrl::SusDataProcessed *susDataProcessed,
|
||||
acsctrl::GyrDataProcessed *gyrDataProcessed,
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed,
|
||||
const AcsParameters *acsParameters) {
|
||||
sensorValues->update();
|
||||
processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
|
||||
sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value,
|
||||
sensorValues->gpsSet.isValid(), &acsParameters->gpsParameters,
|
||||
&outputValues->gcLatitude, &outputValues->gdLongitude,
|
||||
outputValues->gpsVelocity);
|
||||
(sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
|
||||
sensorValues->gpsSet.altitude.isValid()),
|
||||
gpsDataProcessed);
|
||||
|
||||
outputValues->mgmUpdated = processMgm(
|
||||
sensorValues->mgm0Lis3Set.fieldStrengths.value,
|
||||
sensorValues->mgm0Lis3Set.fieldStrengths.isValid(),
|
||||
sensorValues->mgm1Rm3100Set.fieldStrengths.value,
|
||||
sensorValues->mgm1Rm3100Set.fieldStrengths.isValid(),
|
||||
sensorValues->mgm2Lis3Set.fieldStrengths.value,
|
||||
sensorValues->mgm2Lis3Set.fieldStrengths.isValid(),
|
||||
sensorValues->mgm3Rm3100Set.fieldStrengths.value,
|
||||
sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), sensorValues->imtqMgmSet.mtmRawNt.value,
|
||||
sensorValues->imtqMgmSet.mtmRawNt.isValid(), now, &acsParameters->mgmHandlingParameters,
|
||||
outputValues->gcLatitude, outputValues->gdLongitude, sensorValues->gpsSet.altitude.value,
|
||||
sensorValues->gpsSet.isValid(), outputValues->magFieldEst, &outputValues->magFieldEstValid,
|
||||
outputValues->magFieldModel, &outputValues->magFieldModelValid,
|
||||
outputValues->magneticFieldVectorDerivative,
|
||||
&outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ?
|
||||
processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value,
|
||||
sensorValues->mgm0Lis3Set.fieldStrengths.isValid(),
|
||||
sensorValues->mgm1Rm3100Set.fieldStrengths.value,
|
||||
sensorValues->mgm1Rm3100Set.fieldStrengths.isValid(),
|
||||
sensorValues->mgm2Lis3Set.fieldStrengths.value,
|
||||
sensorValues->mgm2Lis3Set.fieldStrengths.isValid(),
|
||||
sensorValues->mgm3Rm3100Set.fieldStrengths.value,
|
||||
sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(),
|
||||
sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(),
|
||||
now, &acsParameters->mgmHandlingParameters, gpsDataProcessed,
|
||||
sensorValues->gpsSet.altitude.value,
|
||||
(sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
|
||||
sensorValues->gpsSet.altitude.isValid()),
|
||||
mgmDataProcessed);
|
||||
|
||||
processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(),
|
||||
sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(),
|
||||
@ -527,10 +644,7 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
|
||||
sensorValues->susSets[10].channels.value, sensorValues->susSets[10].channels.isValid(),
|
||||
sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(),
|
||||
now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters,
|
||||
outputValues->sunDirEst, &outputValues->sunDirEstValid, outputValues->sunDirModel,
|
||||
&outputValues->sunDirModelValid, outputValues->sunVectorDerivative,
|
||||
&outputValues->sunVectorDerivativeValid);
|
||||
// VALID outputs ?
|
||||
susDataProcessed);
|
||||
|
||||
processGyr(
|
||||
sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(),
|
||||
@ -545,6 +659,5 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
|
||||
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);
|
||||
&acsParameters->gyrHandlingParameters, gyrDataProcessed);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user