#ifndef MISSION_CONTROLLER_ACSCONTROLLER_H_ #define MISSION_CONTROLLER_ACSCONTROLLER_H_ #include <eive/objects.h> #include <fsfw/controller/ExtendedControllerBase.h> #include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/health/HealthTable.h> #include <fsfw/parameters/ParameterHelper.h> #include <fsfw/parameters/ReceivesParameterMessagesIF.h> #include <fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h> #include <fsfw_hal/devicehandlers/MgmRM3100Handler.h> #include <mission/devices/devicedefinitions/imtqHelpers.h> #include <mission/devices/devicedefinitions/rwHelpers.h> #include <mission/devices/devicedefinitions/susMax1227Helpers.h> #include <mission/trace.h> #include "acs/ActuatorCmd.h" #include "acs/Guidance.h" #include "acs/MultiplicativeKalmanFilter.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" class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF { public: static constexpr dur_millis_t INIT_DELAY = 500; AcsController(object_id_t objectId); MessageQueueId_t getCommandQueue() const; ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId, ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues, uint16_t startAtIndex) override; protected: void performSafe(); void performDetumble(); void performPointingCtrl(); private: static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1}; static constexpr double ZERO_VEC[3] = {0, 0, 0}; static constexpr double RW_OFF_TORQUE[4] = {0.0, 0.0, 0.0, 0.0}; static constexpr int32_t RW_OFF_SPEED[4] = {0, 0, 0, 0}; AcsParameters acsParameters; SensorProcessing sensorProcessing; Navigation navigation; ActuatorCmd actuatorCmd; Guidance guidance; SafeCtrl safeCtrl; Detumble detumble; PtgCtrl ptgCtrl; ParameterHelper parameterHelper; uint8_t detumbleCounter = 0; uint8_t multipleRwUnavailableCounter = 0; bool mekfInvalidFlag = false; uint16_t mekfInvalidCounter = 0; int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; int16_t cmdDipolMtqs[3] = {0, 0, 0}; #if OBSW_THREAD_TRACING == 1 uint32_t opCounter = 0; #endif enum class InternalState { STARTUP, INITIAL_DELAY, READY }; InternalState internalState = InternalState::STARTUP; /** Device command IDs */ static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0; static const DeviceCommandId_t RESET_MEKF = 0x1; static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL; static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0); ReturnValue_t initialize() override; ReturnValue_t handleCommandMessage(CommandMessage* message) override; void performControlOperation() override; /* HasActionsIF overrides */ ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size) 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); ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed, int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime); void updateActuatorCmdData(const int16_t* mtqTargetDipole); void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed, const int16_t* mtqTargetDipole); void updateCtrlValData(double errAng); void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng, const double* tgtRotRate); void disableCtrlValData(); /* ACS Sensor Values */ ACS::SensorValues sensorValues; /* ACS Actuation Datasets */ imtq::DipoleActuationSet dipoleSet = imtq::DipoleActuationSet(objects::IMTQ_HANDLER); rws::RwSpeedActuationSet rw1SpeedSet = rws::RwSpeedActuationSet(objects::RW1); rws::RwSpeedActuationSet rw2SpeedSet = rws::RwSpeedActuationSet(objects::RW2); rws::RwSpeedActuationSet rw3SpeedSet = rws::RwSpeedActuationSet(objects::RW3); rws::RwSpeedActuationSet rw4SpeedSet = rws::RwSpeedActuationSet(objects::RW4); /* ACS Datasets */ // 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>(); PoolEntry<double> altitude = PoolEntry<double>(); PoolEntry<double> gpsPosition = PoolEntry<double>(3); PoolEntry<double> gpsVelocity = PoolEntry<double>(3); // MEKF acsctrl::MekfData mekfData; PoolEntry<double> quatMekf = PoolEntry<double>(4); PoolEntry<double> satRotRateMekf = PoolEntry<double>(3); PoolEntry<uint8_t> mekfStatus = PoolEntry<uint8_t>(); // Ctrl Values acsctrl::CtrlValData ctrlValData; PoolEntry<double> tgtQuat = PoolEntry<double>(4); PoolEntry<double> errQuat = PoolEntry<double>(4); PoolEntry<double> errAng = PoolEntry<double>(); PoolEntry<double> tgtRotRate = PoolEntry<double>(3); // 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_ */