Merge branch 'eggert/acs' into marquardt/ptgCtrl
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
# 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:
@ -1,5 +1,2 @@
|
||||
target_sources(
|
||||
${LIB_EIVE_MISSION}
|
||||
PRIVATE Detumble.cpp
|
||||
PtgCtrl.cpp
|
||||
SafeCtrl.cpp)
|
||||
target_sources(${LIB_EIVE_MISSION} PRIVATE Detumble.cpp PtgCtrl.cpp
|
||||
SafeCtrl.cpp)
|
||||
|
||||
@ -6,33 +6,26 @@
|
||||
* 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_) {
|
||||
detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters);
|
||||
magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
|
||||
}
|
||||
|
||||
Detumble::~Detumble(){
|
||||
|
||||
}
|
||||
|
||||
void Detumble::loadAcsParameters(AcsParameters *acsParameters_){
|
||||
|
||||
detumbleParameter = &(acsParameters_->detumbleParameter);
|
||||
magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
|
||||
|
||||
}
|
||||
|
||||
|
||||
ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool *magRateValid,
|
||||
const double *magField, const bool *magFieldValid,
|
||||
double *magMom) {
|
||||
@ -47,18 +40,16 @@ ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool *magRateValid,
|
||||
|
||||
}
|
||||
|
||||
ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom) {
|
||||
ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid,
|
||||
double *magMom) {
|
||||
if (!magRateValid) {
|
||||
return DETUMBLE_NO_SENSORDATA;
|
||||
}
|
||||
|
||||
if (!magRateValid) {
|
||||
return DETUMBLE_NO_SENSORDATA;
|
||||
}
|
||||
|
||||
double dipolMax = magnetorquesParameter->DipolMax;
|
||||
for (int i = 0; i<3; i++) {
|
||||
|
||||
magMom[i] = - dipolMax * sign(magRate[i]);
|
||||
|
||||
}
|
||||
double dipolMax = magnetorquesParameter->DipolMax;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
magMom[i] = -dipolMax * sign(magRate[i]);
|
||||
}
|
||||
|
||||
return returnvalue::OK;
|
||||
|
||||
|
||||
@ -8,33 +8,32 @@
|
||||
#ifndef ACS_CONTROL_DETUMBLE_H_
|
||||
#define ACS_CONTROL_DETUMBLE_H_
|
||||
|
||||
#include "../SensorValues.h"
|
||||
#include "../OutputValues.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{
|
||||
class Detumble {
|
||||
public:
|
||||
Detumble(AcsParameters *acsParameters_);
|
||||
virtual ~Detumble();
|
||||
|
||||
public:
|
||||
Detumble(AcsParameters *acsParameters_);
|
||||
virtual ~Detumble();
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE;
|
||||
static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01);
|
||||
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE;
|
||||
static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01);
|
||||
/* @brief: Load AcsParameters für this class
|
||||
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
|
||||
*/
|
||||
void loadAcsParameters(AcsParameters *acsParameters_);
|
||||
|
||||
/* @brief: Load AcsParameters für this class
|
||||
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
|
||||
*/
|
||||
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);
|
||||
|
||||
ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom);
|
||||
|
||||
@ -48,4 +47,3 @@ private:
|
||||
};
|
||||
|
||||
#endif /*ACS_CONTROL_DETUMBLE_H_*/
|
||||
|
||||
|
||||
@ -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,77 +14,76 @@
|
||||
#include <fsfw/globalfunctions/sign.h>
|
||||
#include <math.h>
|
||||
|
||||
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_): torqueMemory {0, 0, 0, 0}, omegaMemory {0, 0, 0, 0} {
|
||||
#include "../util/MathOperations.h"
|
||||
|
||||
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){
|
||||
loadAcsParameters(acsParameters_);
|
||||
}
|
||||
|
||||
PtgCtrl::~PtgCtrl(){
|
||||
PtgCtrl::~PtgCtrl() {}
|
||||
|
||||
}
|
||||
|
||||
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_){
|
||||
pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
|
||||
inertiaEIVE = &(acsParameters_->inertiaEIVE);
|
||||
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
|
||||
rwMatrices =&(acsParameters_->rwMatrices);
|
||||
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
|
||||
pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
|
||||
inertiaEIVE = &(acsParameters_->inertiaEIVE);
|
||||
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
|
||||
rwMatrices = &(acsParameters_->rwMatrices);
|
||||
}
|
||||
|
||||
void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){
|
||||
//------------------------------------------------------------------------------------------------
|
||||
// Compute gain matrix K and P matrix
|
||||
//------------------------------------------------------------------------------------------------
|
||||
double om = pointingModeControllerParameters->om;
|
||||
double zeta = pointingModeControllerParameters->zeta;
|
||||
double qErrorMin = pointingModeControllerParameters->qiMin;
|
||||
double omMax = pointingModeControllerParameters->omMax;
|
||||
|
||||
//------------------------------------------------------------------------------------------------
|
||||
// Compute gain matrix K and P matrix
|
||||
//------------------------------------------------------------------------------------------------
|
||||
double om = pointingModeControllerParameters->om;
|
||||
double zeta = pointingModeControllerParameters->zeta;
|
||||
double qErrorMin = pointingModeControllerParameters->qiMin;
|
||||
double omMax = pointingModeControllerParameters->omMax;
|
||||
double cInt = 2 * om * zeta;
|
||||
double kInt = 2 * pow(om, 2);
|
||||
|
||||
double cInt = 2 * om * zeta;
|
||||
double kInt = 2 * pow(om,2);
|
||||
double qErrorLaw[3] = {0, 0, 0};
|
||||
|
||||
double qErrorLaw[3] = {0, 0, 0};
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (abs(qError[i]) < qErrorMin) {
|
||||
qErrorLaw[i] = qErrorMin;
|
||||
} else {
|
||||
qErrorLaw[i] = abs(qError[i]);
|
||||
}
|
||||
}
|
||||
double qErrorLawNorm = VectorOperations<double>::norm(qErrorLaw, 3);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (abs(qError[i]) < qErrorMin) {
|
||||
qErrorLaw[i] = qErrorMin;
|
||||
}
|
||||
else {
|
||||
qErrorLaw[i] = abs(qError[i]);
|
||||
}
|
||||
}
|
||||
double qErrorLawNorm = VectorOperations<double>::norm(qErrorLaw, 3);
|
||||
double gain1 = cInt * omMax / qErrorLawNorm;
|
||||
double gainVector[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(qErrorLaw, gain1, gainVector, 3);
|
||||
|
||||
double gain1 = cInt * omMax / qErrorLawNorm;
|
||||
double gainVector[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(qErrorLaw, gain1, gainVector, 3);
|
||||
double gainMatrixDiagonal[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double gainMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
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);
|
||||
|
||||
double gainMatrixDiagonal[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double gainMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
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);
|
||||
// Inverse of gainMatrix
|
||||
double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
gainMatrixInverse[0][0] = 1 / gainMatrix[0][0];
|
||||
gainMatrixInverse[1][1] = 1 / gainMatrix[1][1];
|
||||
gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
|
||||
|
||||
// Inverse of gainMatrix
|
||||
double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
gainMatrixInverse[0][0] = 1 / gainMatrix[0][0];
|
||||
gainMatrixInverse[1][1] = 1 / gainMatrix[1][1];
|
||||
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>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
|
||||
|
||||
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>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
|
||||
//------------------------------------------------------------------------------------------------
|
||||
// Torque Calculations for the reaction wheels
|
||||
//------------------------------------------------------------------------------------------------
|
||||
|
||||
//------------------------------------------------------------------------------------------------
|
||||
// Torque Calculations for the reaction wheels
|
||||
//------------------------------------------------------------------------------------------------
|
||||
|
||||
double pError[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*pMatrix, qError, pError, 3, 3, 1);
|
||||
double pErrorSign[3] = {0, 0, 0};
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
double pError[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*pMatrix, qError, pError, 3, 3, 1);
|
||||
double pErrorSign[3] = {0, 0, 0};
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (abs(pError[i]) > 1) {
|
||||
pErrorSign[i] = sign(pError[i]);
|
||||
}
|
||||
@ -113,7 +110,7 @@ void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *delt
|
||||
|
||||
}
|
||||
|
||||
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate,
|
||||
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
|
||||
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
|
||||
int32_t *speedRw3, double *mgtDpDes) {
|
||||
if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) {
|
||||
@ -124,7 +121,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, doubl
|
||||
}
|
||||
|
||||
// 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};
|
||||
VectorOperations<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4);
|
||||
MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4,
|
||||
@ -146,7 +143,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, doubl
|
||||
|
||||
void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
|
||||
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 rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60;
|
||||
// Conversion to [rad/s] for further calculations
|
||||
|
||||
@ -19,7 +19,6 @@
|
||||
#include <time.h>
|
||||
|
||||
#include "../AcsParameters.h"
|
||||
#include "../OutputValues.h"
|
||||
#include "../SensorValues.h"
|
||||
#include "../config/classIds.h"
|
||||
|
||||
@ -45,7 +44,7 @@ class PtgCtrl {
|
||||
void ptgLaw(const double mode, const double *qError, const double *deltaRate,
|
||||
const double *rwPseudoInv, double *torqueRws);
|
||||
|
||||
void ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate,
|
||||
void ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
|
||||
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3,
|
||||
double *mgtDpDes);
|
||||
|
||||
|
||||
@ -6,182 +6,173 @@
|
||||
*/
|
||||
|
||||
#include "SafeCtrl.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 <math.h>
|
||||
|
||||
#include "../util/MathOperations.h"
|
||||
|
||||
SafeCtrl::SafeCtrl(AcsParameters *acsParameters_){
|
||||
loadAcsParameters(acsParameters_);
|
||||
MatrixOperations<double>::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3, 3);
|
||||
SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) {
|
||||
loadAcsParameters(acsParameters_);
|
||||
MatrixOperations<double>::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3,
|
||||
3);
|
||||
}
|
||||
|
||||
SafeCtrl::~SafeCtrl(){
|
||||
SafeCtrl::~SafeCtrl() {}
|
||||
|
||||
void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
|
||||
safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters);
|
||||
inertiaEIVE = &(acsParameters_->inertiaEIVE);
|
||||
}
|
||||
|
||||
void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_){
|
||||
safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters);
|
||||
inertiaEIVE = &(acsParameters_->inertiaEIVE);
|
||||
}
|
||||
ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
|
||||
double *magFieldModel, bool magFieldModelValid,
|
||||
double *sunDirModel, bool sunDirModelValid, double *satRateMekf,
|
||||
bool rateMekfValid, double *sunDirRef, double *satRatRef,
|
||||
double *outputAngle, double *outputMagMomB, bool *outputValid) {
|
||||
if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) {
|
||||
*outputValid = false;
|
||||
return SAFECTRL_MEKF_INPUT_INVALID;
|
||||
}
|
||||
|
||||
ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
|
||||
double *magFieldModel, bool *magFieldModelValid,
|
||||
double *sunDirModel, bool *sunDirModelValid,
|
||||
double *satRateMekf, bool *rateMekfValid,
|
||||
double *sunDirRef, double *satRatRef,
|
||||
double *outputMagMomB, bool *outputValid){
|
||||
double kRate = 0, kAlign = 0;
|
||||
kRate = safeModeControllerParameters->k_rate_mekf;
|
||||
kAlign = safeModeControllerParameters->k_align_mekf;
|
||||
|
||||
if ( !(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) ||
|
||||
!(*rateMekfValid)) {
|
||||
*outputValid = false;
|
||||
return SAFECTRL_MEKF_INPUT_INVALID;
|
||||
}
|
||||
// Calc sunDirB ,magFieldB with mekf output and model
|
||||
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::dcmFromQuat(quatBJ, *dcmBJ);
|
||||
double sunDirB[3] = {0, 0, 0}, magFieldB[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1);
|
||||
|
||||
double kRate = 0, kAlign = 0;
|
||||
kRate = safeModeControllerParameters->k_rate_mekf;
|
||||
kAlign = safeModeControllerParameters->k_align_mekf;
|
||||
double crossSun[3] = {0, 0, 0};
|
||||
|
||||
// Calc sunDirB ,magFieldB with mekf output and model
|
||||
double dcmBJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
|
||||
MathOperations<double>::dcmFromQuat(quatBJ, *dcmBJ);
|
||||
double sunDirB[3] = {0,0,0}, magFieldB[3] = {0,0,0};
|
||||
MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1);
|
||||
VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
|
||||
double normCrossSun = VectorOperations<double>::norm(crossSun, 3);
|
||||
|
||||
double crossSun[3] = {0, 0, 0};
|
||||
// calc angle alpha between sunDirRef and sunDIr
|
||||
double alpha = 0, dotSun = 0;
|
||||
dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
|
||||
alpha = acos(dotSun);
|
||||
|
||||
VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
|
||||
double normCrossSun = VectorOperations<double>::norm(crossSun, 3);
|
||||
// Law Torque calculations
|
||||
double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, torqueRate[3] = {0, 0, 0},
|
||||
torqueAll[3] = {0, 0, 0};
|
||||
|
||||
// calc angle alpha between sunDirRef and sunDIr
|
||||
double alpha = 0, dotSun = 0;
|
||||
dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
|
||||
alpha = acos(dotSun);
|
||||
double scalarFac = 0;
|
||||
scalarFac = kAlign * alpha / normCrossSun;
|
||||
VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);
|
||||
|
||||
// Law Torque calculations
|
||||
double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0},
|
||||
torqueRate[3] = {0, 0, 0}, torqueAll[3] = {0, 0, 0};
|
||||
double rateSafeMode[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(satRateMekf, satRatRef, rateSafeMode, 3);
|
||||
VectorOperations<double>::mulScalar(rateSafeMode, -kRate, torqueRate, 3);
|
||||
|
||||
double scalarFac = 0;
|
||||
scalarFac = kAlign * alpha / normCrossSun;
|
||||
VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);
|
||||
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
|
||||
// Adding factor of inertia for axes
|
||||
MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);
|
||||
|
||||
double rateSafeMode[3] = {0,0,0};
|
||||
VectorOperations<double>::subtract(satRateMekf, satRatRef, rateSafeMode, 3);
|
||||
VectorOperations<double>::mulScalar(rateSafeMode, -kRate, torqueRate, 3);
|
||||
// MagMom B (orthogonal torque)
|
||||
double torqueMgt[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt);
|
||||
double normMag = VectorOperations<double>::norm(magFieldB, 3);
|
||||
VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3);
|
||||
|
||||
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
|
||||
// Adding factor of inertia for axes
|
||||
MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);
|
||||
|
||||
// MagMom B (orthogonal torque)
|
||||
double torqueMgt[3] = {0,0,0};
|
||||
VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt);
|
||||
double normMag = VectorOperations<double>::norm(magFieldB, 3);
|
||||
VectorOperations<double>::mulScalar(torqueMgt, 1/pow(normMag,2), outputMagMomB, 3);
|
||||
*outputValid = true;
|
||||
|
||||
return returnvalue::OK;
|
||||
*outputAngle = alpha;
|
||||
*outputValid = true;
|
||||
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
// Will be the version in worst case scenario in event of no working MEKF (nor RMUs)
|
||||
void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid,
|
||||
double *sunRateB, bool *sunRateBValid,
|
||||
double *magFieldB, bool *magFieldBValid,
|
||||
double *magRateB, bool *magRateBValid,
|
||||
double *sunDirRef, double *satRateRef,
|
||||
double *outputMagMomB, bool *outputValid){
|
||||
void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
|
||||
bool sunRateBValid, double *magFieldB, bool magFieldBValid,
|
||||
double *magRateB, bool magRateBValid, double *sunDirRef,
|
||||
double *satRateRef, double *outputAngle, double *outputMagMomB,
|
||||
bool *outputValid) {
|
||||
// Check for invalid Inputs
|
||||
if (!susDirBValid || !magFieldBValid || !magRateBValid) {
|
||||
*outputValid = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// Check for invalid Inputs
|
||||
if ( !susDirBValid || !magFieldBValid || !magRateBValid) {
|
||||
*outputValid = false;
|
||||
return;
|
||||
}
|
||||
// normalize sunDir and magDir
|
||||
double magDirB[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(magFieldB, magDirB, 3);
|
||||
VectorOperations<double>::normalize(susDirB, susDirB, 3);
|
||||
|
||||
// normalize sunDir and magDir
|
||||
double magDirB[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(magFieldB, magDirB, 3);
|
||||
VectorOperations<double>::normalize(susDirB, susDirB, 3);
|
||||
// Cosinus angle between sunDir and magDir
|
||||
double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);
|
||||
|
||||
// Cosinus angle between sunDir and magDir
|
||||
double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);
|
||||
// Rate parallel to sun direction and magnetic field direction
|
||||
double rateParaSun = 0, rateParaMag = 0;
|
||||
double dotSunRateMag = 0, dotmagRateSun = 0, rateFactor = 0;
|
||||
dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
|
||||
dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
|
||||
rateFactor = 1 - pow(cosAngleSunMag, 2);
|
||||
rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
|
||||
rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;
|
||||
|
||||
// Rate parallel to sun direction and magnetic field direction
|
||||
double rateParaSun = 0, rateParaMag = 0;
|
||||
double dotSunRateMag = 0, dotmagRateSun = 0,
|
||||
rateFactor = 0;
|
||||
dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
|
||||
dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
|
||||
rateFactor = 1 - pow(cosAngleSunMag,2);
|
||||
rateParaSun = ( dotmagRateSun + cosAngleSunMag * dotSunRateMag ) / rateFactor;
|
||||
rateParaMag = ( dotSunRateMag + cosAngleSunMag * dotmagRateSun ) / rateFactor;
|
||||
// Full rate or estimate
|
||||
double estSatRate[3] = {0, 0, 0};
|
||||
double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(susDirB, rateParaSun, estSatRateSun, 3);
|
||||
VectorOperations<double>::add(sunRateB, estSatRateSun, estSatRateSun, 3);
|
||||
VectorOperations<double>::mulScalar(magDirB, rateParaMag, estSatRateMag, 3);
|
||||
VectorOperations<double>::add(magRateB, estSatRateMag, estSatRateMag, 3);
|
||||
VectorOperations<double>::add(estSatRateSun, estSatRateMag, estSatRate, 3);
|
||||
VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3);
|
||||
|
||||
// Full rate or estimate
|
||||
double estSatRate[3] = {0, 0, 0};
|
||||
double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(susDirB, rateParaSun, estSatRateSun, 3);
|
||||
VectorOperations<double>::add(sunRateB, estSatRateSun, estSatRateSun, 3);
|
||||
VectorOperations<double>::mulScalar(magDirB, rateParaMag, estSatRateMag, 3);
|
||||
VectorOperations<double>::add(magRateB, estSatRateMag, estSatRateMag, 3);
|
||||
VectorOperations<double>::add(estSatRateSun, estSatRateMag, estSatRate, 3);
|
||||
VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3);
|
||||
/* Only valid if angle between sun direction and magnetic field direction
|
||||
is sufficiently large */
|
||||
|
||||
/* Only valid if angle between sun direction and magnetic field direction
|
||||
is sufficiently large */
|
||||
double sinAngle = 0;
|
||||
sinAngle = sin(acos(cos(cosAngleSunMag)));
|
||||
|
||||
double sinAngle = 0;
|
||||
sinAngle = sin(acos(cos(cosAngleSunMag)));
|
||||
if (!(sinAngle > sin(safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) {
|
||||
return;
|
||||
}
|
||||
|
||||
if ( !(sinAngle > sin( safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) {
|
||||
return;
|
||||
}
|
||||
// Rate for Torque Calculation
|
||||
double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */
|
||||
VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
|
||||
|
||||
// Rate for Torque Calculation
|
||||
double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */
|
||||
VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
|
||||
// Torque Align calculation
|
||||
double kRateNoMekf = 0, kAlignNoMekf = 0;
|
||||
kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
|
||||
kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
|
||||
|
||||
// Torque Align calculation
|
||||
double kRateNoMekf = 0, kAlignNoMekf = 0;
|
||||
kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
|
||||
kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
|
||||
double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
|
||||
double crossSusSunRef[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(sunDirRef, susDirB, crossSusSunRef);
|
||||
double sinAngleAlignErr = VectorOperations<double>::norm(crossSusSunRef, 3);
|
||||
|
||||
double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
|
||||
double crossSusSunRef[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(sunDirRef, susDirB, crossSusSunRef);
|
||||
double sinAngleAlignErr = VectorOperations<double>::norm(crossSusSunRef, 3);
|
||||
double torqueAlign[3] = {0, 0, 0};
|
||||
double angleAlignErr = acos(cosAngleAlignErr);
|
||||
double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr;
|
||||
VectorOperations<double>::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3);
|
||||
|
||||
double torqueAlign[3] = {0, 0, 0};
|
||||
double angleAlignErr = acos(cosAngleAlignErr);
|
||||
double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr;
|
||||
VectorOperations<double>::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3);
|
||||
// Torque Rate Calculations
|
||||
double torqueRate[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3);
|
||||
|
||||
//Torque Rate Calculations
|
||||
double torqueRate[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3);
|
||||
// Final torque
|
||||
double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0};
|
||||
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3);
|
||||
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3,
|
||||
1);
|
||||
|
||||
//Final torque
|
||||
double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0};
|
||||
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3);
|
||||
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3, 1);
|
||||
|
||||
//Magnetic moment
|
||||
double magMomB[3] = {0, 0, 0};
|
||||
double crossMagFieldTorque[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(magFieldB, torqueB, crossMagFieldTorque);
|
||||
double magMomFactor = pow( VectorOperations<double>::norm(magFieldB, 3), 2 );
|
||||
VectorOperations<double>::mulScalar(crossMagFieldTorque, 1/magMomFactor, magMomB, 3);
|
||||
|
||||
outputMagMomB[0] = magMomB[0];
|
||||
outputMagMomB[1] = magMomB[1];
|
||||
outputMagMomB[2] = magMomB[2];
|
||||
|
||||
*outputValid = true;
|
||||
// Magnetic moment
|
||||
double magMomB[3] = {0, 0, 0};
|
||||
double crossMagFieldTorque[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(magFieldB, torqueB, crossMagFieldTorque);
|
||||
double magMomFactor = pow(VectorOperations<double>::norm(magFieldB, 3), 2);
|
||||
VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);
|
||||
|
||||
std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));
|
||||
*outputAngle = angleAlignErr;
|
||||
*outputValid = true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -8,57 +8,45 @@
|
||||
#ifndef SAFECTRL_H_
|
||||
#define SAFECTRL_H_
|
||||
|
||||
#include "../SensorValues.h"
|
||||
#include "../OutputValues.h"
|
||||
#include "../AcsParameters.h"
|
||||
#include "../config/classIds.h"
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
#include <fsfw/returnvalues/HasReturnvaluesIF.h>
|
||||
|
||||
#include "../AcsParameters.h"
|
||||
#include "../SensorValues.h"
|
||||
#include "../config/classIds.h"
|
||||
|
||||
class SafeCtrl{
|
||||
class SafeCtrl {
|
||||
public:
|
||||
SafeCtrl(AcsParameters *acsParameters_);
|
||||
virtual ~SafeCtrl();
|
||||
|
||||
public:
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::SAFE;
|
||||
static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
|
||||
|
||||
SafeCtrl(AcsParameters *acsParameters_);
|
||||
virtual ~SafeCtrl();
|
||||
void loadAcsParameters(AcsParameters *acsParameters_);
|
||||
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::SAFE;
|
||||
static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
|
||||
ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel,
|
||||
bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid,
|
||||
double *satRateMekf, bool rateMekfValid, double *sunDirRef,
|
||||
double *satRatRef, // From Guidance (!)
|
||||
double *outputAngle, double *outputMagMomB, bool *outputValid);
|
||||
|
||||
void loadAcsParameters(AcsParameters *acsParameters_);
|
||||
void safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
|
||||
bool sunRateBValid, double *magFieldB, bool magFieldBValid, double *magRateB,
|
||||
bool magRateBValid, double *sunDirRef, double *satRateRef, double *outputAngle,
|
||||
double *outputMagMomB, bool *outputValid);
|
||||
|
||||
ReturnValue_t safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
|
||||
double *magFieldModel, bool *magFieldModelValid,
|
||||
double *sunDirModel, bool *sunDirModelValid,
|
||||
double *satRateMekf, bool *rateMekfValid,
|
||||
double *sunDirRef, double *satRatRef, // From Guidance (!)
|
||||
double *outputMagMomB, bool *outputValid);
|
||||
void idleSunPointing(); // with reaction wheels
|
||||
|
||||
void safeNoMekf(timeval now, double *susDirB, bool *susDirBValid,
|
||||
double *sunRateB, bool *sunRateBValid,
|
||||
double *magFieldB, bool *magFieldBValid,
|
||||
double *magRateB, bool *magRateBValid,
|
||||
double *sunDirRef, double *satRateRef,
|
||||
double *outputMagMomB, bool *outputValid);
|
||||
|
||||
void idleSunPointing(); // with reaction wheels
|
||||
|
||||
protected:
|
||||
|
||||
private:
|
||||
AcsParameters::SafeModeControllerParameters* safeModeControllerParameters;
|
||||
AcsParameters::InertiaEIVE* inertiaEIVE;
|
||||
double gainMatrixInertia[3][3];
|
||||
|
||||
double magFieldBState[3];
|
||||
timeval magFieldBStateTime;
|
||||
protected:
|
||||
private:
|
||||
AcsParameters::SafeModeControllerParameters *safeModeControllerParameters;
|
||||
AcsParameters::InertiaEIVE *inertiaEIVE;
|
||||
double gainMatrixInertia[3][3];
|
||||
|
||||
double magFieldBState[3];
|
||||
timeval magFieldBStateTime;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif /* ACS_CONTROL_SAFECTRL_H_ */
|
||||
|
||||
|
||||
Reference in New Issue
Block a user