This commit is contained in:
parent
497149a628
commit
40917ab988
186
mission/controller/acs/control/SafeCtrl.cpp
Normal file
186
mission/controller/acs/control/SafeCtrl.cpp
Normal file
@ -0,0 +1,186 @@
|
|||||||
|
/*
|
||||||
|
* SafeCtrl.cpp
|
||||||
|
*
|
||||||
|
* Created on: 19 Apr 2022
|
||||||
|
* Author: Robin Marquardt
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "SafeCtrl.h"
|
||||||
|
#include <fsfw/src/fsfw/globalfunctions/constants.h>
|
||||||
|
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
|
||||||
|
#include <acs/util/MathOperations.h>
|
||||||
|
#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||||
|
#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
SafeCtrl::SafeCtrl(AcsParameters *acsParameters_){
|
||||||
|
loadAcsParameters(acsParameters_);
|
||||||
|
MatrixOperations<double>::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3, 3);
|
||||||
|
}
|
||||||
|
|
||||||
|
SafeCtrl::~SafeCtrl(){
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
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 *outputMagMomB, bool *outputValid){
|
||||||
|
|
||||||
|
if ( !(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) ||
|
||||||
|
!(*rateMekfValid)) {
|
||||||
|
*outputValid = false;
|
||||||
|
return SAFECTRL_MEKF_INPUT_INVALID;
|
||||||
|
}
|
||||||
|
|
||||||
|
double kRate = 0, kAlign = 0;
|
||||||
|
kRate = safeModeControllerParameters->k_rate_mekf;
|
||||||
|
kAlign = safeModeControllerParameters->k_align_mekf;
|
||||||
|
|
||||||
|
// 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 crossSun[3] = {0, 0, 0};
|
||||||
|
|
||||||
|
VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
|
||||||
|
double normCrossSun = VectorOperations<double>::norm(crossSun, 3);
|
||||||
|
|
||||||
|
// calc angle alpha between sunDirRef and sunDIr
|
||||||
|
double alpha = 0, dotSun = 0;
|
||||||
|
dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
|
||||||
|
alpha = acos(dotSun);
|
||||||
|
|
||||||
|
// 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 scalarFac = 0;
|
||||||
|
scalarFac = kAlign * alpha / normCrossSun;
|
||||||
|
VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);
|
||||||
|
|
||||||
|
double rateSafeMode[3] = {0,0,0};
|
||||||
|
VectorOperations<double>::subtract(satRateMekf, satRatRef, rateSafeMode, 3);
|
||||||
|
VectorOperations<double>::mulScalar(rateSafeMode, -kRate, torqueRate, 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 RETURN_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){
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
|
||||||
|
// 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 */
|
||||||
|
|
||||||
|
double sinAngle = 0;
|
||||||
|
sinAngle = sin(acos(cos(cosAngleSunMag)));
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
// 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 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);
|
||||||
|
|
||||||
|
//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;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
63
mission/controller/acs/control/SafeCtrl.h
Normal file
63
mission/controller/acs/control/SafeCtrl.h
Normal file
@ -0,0 +1,63 @@
|
|||||||
|
/*
|
||||||
|
* safeCtrl.h
|
||||||
|
*
|
||||||
|
* Created on: 19 Apr 2022
|
||||||
|
* Author: rooob
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef SAFECTRL_H_
|
||||||
|
#define SAFECTRL_H_
|
||||||
|
|
||||||
|
#include <string.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <acs/SensorValues.h>
|
||||||
|
#include <acs/OutputValues.h>
|
||||||
|
#include <acs/AcsParameters.h>
|
||||||
|
#include "acs/config/classIds.h"
|
||||||
|
#include <fsfw/src/fsfw/returnvalues/HasReturnvaluesIF.h>
|
||||||
|
#include <time.h>
|
||||||
|
|
||||||
|
class SafeCtrl : public HasReturnvaluesIF {
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
SafeCtrl(AcsParameters *acsParameters_);
|
||||||
|
virtual ~SafeCtrl();
|
||||||
|
|
||||||
|
static const uint8_t INTERFACE_ID = CLASS_ID::SAFE;
|
||||||
|
static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
|
||||||
|
|
||||||
|
void loadAcsParameters(AcsParameters *acsParameters_);
|
||||||
|
|
||||||
|
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 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;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#endif /* ACS_CONTROL_SAFECTRL_H_ */
|
||||||
|
|
||||||
Loading…
Reference in New Issue
Block a user