296 lines
12 KiB
C++
296 lines
12 KiB
C++
#ifndef MISSION_CONTROLLER_ACSCONTROLLER_H_
|
|
#define MISSION_CONTROLLER_ACSCONTROLLER_H_
|
|
|
|
#include <eive/objects.h>
|
|
#include <fsfw/controller/ExtendedControllerBase.h>
|
|
#include <fsfw/coordinates/Sgp4Propagator.h>
|
|
#include <fsfw/datapool/PoolReadGuard.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/acs/defs.h>
|
|
#include <mission/acs/imtqHelpers.h>
|
|
#include <mission/acs/rwHelpers.h>
|
|
#include <mission/acs/susMax1227Helpers.h>
|
|
#include <mission/config/torquer.h>
|
|
#include <mission/controller/acs/ActuatorCmd.h>
|
|
#include <mission/controller/acs/AttitudeEstimation.h>
|
|
#include <mission/controller/acs/FusedRotationEstimation.h>
|
|
#include <mission/controller/acs/Guidance.h>
|
|
#include <mission/controller/acs/MultiplicativeKalmanFilter.h>
|
|
#include <mission/controller/acs/Navigation.h>
|
|
#include <mission/controller/acs/SensorProcessing.h>
|
|
#include <mission/controller/acs/control/Detumble.h>
|
|
#include <mission/controller/acs/control/PtgCtrl.h>
|
|
#include <mission/controller/acs/control/SafeCtrl.h>
|
|
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
|
#include <mission/memory/SdCardMountedIF.h>
|
|
#include <mission/utility/trace.h>
|
|
|
|
#include <filesystem>
|
|
#include <fstream>
|
|
#include <optional>
|
|
|
|
class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
|
|
public:
|
|
static constexpr dur_millis_t INIT_DELAY = 500;
|
|
|
|
AcsController(object_id_t objectId, bool enableHkSets, SdCardMountedIF& sdcMan);
|
|
|
|
MessageQueueId_t getCommandQueue() const;
|
|
ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId,
|
|
ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues,
|
|
uint16_t startAtIndex) override;
|
|
|
|
protected:
|
|
private:
|
|
static constexpr int16_t ZERO_VEC3_INT16[3] = {0, 0, 0};
|
|
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
|
|
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
|
|
static constexpr double RW_OFF_TORQUE[4] = {0, 0, 0, 0};
|
|
static constexpr int32_t RW_OFF_SPEED[4] = {0, 0, 0, 0};
|
|
|
|
bool enableHkSets = false;
|
|
|
|
SdCardMountedIF& sdcMan;
|
|
|
|
timeval timeAbsolute;
|
|
timeval timeRelative;
|
|
double timeDelta = 0.0;
|
|
timeval oldTimeRelative;
|
|
|
|
AcsParameters acsParameters;
|
|
SensorProcessing sensorProcessing;
|
|
AttitudeEstimation attitudeEstimation;
|
|
FusedRotationEstimation fusedRotationEstimation;
|
|
Navigation navigation;
|
|
ActuatorCmd actuatorCmd;
|
|
Guidance guidance;
|
|
|
|
SafeCtrl safeCtrl;
|
|
Detumble detumble;
|
|
PtgCtrl ptgCtrl;
|
|
|
|
ParameterHelper parameterHelper;
|
|
|
|
bool tleTooOldFlag = false;
|
|
uint8_t detumbleCounter = 0;
|
|
uint8_t multipleRwUnavailableCounter = 0;
|
|
bool mekfInvalidFlag = false;
|
|
uint16_t ptgCtrlLostCounter = 0;
|
|
bool safeCtrlFailureFlag = false;
|
|
uint8_t safeCtrlFailureCounter = 0;
|
|
uint8_t resetMekfCount = 0;
|
|
bool mekfLost = false;
|
|
|
|
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
|
|
int16_t cmdDipoleMtqs[3] = {0, 0, 0};
|
|
|
|
acsctrl::RwAvail rwAvail;
|
|
|
|
#if OBSW_THREAD_TRACING == 1
|
|
uint32_t opCounter = 0;
|
|
#endif
|
|
|
|
enum class InternalState { STARTUP, INITIAL_DELAY, READY };
|
|
InternalState internalState = InternalState::STARTUP;
|
|
|
|
enum class DetumbleState {
|
|
NO_DETUMBLE,
|
|
DETUMBLE_FROM_PTG,
|
|
PTG_TO_SAFE_TRANSITION,
|
|
DETUMBLE_FROM_SAFE,
|
|
IN_DETUMBLE
|
|
};
|
|
DetumbleState detumbleState = DetumbleState::NO_DETUMBLE;
|
|
|
|
/** Device command IDs */
|
|
static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0;
|
|
static const DeviceCommandId_t RESET_MEKF = 0x1;
|
|
static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2;
|
|
static const DeviceCommandId_t UPDATE_TLE = 0x3;
|
|
static const DeviceCommandId_t READ_TLE = 0x4;
|
|
static const DeviceCommandId_t UPDATE_MEKF_STANDARD_DEVIATIONS = 0x5;
|
|
|
|
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);
|
|
|
|
void performAttitudeControl();
|
|
void performSafe();
|
|
void performDetumble();
|
|
void performPointingCtrl();
|
|
|
|
void handleDetumbling();
|
|
|
|
void safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure);
|
|
|
|
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
|
|
uint16_t dipoleTorqueDuration);
|
|
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(acs::ControlModeStrategy ctrlStrat);
|
|
void updateCtrlValData(double errAng, acs::ControlModeStrategy ctrlStrat);
|
|
void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng,
|
|
const double* tgtRotRate, acs::ControlModeStrategy cStrat);
|
|
|
|
ReturnValue_t updateTle(const uint8_t* line1, const uint8_t* line2, bool fromFile);
|
|
ReturnValue_t writeTleToFs(const uint8_t* tle);
|
|
ReturnValue_t readTleFromFs(uint8_t* line1, uint8_t* line2);
|
|
|
|
const std::string TLE_FILE = "/conf/tle.txt";
|
|
|
|
/* 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);
|
|
PoolEntry<uint8_t> gpsSource = PoolEntry<uint8_t>();
|
|
|
|
// Attitude Estimation
|
|
acsctrl::AttitudeEstimationData attitudeEstimationData;
|
|
PoolEntry<double> quatMekf = PoolEntry<double>(4);
|
|
PoolEntry<double> satRotRateMekf = PoolEntry<double>(3);
|
|
PoolEntry<uint8_t> mekfStatus = PoolEntry<uint8_t>();
|
|
PoolEntry<double> quatQuest = PoolEntry<double>(4);
|
|
|
|
// Ctrl Values
|
|
acsctrl::CtrlValData ctrlValData;
|
|
PoolEntry<uint8_t> safeStrat = PoolEntry<uint8_t>();
|
|
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);
|
|
|
|
// Fused Rot Rate
|
|
acsctrl::FusedRotRateData fusedRotRateData;
|
|
PoolEntry<double> rotRateOrthogonal = PoolEntry<double>(3);
|
|
PoolEntry<double> rotRateParallel = PoolEntry<double>(3);
|
|
PoolEntry<double> rotRateTotal = PoolEntry<double>(3);
|
|
PoolEntry<uint8_t> rotRateSource = PoolEntry<uint8_t>();
|
|
|
|
// Fused Rot Rate Sources
|
|
acsctrl::FusedRotRateSourcesData fusedRotRateSourcesData;
|
|
PoolEntry<double> rotRateOrthogonalSusMgm = PoolEntry<double>(3);
|
|
PoolEntry<double> rotRateParallelSusMgm = PoolEntry<double>(3);
|
|
PoolEntry<double> rotRateTotalSusMgm = PoolEntry<double>(3);
|
|
PoolEntry<double> rotRateTotalQuest = PoolEntry<double>(3);
|
|
PoolEntry<double> rotRateTotalStr = PoolEntry<double>(3);
|
|
|
|
// Initial delay to make sure all pool variables have been initialized their owners
|
|
Countdown initialCountdown = Countdown(INIT_DELAY);
|
|
|
|
// Countdown after which the detumbling mode change should have been finished
|
|
static constexpr dur_millis_t MAX_DURATION = 60 * 1e3;
|
|
Countdown detumbleTransitionCountdow = Countdown(MAX_DURATION);
|
|
};
|
|
|
|
#endif /* MISSION_CONTROLLER_ACSCONTROLLER_H_ */
|