64 lines
1.6 KiB
C
64 lines
1.6 KiB
C
|
/*
|
||
|
|
* 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_ */
|
||
|
|
|