eive-obsw/mission/controller/AcsController.h

227 lines
9.1 KiB
C
Raw Normal View History

2022-08-12 12:29:28 +02:00
#ifndef MISSION_CONTROLLER_ACSCONTROLLER_H_
#define MISSION_CONTROLLER_ACSCONTROLLER_H_
2023-02-23 11:06:45 +01:00
#include <eive/objects.h>
2022-08-12 12:29:28 +02:00
#include <fsfw/controller/ExtendedControllerBase.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
2023-03-08 18:09:50 +01:00
#include <fsfw/health/HealthTable.h>
#include <fsfw/parameters/ParameterHelper.h>
#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
2023-02-23 11:06:45 +01:00
#include <fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h>
#include <fsfw_hal/devicehandlers/MgmRM3100Handler.h>
2023-03-24 20:50:33 +01:00
#include <mission/acs/imtqHelpers.h>
#include <mission/acs/rwHelpers.h>
2023-02-28 19:14:15 +01:00
#include <mission/devices/devicedefinitions/susMax1227Helpers.h>
2023-02-23 11:06:45 +01:00
#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"
2022-10-20 11:07:45 +02:00
#include "acs/control/SafeCtrl.h"
#include "controllerdefinitions/AcsCtrlDefinitions.h"
2022-08-12 12:29:28 +02:00
class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
2022-08-15 09:58:18 +02:00
public:
2022-08-15 10:50:19 +02:00
static constexpr dur_millis_t INIT_DELAY = 500;
2022-08-15 09:58:18 +02:00
AcsController(object_id_t objectId);
2022-08-12 12:29:28 +02:00
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();
2022-08-15 09:58:18 +02:00
private:
2023-02-23 18:34:28 +01:00
static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1};
static constexpr double ZERO_VEC[3] = {0, 0, 0};
2023-02-23 18:39:14 +01:00
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};
2023-02-23 18:34:28 +01:00
2022-10-06 15:37:41 +02:00
AcsParameters acsParameters;
SensorProcessing sensorProcessing;
Navigation navigation;
ActuatorCmd actuatorCmd;
Guidance guidance;
SafeCtrl safeCtrl;
Detumble detumble;
PtgCtrl ptgCtrl;
ParameterHelper parameterHelper;
2023-02-22 14:50:38 +01:00
uint8_t detumbleCounter = 0;
uint8_t multipleRwUnavailableCounter = 0;
2023-02-24 10:28:13 +01:00
bool mekfInvalidFlag = false;
2023-03-14 14:12:55 +01:00
uint16_t mekfInvalidCounter = 0;
2023-03-15 10:06:24 +01:00
bool safeCtrlFailureFlag = false;
uint8_t safeCtrlFailureCounter = 0;
uint8_t resetMekfCount = 0;
bool mekfLost = false;
2023-02-22 14:50:38 +01:00
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
int16_t cmdDipolMtqs[3] = {0, 0, 0};
2023-02-14 10:59:35 +01:00
#if OBSW_THREAD_TRACING == 1
uint32_t opCounter = 0;
#endif
2022-08-15 10:50:19 +02:00
enum class InternalState { STARTUP, INITIAL_DELAY, READY };
InternalState internalState = InternalState::STARTUP;
2023-02-22 17:10:42 +01:00
/** 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;
2023-02-22 17:10:42 +01:00
2023-02-23 11:26:43 +01:00
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL;
static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0);
2023-02-08 15:02:56 +01:00
ReturnValue_t initialize() override;
2022-08-12 12:29:28 +02:00
ReturnValue_t handleCommandMessage(CommandMessage* message) override;
void performControlOperation() override;
2023-02-22 16:10:30 +01:00
/* HasActionsIF overrides */
ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
const uint8_t* data, size_t size) override;
2022-08-12 12:29:28 +02:00
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
2022-08-15 09:58:18 +02:00
LocalDataPoolManager& poolManager) override;
2022-08-12 12:29:28 +02:00
LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
// Mode abstract functions
ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
2022-08-15 09:58:18 +02:00
uint32_t* msToReachTheMode) override;
void modeChanged(Mode_t mode, Submode_t submode);
void announceMode(bool recursive);
2022-08-12 12:29:28 +02:00
2023-02-10 11:27:25 +01:00
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);
2023-02-23 18:34:28 +01:00
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();
2023-02-10 11:27:25 +01:00
2023-02-08 17:02:22 +01:00
/* ACS Sensor Values */
ACS::SensorValues sensorValues;
/* ACS Actuation Datasets */
2023-02-19 12:25:26 +01:00
imtq::DipoleActuationSet dipoleSet = imtq::DipoleActuationSet(objects::IMTQ_HANDLER);
2023-02-15 17:02:22 +01:00
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 */
2022-08-12 12:29:28 +02:00
// 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);
2022-08-15 11:19:08 +02:00
PoolEntry<uint8_t> imtqCalActStatus = PoolEntry<uint8_t>();
2022-08-15 10:50:19 +02:00
void copyMgmData();
2022-08-12 12:29:28 +02:00
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);
2022-10-25 11:31:54 +02:00
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);
2022-09-28 15:27:51 +02:00
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>();
2023-01-11 13:42:48 +01:00
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);
2023-02-21 16:02:26 +01:00
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);
2022-11-08 10:56:12 +01:00
PoolEntry<int32_t> rwTargetSpeed = PoolEntry<int32_t>(4);
PoolEntry<int16_t> mtqTargetDipole = PoolEntry<int16_t>(3);
2022-08-15 10:50:19 +02:00
// Initial delay to make sure all pool variables have been initialized their owners
Countdown initialCountdown = Countdown(INIT_DELAY);
2022-08-12 12:29:28 +02:00
};
#endif /* MISSION_CONTROLLER_ACSCONTROLLER_H_ */