First Version of ACS Controller #329
@ -8,6 +8,7 @@ AcsController::AcsController(object_id_t objectId)
|
||||
navigation(&acsParameters),
|
||||
actuatorCmd(&acsParameters),
|
||||
guidance(&acsParameters),
|
||||
safeCtrl(&acsParameters),
|
||||
detumble(&acsParameters),
|
||||
ptgCtrl(&acsParameters),
|
||||
detumbleCounter{0},
|
||||
@ -35,7 +36,7 @@ void AcsController::performControlOperation() {
|
||||
if (mode != MODE_OFF) {
|
||||
switch (submode) {
|
||||
case SUBMODE_SAFE:
|
||||
// performSafe();
|
||||
performSafe();
|
||||
break;
|
||||
|
||||
case SUBMODE_DETUMBLE:
|
||||
@ -72,7 +73,66 @@ void AcsController::performControlOperation() {
|
||||
// DEBUG END
|
||||
}
|
||||
|
||||
void AcsController::performSafe() {}
|
||||
void AcsController::performSafe() {
|
||||
// Concept: SAFE MODE WITH MEKF
|
||||
// -do the sensor processing, maybe is does make more sense do call this class function in
|
||||
// another place since we have to do it for every mode regardless of safe or not
|
||||
|
||||
ACS::SensorValues sensorValues;
|
||||
ACS::OutputValues outputValues;
|
||||
|
||||
timeval now; // We need to give the actual time here
|
||||
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
|
||||
ReturnValue_t validMekf;
|
||||
navigation.useMekf(&sensorValues, &outputValues, &validMekf); // DOES THIS WORK WITH VALID?
|
||||
// Give desired satellite rate and sun direction to align
|
||||
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
|
||||
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
|
||||
// IF MEKF is working
|
||||
double magMomMtq[3] = {0, 0, 0};
|
||||
bool magMomMtqValid = false;
|
||||
if (validMekf == returnvalue::OK) {
|
||||
safeCtrl.safeMekf(now, (outputValues.quatMekfBJ), &(outputValues.quatMekfBJValid),
|
||||
(outputValues.magFieldModel), &(outputValues.magFieldModelValid),
|
||||
(outputValues.sunDirModel), &(outputValues.sunDirModelValid),
|
||||
(outputValues.satRateMekf), &(outputValues.satRateMekfValid), sunTargetDir,
|
||||
satRateSafe, magMomMtq, &magMomMtqValid);
|
||||
} else {
|
||||
safeCtrl.safeNoMekf(now, outputValues.sunDirEst, &outputValues.sunDirEstValid,
|
||||
outputValues.sunVectorDerivative, &(outputValues.sunVectorDerivativeValid),
|
||||
outputValues.magFieldEst, &(outputValues.magFieldEstValid),
|
||||
outputValues.magneticFieldVectorDerivative,
|
||||
&(outputValues.magneticFieldVectorDerivativeValid), sunTargetDir,
|
||||
satRateSafe, magMomMtq, &magMomMtqValid);
|
||||
}
|
||||
|
||||
double dipolCmdUnits[3] = {0, 0, 0};
|
||||
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
|
||||
|
||||
// Detumble check and switch
|
||||
if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) >
|
||||
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
||||
detumbleCounter++;
|
||||
}
|
||||
|
||||
else if (outputValues.satRateEstValid &&
|
||||
VectorOperations<double>::norm(outputValues.satRateEst, 3) >
|
||||
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
||||
detumbleCounter++;
|
||||
|
||||
}
|
||||
|
||||
else {
|
||||
detumbleCounter = 0;
|
||||
}
|
||||
|
||||
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
|
||||
submode = SUBMODE_DETUMBLE;
|
||||
detumbleCounter = 0;
|
||||
}
|
||||
|
||||
// commanding.magnetorquesDipol();
|
||||
}
|
||||
|
||||
void AcsController::performDetumble() {
|
||||
ACS::SensorValues sensorValues;
|
||||
@ -293,6 +353,8 @@ void AcsController::copySusData() {
|
||||
PoolReadGuard pg(&susSets[9]);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susData.sus9.value, susSets[9].channels.value, 6 * sizeof(uint16_t));
|
||||
sif::debug << susData.sus9.isValid() << std::endl;
|
||||
sif::debug << susSets[9].channels.isValid() << std::endl;
|
||||
}
|
||||
}
|
||||
{
|
||||
|
||||
@ -11,6 +11,7 @@
|
||||
#include "acs/SensorProcessing.h"
|
||||
#include "acs/control/Detumble.h"
|
||||
#include "acs/control/PtgCtrl.h"
|
||||
#include "acs/control/SafeCtrl.h"
|
||||
#include "controllerdefinitions/AcsCtrlDefinitions.h"
|
||||
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
|
||||
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
|
||||
@ -29,7 +30,6 @@ class AcsController : public ExtendedControllerBase {
|
||||
static const Submode_t SUBMODE_PTG_NADIR = 5;
|
||||
|
||||
protected:
|
||||
|
||||
void performSafe();
|
||||
void performDetumble();
|
||||
void performPointingCtrl();
|
||||
@ -41,6 +41,7 @@ class AcsController : public ExtendedControllerBase {
|
||||
ActuatorCmd actuatorCmd;
|
||||
Guidance guidance;
|
||||
|
||||
SafeCtrl safeCtrl;
|
||||
Detumble detumble;
|
||||
PtgCtrl ptgCtrl;
|
||||
|
||||
|
||||
@ -6,22 +6,22 @@
|
||||
*/
|
||||
|
||||
#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);
|
||||
MatrixOperations<double>::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3,
|
||||
3);
|
||||
}
|
||||
|
||||
SafeCtrl::~SafeCtrl(){
|
||||
|
||||
}
|
||||
SafeCtrl::~SafeCtrl() {}
|
||||
|
||||
void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
|
||||
safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters);
|
||||
@ -30,13 +30,10 @@ void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_){
|
||||
|
||||
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 *sunDirModel, bool *sunDirModelValid, double *satRateMekf,
|
||||
bool *rateMekfValid, double *sunDirRef, double *satRatRef,
|
||||
double *outputMagMomB, bool *outputValid) {
|
||||
|
||||
if ( !(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) ||
|
||||
!(*rateMekfValid)) {
|
||||
if (!(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) || !(*rateMekfValid)) {
|
||||
*outputValid = false;
|
||||
return SAFECTRL_MEKF_INPUT_INVALID;
|
||||
}
|
||||
@ -63,8 +60,8 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
|
||||
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 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;
|
||||
@ -86,17 +83,13 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
|
||||
*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 *outputMagMomB, bool *outputValid) {
|
||||
// Check for invalid Inputs
|
||||
if (!susDirBValid || !magFieldBValid || !magRateBValid) {
|
||||
*outputValid = false;
|
||||
@ -113,8 +106,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid,
|
||||
|
||||
// Rate parallel to sun direction and magnetic field direction
|
||||
double rateParaSun = 0, rateParaMag = 0;
|
||||
double dotSunRateMag = 0, dotmagRateSun = 0,
|
||||
rateFactor = 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);
|
||||
@ -167,7 +159,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid,
|
||||
// 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);
|
||||
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3,
|
||||
1);
|
||||
|
||||
// Magnetic moment
|
||||
double magMomB[3] = {0, 0, 0};
|
||||
@ -181,7 +174,4 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid,
|
||||
outputMagMomB[2] = magMomB[2];
|
||||
|
||||
*outputValid = true;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -8,20 +8,17 @@
|
||||
#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 "../OutputValues.h"
|
||||
#include "../SensorValues.h"
|
||||
#include "../config/classIds.h"
|
||||
|
||||
class SafeCtrl {
|
||||
|
||||
public:
|
||||
|
||||
SafeCtrl(AcsParameters *acsParameters_);
|
||||
virtual ~SafeCtrl();
|
||||
|
||||
@ -30,24 +27,20 @@ public:
|
||||
|
||||
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 (!)
|
||||
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 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;
|
||||
@ -55,10 +48,6 @@ private:
|
||||
|
||||
double magFieldBState[3];
|
||||
timeval magFieldBStateTime;
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif /* ACS_CONTROL_SAFECTRL_H_ */
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user