131 lines
4.8 KiB
C++
131 lines
4.8 KiB
C++
#ifndef MISSION_CONTROLLER_ACSCONTROLLER_H_
|
|
#define MISSION_CONTROLLER_ACSCONTROLLER_H_
|
|
|
|
#include <commonObjects.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/SafeCtrl.h"
|
|
#include "acs/control/Detumble.h"
|
|
#include "acs/control/PtgCtrl.h"
|
|
#include "controllerdefinitions/AcsCtrlDefinitions.h"
|
|
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
|
|
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
|
|
#include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h"
|
|
#include "mission/devices/devicedefinitions/SusDefinitions.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_SUN = 6;
|
|
static const Submode_t SUBMODE_PTG_INERTIAL = 7;
|
|
|
|
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);
|
|
|
|
// MGMs
|
|
acsctrl::MgmDataRaw mgmData;
|
|
|
|
MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set =
|
|
MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER);
|
|
RM3100::Rm3100PrimaryDataset mgm1Rm3100Set =
|
|
RM3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER);
|
|
MGMLIS3MDL::MgmPrimaryDataset mgm2Lis3Set =
|
|
MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER);
|
|
RM3100::Rm3100PrimaryDataset mgm3Rm3100Set =
|
|
RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER);
|
|
IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_HANDLER);
|
|
|
|
PoolEntry<float> mgm0PoolVec = PoolEntry<float>(3);
|
|
PoolEntry<float> mgm1PoolVec = PoolEntry<float>(3);
|
|
PoolEntry<float> mgm2PoolVec = PoolEntry<float>(3);
|
|
PoolEntry<float> mgm3PoolVec = PoolEntry<float>(3);
|
|
PoolEntry<float> imtqMgmPoolVec = PoolEntry<float>(3);
|
|
PoolEntry<uint8_t> imtqCalActStatus = PoolEntry<uint8_t>();
|
|
|
|
void copyMgmData();
|
|
|
|
// Sun Sensors
|
|
|
|
acsctrl::SusDataRaw susData;
|
|
|
|
std::array<SUS::SusDataset, 12> susSets{
|
|
SUS::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF),
|
|
SUS::SusDataset(objects::SUS_1_N_LOC_XBYFZM_PT_XB),
|
|
SUS::SusDataset(objects::SUS_2_N_LOC_XFYBZB_PT_YB),
|
|
SUS::SusDataset(objects::SUS_3_N_LOC_XFYBZF_PT_YF),
|
|
SUS::SusDataset(objects::SUS_4_N_LOC_XMYFZF_PT_ZF),
|
|
SUS::SusDataset(objects::SUS_5_N_LOC_XFYMZB_PT_ZB),
|
|
SUS::SusDataset(objects::SUS_6_R_LOC_XFYBZM_PT_XF),
|
|
SUS::SusDataset(objects::SUS_7_R_LOC_XBYBZM_PT_XB),
|
|
SUS::SusDataset(objects::SUS_8_R_LOC_XBYBZB_PT_YB),
|
|
SUS::SusDataset(objects::SUS_9_R_LOC_XBYBZB_PT_YF),
|
|
SUS::SusDataset(objects::SUS_10_N_LOC_XMYBZF_PT_ZF),
|
|
SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB),
|
|
};
|
|
|
|
PoolEntry<uint16_t> sus0PoolVec = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus1PoolVec = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus2PoolVec = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus3PoolVec = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus4PoolVec = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus5PoolVec = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus6PoolVec = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus7PoolVec = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus8PoolVec = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus9PoolVec = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus10PoolVec = PoolEntry<uint16_t>(6);
|
|
PoolEntry<uint16_t> sus11PoolVec = PoolEntry<uint16_t>(6);
|
|
|
|
void copySusData();
|
|
|
|
// Initial delay to make sure all pool variables have been initialized their owners
|
|
Countdown initialCountdown = Countdown(INIT_DELAY);
|
|
};
|
|
|
|
#endif /* MISSION_CONTROLLER_ACSCONTROLLER_H_ */
|