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

fixed int32_t to double warnings. reformats

Browse Source
This commit is contained in:
Marius Eggert 2022-10-24 10:29:57 +02:00
parent 0d3509b991
commit 8b23fd3dd2
4 changed files with 1186 additions and 1248 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2274
mission/controller/acs/MultiplicativeKalmanFilter.cpp
View File

File diff suppressed because it is too large Load Diff

148
mission/controller/acs/MultiplicativeKalmanFilter.h
View File

@ -7,101 +7,95 @@
* @brief: This class handles the calculation of an estimated quaternion and the gyro bias by * @brief: This class handles the calculation of an estimated quaternion and the gyro bias by
* means of the spacecraft attitude sensors * means of the spacecraft attitude sensors
* *
* @note: A description of the used algorithms can be found in the bachelor thesis of Robin Marquardt * @note: A description of the used algorithms can be found in the bachelor thesis of Robin
* Marquardt
* https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=500811 * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=500811
*/ */
#ifndef MULTIPLICATIVEKALMANFILTER_H_ #ifndef MULTIPLICATIVEKALMANFILTER_H_
#define MULTIPLICATIVEKALMANFILTER_H_ #define MULTIPLICATIVEKALMANFILTER_H_
#include <stdint.h> //uint8_t
#include <time.h> /*purpose, timeval ?*/
#include "config/classIds.h" #include "config/classIds.h"
#include <stdint.h> //uint8_t
#include <time.h> /*purpose, timeval ?*/
//#include <_timeval.h> //#include <_timeval.h>
#include "AcsParameters.h" #include "AcsParameters.h"
class MultiplicativeKalmanFilter{ class MultiplicativeKalmanFilter {
public: public:
/* @brief: Constructor /* @brief: Constructor
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/ */
MultiplicativeKalmanFilter(AcsParameters *acsParameters_); MultiplicativeKalmanFilter(AcsParameters *acsParameters_);
virtual ~MultiplicativeKalmanFilter(); virtual ~MultiplicativeKalmanFilter();
void reset(); // NOT YET DEFINED - should only reset all mekf variables void reset(); // NOT YET DEFINED - should only reset all mekf variables
/* @brief: init() - This function initializes the Kalman Filter and will provide the first quaternion through /* @brief: init() - This function initializes the Kalman Filter and will provide the first
* the QUEST algorithm * quaternion through the QUEST algorithm
* @param: magneticField_ magnetic field vector in the body frame * @param: magneticField_ magnetic field vector in the body frame
* sunDir_ sun direction vector in the body frame * sunDir_ sun direction vector in the body frame
* sunDirJ sun direction vector in the ECI frame * sunDirJ sun direction vector in the ECI frame
* magFieldJ magnetic field vector in the ECI frame * magFieldJ magnetic field vector in the ECI frame
*/ */
void init(const double *magneticField_, const bool *validMagField_, void init(const double *magneticField_, const bool *validMagField_, const double *sunDir_,
const double *sunDir_, const bool *validSS, const bool *validSS, const double *sunDirJ, const bool *validSSModel,
const double *sunDirJ, const bool *validSSModel, const double *magFieldJ, const bool *validMagModel);
const double *magFieldJ,const bool *validMagModel);
/* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter for the current step /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
* after the initalization * for the current step after the initalization
* @param: quaternionSTR Star Tracker Quaternion between from body to ECI frame * @param: quaternionSTR Star Tracker Quaternion between from body to ECI frame
* rateRMUs_ Estimated satellite rotation rate from the Rate Measurement Units [rad/s] * rateGYRs_ Estimated satellite rotation rate from the
* magneticField_ magnetic field vector in the body frame * Gyroscopes [rad/s] magneticField_ magnetic field vector in the body frame sunDir_
* sunDir_ sun direction vector in the body frame * sun direction vector in the body frame sunDirJ sun direction vector in the ECI
* sunDirJ sun direction vector in the ECI frame * frame magFieldJ magnetic field vector in the ECI frame
* magFieldJ magnetic field vector in the ECI frame * outputQuat Stores the calculated quaternion
* outputQuat Stores the calculated quaternion * outputSatRate Stores the adjusted satellite rate
* outputSatRate Stores the adjusted satellite rate * @return ReturnValue_t Feedback of this class, KALMAN_NO_GYR_MEAS if no satellite rate from
* @return ReturnValue_t Feedback of this class, KALMAN_NO_RMU_MEAS if no satellite rate from the sensors was provided, * the sensors was provided, KALMAN_NO_MODEL if no sunDirJ or magFieldJ was given from the model
* KALMAN_NO_MODEL if no sunDirJ or magFieldJ was given from the model calculations, * calculations, KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible,
* KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible, * RETURN_OK else
* RETURN_OK else */
*/ ReturnValue_t mekfEst(const double *quaternionSTR, const bool *validSTR_, const double *rateGYRs_,
ReturnValue_t mekfEst( const bool *validGYRs_, const double *magneticField_,
const double *quaternionSTR, const bool *validSTR_, const bool *validMagField_, const double *sunDir_, const bool *validSS,
const double *rateRMUs_, const bool *validRMUs_, const double *sunDirJ, const bool *validSSModel, const double *magFieldJ,
const double *magneticField_, const bool *validMagField_, const bool *validMagModel, double *outputQuat, double *outputSatRate);
const double *sunDir_, const bool *validSS,
const double *sunDirJ, const bool *validSSModel,
const double *magFieldJ,const bool *validMagModel,
double *outputQuat, double *outputSatRate);
// Declaration of Events (like init) and memberships
// static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND
// (/config/returnvalues/classIDs.h) static const Event RESET =
// MAKE_EVENT(1,severity::INFO);//typedef uint32_t Event (Event.h), should be
// resetting Mekf
static const uint8_t INTERFACE_ID = CLASS_ID::KALMAN;
static const ReturnValue_t KALMAN_NO_GYR_MEAS = MAKE_RETURN_CODE(0x01);
static const ReturnValue_t KALMAN_NO_MODEL = MAKE_RETURN_CODE(0x02);
static const ReturnValue_t KALMAN_INVERSION_FAILED = MAKE_RETURN_CODE(0x03);
// Declaration of Events (like init) and memberships private:
//static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND (/config/returnvalues/classIDs.h) /*Parameters*/
//static const Event RESET = MAKE_EVENT(1,severity::INFO);//typedef uint32_t Event (Event.h), should be AcsParameters::InertiaEIVE *inertiaEIVE;
// resetting Mekf AcsParameters::KalmanFilterParameters *kalmanFilterParameters;
static const uint8_t INTERFACE_ID = CLASS_ID::KALMAN; double quaternion_STR_SB[4];
static const ReturnValue_t KALMAN_NO_RMU_MEAS = MAKE_RETURN_CODE(0x01); bool validInit;
static const ReturnValue_t KALMAN_NO_MODEL = MAKE_RETURN_CODE(0x02); double sampleTime = 0.1;
static const ReturnValue_t KALMAN_INVERSION_FAILED = MAKE_RETURN_CODE(0x03);
private: /*States*/
/*Parameters*/ double initialQuaternion[4]; /*after reset?QUEST*/
AcsParameters::InertiaEIVE* inertiaEIVE; double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/
AcsParameters::KalmanFilterParameters* kalmanFilterParameters; double propagatedQuaternion[4]; /*Filter Quaternion for next step*/
double quaternion_STR_SB[4]; bool validMekf;
bool validInit; uint8_t sensorsAvail;
double sampleTime = 0.1;
/*Outputs*/
/*States*/ double quatBJ[4]; /* Output Quaternion */
double initialQuaternion[4]; /*after reset?QUEST*/ double biasGYR[3]; /*Between measured and estimated sat Rate*/
double initialCovarianceMatrix[6][6];/*after reset?QUEST*/ /*Parameter INIT*/
double propagatedQuaternion[4]; /*Filter Quaternion for next step*/ // double alpha, gamma, beta;
bool validMekf; /*Functions*/
uint8_t sensorsAvail; void loadAcsParameters(AcsParameters *acsParameters_);
/*Outputs*/
double quatBJ[4]; /* Output Quaternion */
double biasRMU[3]; /*Between measured and estimated sat Rate*/
/*Parameter INIT*/
//double alpha, gamma, beta;
/*Functions*/
void loadAcsParameters(AcsParameters *acsParameters_);
}; };
#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */ #endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */

