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

2
dummies/ImtqDummy.cpp
View File

@ -39,7 +39,7 @@ uint32_t ImtqDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur
ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry<float>({0.0,0.0,0.0}));
localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry<float>({0.0, 0.0, 0.0}));
localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry<float>({0.12, 0.76, -0.45}, true));
localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>({0}));

2
dummies/SusDummy.cpp
View File

@ -37,7 +37,7 @@ ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool &localDataPo
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry<float>({0}, 1, true));
localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC,
new PoolEntry<uint16_t>({0, 0, 0, 0, 0, 0},true));
new PoolEntry<uint16_t>({0, 0, 0, 0, 0, 0}, true));
return returnvalue::OK;
}

2
mission/controller/AcsController.h
View File

@ -70,7 +70,7 @@ class AcsController : public ExtendedControllerBase {
void announceMode(bool recursive);
/* ACS Datasets */
IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
// MGMs
acsctrl::MgmDataRaw mgmDataRaw;
PoolEntry<float> mgm0VecRaw = PoolEntry<float>(3);

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

@ -5,7 +5,7 @@
#include <fsfw/returnvalues/FwClassIds.h>
namespace CLASS_ID {
enum eiveclassIds: uint8_t {
enum eiveclassIds : uint8_t {
EIVE_CLASS_ID_START = COMMON_CLASS_ID_END,
KALMAN,
SAFE,
@ -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)

18
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(){
}
void Detumble::loadAcsParameters(AcsParameters *acsParameters_){
Detumble::~Detumble() {}
void Detumble::loadAcsParameters(AcsParameters *acsParameters_) {
detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters);
magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
}
ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,

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

@ -8,18 +8,17 @@
#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:
class Detumble {
public:
Detumble(AcsParameters *acsParameters_);
virtual ~Detumble();
@ -31,15 +30,14 @@ 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);
private:
AcsParameters::DetumbleCtrlParameters* detumbleCtrlParameters;
AcsParameters::MagnetorquesParameter* magnetorquesParameter;
AcsParameters::DetumbleCtrlParameters *detumbleCtrlParameters;
AcsParameters::MagnetorquesParameter *magnetorquesParameter;
};
#endif /*ACS_CONTROL_DETUMBLE_H_*/

42
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,23 +14,21 @@
#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_){
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
rwMatrices =&(acsParameters_->rwMatrices);
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
//------------------------------------------------------------------------------------------------
@ -42,15 +38,14 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
double omMax = pointingModeControllerParameters->omMax;
double cInt = 2 * om * zeta;
double kInt = 2 * pow(om,2);
double kInt = 2 * pow(om, 2);
double qErrorLaw[3] = {0, 0, 0};
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,30 +83,27 @@ 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];
}
}
// Torque for quaternion error
// Torque for quaternion error
double torqueQuat[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueQuat, -1, torqueQuat, 3);
// Torque for rate error
// Torque for rate error
double torqueRate[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);
// Final commanded Torque for every reaction wheel
// Final commanded Torque for every reaction wheel
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,

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

@ -8,36 +8,35 @@
#ifndef CHOLESKYDECOMPOSITION_H_
#define CHOLESKYDECOMPOSITION_H_
#include <math.h>
//typedef unsigned int uint8_t;
// typedef unsigned int uint8_t;
template<typename T1, typename T2=T1, typename T3=T2>
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)
{
public:
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:
private:
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
static uint8_t choldc1(double * a, double * p, uint8_t n) {
int8_t i,j,k;
static uint8_t choldc1(double *a, double *p, uint8_t n) {
int8_t i, j, k;
double sum;
for (i = 0; i < n; i++) {
for (j = i; j < n; j++) {
sum = a[i*n+j];
sum = a[i * n + j];
for (k = i - 1; k >= 0; k--) {
sum -= a[i*n+k] * a[j*n+k];
sum -= a[i * n + k] * a[j * n + k];
}
if (i == j) {
if (sum <= 0) {
return 1; /* error */
}
p[i] = sqrt(sum);
}
else {
a[j*n+i] = sum / p[i];
} else {
a[j * n + i] = sum / p[i];
}
}
}
@ -46,52 +45,49 @@ 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];
a[i * n + i] = 1 / p[i];
for (j = i + 1; j < n; j++) {
sum = 0;
for (k = i; k < j; k++) {
sum -= a[j*n+k] * a[k*n+i];
sum -= a[j * n + k] * a[k * n + i];
}
a[j*n+i] = sum / p[j];
a[j * n + i] = sum / p[j];
}
}
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)
{
uint8_t i,j,k;
if (choldcsl(A,a,p,n)) return 1;
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++) {
for (j = i + 1; j < n; j++) {
a[i*n+j] = 0.0;
a[i * n + j] = 0.0;
}
}
for (i = 0; i < n; i++) {
a[i*n+i] *= a[i*n+i];
a[i * n + i] *= a[i * n + i];
for (k = i + 1; k < n; k++) {
a[i*n+i] += a[k*n+i] * a[k*n+i];
a[i * n + i] += a[k * n + i] * a[k * n + i];
}
for (j = i + 1; j < n; j++) {
for (k = j; k < n; k++) {
a[i*n+j] += a[k*n+i] * a[k*n+j];
a[i * n + j] += a[k * n + i] * a[k * n + j];
}
}
}
for (i = 0; i < n; i++) {
for (j = 0; j < i; j++) {
a[i*n+j] = a[j*n+i];
a[i * n + j] = a[j * n + i];
}
}