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

afmt
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
Details

Browse Source
This commit is contained in:
Robin Müller 2022-12-01 15:56:55 +01:00
parent a3a919437e
commit 828738ba0e
No known key found for this signature in database
GPG Key ID: 11D4952C8CCEF814
14 changed files with 217 additions and 240 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
dummies/GyroL3GD20Dummy.cpp
View File

@ -38,7 +38,6 @@ void GyroL3GD20Dummy::fillCommandAndReplyMap() {}
uint32_t GyroL3GD20Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 500; }
ReturnValue_t GyroL3GD20Dummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry<float>({1.2}, true));

1
mission/controller/acs/config/classIds.h
View File

@ -15,5 +15,4 @@ enum eiveclassIds: uint8_t {
};
}
#endif /* ACS_CONFIG_CLASSIDS_H_ */

5
mission/controller/acs/control/CMakeLists.txt
View File

@ -1,5 +1,2 @@
target_sources(
${LIB_EIVE_MISSION}
PRIVATE Detumble.cpp
PtgCtrl.cpp
target_sources(${LIB_EIVE_MISSION} PRIVATE Detumble.cpp PtgCtrl.cpp
SafeCtrl.cpp)

16
mission/controller/acs/control/Detumble.cpp
View File

@ -6,30 +6,24 @@
* Author: Robin Marquardt
*/
#include "Detumble.h"
#include "../util/MathOperations.h"
#include <math.h>
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/sign.h>
#include <math.h>
#include "../util/MathOperations.h"
Detumble::Detumble(AcsParameters *acsParameters_){
loadAcsParameters(acsParameters_);
}
Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }
Detumble::~Detumble(){
}
Detumble::~Detumble() {}
void Detumble::loadAcsParameters(AcsParameters *acsParameters_) {
detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters);
magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
}
ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,

18
mission/controller/acs/control/Detumble.h
View File

@ -8,17 +8,16 @@
#ifndef ACS_CONTROL_DETUMBLE_H_
#define ACS_CONTROL_DETUMBLE_H_
#include "../SensorValues.h"
#include "../AcsParameters.h"
#include "../config/classIds.h"
#include <string.h>
#include <stdio.h>
#include <time.h>
#include <fsfw/returnvalues/returnvalue.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include "../AcsParameters.h"
#include "../SensorValues.h"
#include "../config/classIds.h"
class Detumble {
public:
Detumble(AcsParameters *acsParameters_);
virtual ~Detumble();
@ -31,8 +30,8 @@ public:
*/
void loadAcsParameters(AcsParameters *acsParameters_);
ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid,
const double *magField, const bool magFieldValid, double *magMom);
ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid, const double *magField,
const bool magFieldValid, double *magMom);
ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom);
@ -42,4 +41,3 @@ public:
};
#endif /*ACS_CONTROL_DETUMBLE_H_*/

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

@ -5,10 +5,8 @@
* Author: Robin Marquardt
*/
#include "PtgCtrl.h"
#include "../util/MathOperations.h"
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
@ -16,13 +14,11 @@
#include <fsfw/globalfunctions/sign.h>
#include <math.h>
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){
loadAcsParameters(acsParameters_);
}
#include "../util/MathOperations.h"
PtgCtrl::~PtgCtrl(){
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }
}
PtgCtrl::~PtgCtrl() {}
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
@ -31,8 +27,8 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_){
rwMatrices = &(acsParameters_->rwMatrices);
}
void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){
void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,
const double *rwPseudoInv, double *torqueRws) {
//------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix
//------------------------------------------------------------------------------------------------
@ -49,8 +45,7 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
for (int i = 0; i < 3; i++) {
if (abs(qError[i]) < qErrorMin) {
qErrorLaw[i] = qErrorMin;
}
else {
} else {
qErrorLaw[i] = abs(qError[i]);
}
}
@ -65,7 +60,8 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
gainMatrixDiagonal[0][0] = gainVector[0];
gainMatrixDiagonal[1][1] = gainVector[1];
gainMatrixDiagonal[2][2] = gainVector[2];
MatrixOperations<double>::multiply( *gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), *gainMatrix, 3, 3, 3);
MatrixOperations<double>::multiply(*gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix),
*gainMatrix, 3, 3, 3);
// Inverse of gainMatrix
double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -74,7 +70,8 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, 3, 3);
MatrixOperations<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3,
3, 3);
MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
//------------------------------------------------------------------------------------------------
@ -86,11 +83,9 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
double pErrorSign[3] = {0, 0, 0};
for (int i = 0; i < 3; i++) {
if (abs(pError[i]) > 1) {
pErrorSign[i] = sign(pError[i]);
}
else {
} else {
pErrorSign[i] = pError[i];
}
}
@ -109,7 +104,6 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
double torque[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);
}
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,

20
mission/controller/acs/util/CholeskyDecomposition.h
View File

@ -13,11 +13,11 @@
template <typename T1, typename T2 = T1, typename T3 = T2>
class CholeskyDecomposition {
public:
static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension)
{
static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension) {
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
return cholsl(matrix, result, tempMatrix, dimension);
}
private:
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
static uint8_t choldc1(double *a, double *p, uint8_t n) {
@ -35,8 +35,7 @@ private:
return 1; /* error */
}
p[i] = sqrt(sum);
}
else {
} else {
a[j * n + i] = sum / p[i];
}
}
@ -46,12 +45,11 @@ private:
}
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
static uint8_t choldcsl(double * A, double * a, double * p, uint8_t n)
{
uint8_t i,j,k; double sum;
static uint8_t choldcsl(double *A, double *a, double *p, uint8_t n) {
uint8_t i, j, k;
double sum;
for (i = 0; i < n; i++)
for (j = 0; j < n; j++)
a[i*n+j] = A[i*n+j];
for (j = 0; j < n; j++) a[i * n + j] = A[i * n + j];
if (choldc1(a, p, n)) return 1;
for (i = 0; i < n; i++) {
a[i * n + i] = 1 / p[i];
@ -67,10 +65,8 @@ private:
return 0; /* success */
}
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
static uint8_t cholsl(double * A,double * a,double * p, uint8_t n)
{
static uint8_t cholsl(double *A, double *a, double *p, uint8_t n) {
uint8_t i, j, k;
if (choldcsl(A, a, p, n)) return 1;
for (i = 0; i < n; i++) {