8
mission/controller/acs/Navigation.cpp
View File

@ -36,8 +36,7 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *out
&outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid, &outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid,
outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->quatMekfBJ, outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->quatMekfBJ,
outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ?? outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ??
} } else {
else {
multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid, multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid,
outputValues->sunDirEst, &outputValues->sunDirEstValid, outputValues->sunDirEst, &outputValues->sunDirEstValid,
outputValues->sunDirModel, &outputValues->sunDirModelValid, outputValues->sunDirModel, &outputValues->sunDirModelValid,
@ -45,7 +44,8 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *out
kalmanInit = true; kalmanInit = true;
*mekfValid = 0; *mekfValid = 0;
// Maybe we need feedback from kalmanfilter to identify if there was a problem with the init // Maybe we need feedback from kalmanfilter to identify if there was a problem with the
//of kalman filter where does this class know from that kalman filter was not initialized ? // init of kalman filter where does this class know from that kalman filter was not
// initialized ?
} }
} }

4
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -123,7 +123,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, doubl
} }
// calculating momentum of satellite and momentum of reaction wheels // calculating momentum of satellite and momentum of reaction wheels
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4); VectorOperations<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4);
MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4,
@ -145,7 +145,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, doubl
void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double wheelMomentum[4] = {0, 0, 0, 0}; double wheelMomentum[4] = {0, 0, 0, 0};
double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60;
// Conversion to [rad/s] for further calculations // Conversion to [rad/s] for further calculations