169 lines
6.4 KiB
C++
169 lines
6.4 KiB
C++
#ifndef MISSION_CONTROLLER_ACSCONTROLLER_H_
|
|
#define MISSION_CONTROLLER_ACSCONTROLLER_H_
|
|
|
|
#include <fsfw/controller/ExtendedControllerBase.h>
|
|
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
|
|
|
#include "acs/ActuatorCmd.h"
|
|
#include "acs/Guidance.h"
|
|
#include "acs/Navigation.h"
|
|
#include "acs/SensorProcessing.h"
|
|
#include "acs/control/Detumble.h"
|
|
#include "acs/control/PtgCtrl.h"
|
|
#include "acs/control/SafeCtrl.h"
|
|
#include "controllerdefinitions/AcsCtrlDefinitions.h"
|
|
#include "eive/objects.h"
|
|
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
|
|
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
|
|
#include "mission/devices/devicedefinitions/SusDefinitions.h"
|
|
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
|
|
|
|
class AcsController : public ExtendedControllerBase {
|
|
public:
|
|
static constexpr dur_millis_t INIT_DELAY = 500;
|
|
|
|
AcsController(object_id_t objectId);
|
|
|
|
static const Submode_t SUBMODE_SAFE = 2;
|
|
static const Submode_t SUBMODE_DETUMBLE = 3;
|
|
static const Submode_t SUBMODE_PTG_TARGET = 4;
|
|
static const Submode_t SUBMODE_PTG_NADIR = 5;
|
|
static const Submode_t SUBMODE_PTG_INERTIAL = 6;
|
|
|
|
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
|
|
static const Event SAFE_RATE_VIOLATION =
|
|
MAKE_EVENT(0, severity::MEDIUM); //!< The limits for the rotation in safe mode were violated.
|
|
|
|
protected:
|
|
void performSafe();
|
|
void performDetumble();
|
|
void performPointingCtrl();
|
|
|
|
private:
|
|
AcsParameters acsParameters;
|
|
SensorProcessing sensorProcessing;
|
|
Navigation navigation;
|
|
ActuatorCmd actuatorCmd;
|
|
Guidance guidance;
|
|
|
|
SafeCtrl safeCtrl;
|
|
Detumble detumble;
|
|
PtgCtrl ptgCtrl;
|
|
|
|
uint8_t detumbleCounter;
|
|
|
|
enum class InternalState { STARTUP, INITIAL_DELAY, READY };
|
|
|
|
InternalState internalState = InternalState::STARTUP;
|
|
|
|
ReturnValue_t handleCommandMessage(CommandMessage* message) override;
|
|
void performControlOperation() override;
|
|
|
|
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
|
LocalDataPoolManager& poolManager) override;
|
|
LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
|
|
|
|
// Mode abstract functions
|
|
ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
|
|
uint32_t* msToReachTheMode) override;
|
|
void modeChanged(Mode_t mode, Submode_t submode);
|
|
void announceMode(bool recursive);
|
|
|
|
/* ACS Datasets */
|
|
IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
|
|
// MGMs
|
|
acsctrl::MgmDataRaw mgmDataRaw;
|
|
PoolEntry<float> mgm0VecRaw = PoolEntry<float>(3);
|
|
PoolEntry<float> mgm1VecRaw = PoolEntry<float>(3);
|
|
PoolEntry<float> mgm2VecRaw = PoolEntry<float>(3);
|
|
PoolEntry<float> mgm3VecRaw = PoolEntry<float>(3);
|
|
PoolEntry<float> imtqMgmVecRaw = PoolEntry<float>(3);
|
|
PoolEntry<uint8_t> imtqCalActStatus = PoolEntry<uint8_t>();
|
|
void copyMgmData();
|
|
|
|
acsctrl::MgmDataProcessed mgmDataProcessed;
|
|
PoolEntry<float> mgm0VecProc = PoolEntry<float>(3);
|
|
PoolEntry<float> mgm1VecProc = PoolEntry<float>(3);
|
|
PoolEntry<float> mgm2VecProc = PoolEntry<float>(3);
|
|
PoolEntry<float> mgm3VecProc = PoolEntry<float>(3);
|
|
PoolEntry<float> mgm4VecProc = PoolEntry<float>(3);
|
|
PoolEntry<double> mgmVecTot = PoolEntry<double>(3);
|
|
PoolEntry<double> mgmVecTotDer = PoolEntry<double>(3);
|
|
PoolEntry<double> magIgrf = PoolEntry<double>(3);
|
|
|
|
// SUSs
|
|
acsctrl::SusDataRaw susDataRaw;
|
|
PoolEntry<uint16_t> sus0ValRaw = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus1ValRaw = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus2ValRaw = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus3ValRaw = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus4ValRaw = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus5ValRaw = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus6ValRaw = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus7ValRaw = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus8ValRaw = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus9ValRaw = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus10ValRaw = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus11ValRaw = PoolEntry<uint16_t>(6);
|
|
void copySusData();
|
|
|
|
acsctrl::SusDataProcessed susDataProcessed;
|
|
PoolEntry<float> sus0VecProc = PoolEntry<float>(3);
|
|
PoolEntry<float> sus1VecProc = PoolEntry<float>(3);
|
|
PoolEntry<float> sus2VecProc = PoolEntry<float>(3);
|
|
PoolEntry<float> sus3VecProc = PoolEntry<float>(3);
|
|
PoolEntry<float> sus4VecProc = PoolEntry<float>(3);
|
|
PoolEntry<float> sus5VecProc = PoolEntry<float>(3);
|
|
PoolEntry<float> sus6VecProc = PoolEntry<float>(3);
|
|
PoolEntry<float> sus7VecProc = PoolEntry<float>(3);
|
|
PoolEntry<float> sus8VecProc = PoolEntry<float>(3);
|
|
PoolEntry<float> sus9VecProc = PoolEntry<float>(3);
|
|
PoolEntry<float> sus10VecProc = PoolEntry<float>(3);
|
|
PoolEntry<float> sus11VecProc = PoolEntry<float>(3);
|
|
PoolEntry<double> susVecTot = PoolEntry<double>(3);
|
|
PoolEntry<double> susVecTotDer = PoolEntry<double>(3);
|
|
PoolEntry<double> sunIjk = PoolEntry<double>(3);
|
|
|
|
// GYRs
|
|
acsctrl::GyrDataRaw gyrDataRaw;
|
|
PoolEntry<double> gyr0VecRaw = PoolEntry<double>(3);
|
|
PoolEntry<float> gyr1VecRaw = PoolEntry<float>(3);
|
|
PoolEntry<double> gyr2VecRaw = PoolEntry<double>(3);
|
|
PoolEntry<float> gyr3VecRaw = PoolEntry<float>(3);
|
|
void copyGyrData();
|
|
|
|
acsctrl::GyrDataProcessed gyrDataProcessed;
|
|
PoolEntry<double> gyr0VecProc = PoolEntry<double>(3);
|
|
PoolEntry<double> gyr1VecProc = PoolEntry<double>(3);
|
|
PoolEntry<double> gyr2VecProc = PoolEntry<double>(3);
|
|
PoolEntry<double> gyr3VecProc = PoolEntry<double>(3);
|
|
PoolEntry<double> gyrVecTot = PoolEntry<double>(3);
|
|
|
|
// GPS
|
|
acsctrl::GpsDataProcessed gpsDataProcessed;
|
|
PoolEntry<double> gcLatitude = PoolEntry<double>();
|
|
PoolEntry<double> gdLongitude = PoolEntry<double>();
|
|
|
|
// MEKF
|
|
acsctrl::MekfData mekfData;
|
|
PoolEntry<double> quatMekf = PoolEntry<double>(4);
|
|
PoolEntry<double> satRotRateMekf = PoolEntry<double>(3);
|
|
|
|
// Ctrl Values
|
|
acsctrl::CtrlValData ctrlValData;
|
|
PoolEntry<double> tgtQuat = PoolEntry<double>(4);
|
|
PoolEntry<double> errQuat = PoolEntry<double>(4);
|
|
PoolEntry<double> errAng = PoolEntry<double>();
|
|
|
|
// Actuator CMD
|
|
acsctrl::ActuatorCmdData actuatorCmdData;
|
|
PoolEntry<double> rwTargetTorque = PoolEntry<double>(4);
|
|
PoolEntry<int32_t> rwTargetSpeed = PoolEntry<int32_t>(4);
|
|
PoolEntry<int16_t> mtqTargetDipole = PoolEntry<int16_t>(3);
|
|
|
|
// Initial delay to make sure all pool variables have been initialized their owners
|
|
Countdown initialCountdown = Countdown(INIT_DELAY);
|
|
};
|
|
|
|
#endif /* MISSION_CONTROLLER_ACSCONTROLLER_H_ */
|