diff --git a/CHANGELOG.md b/CHANGELOG.md index 62e39ed8..c999804b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -29,6 +29,30 @@ will consitute of a breaking change warranting a new major release: ## Fixed - ADIS1650X: Added missing MDL_RANG pool entry for configuration set +- Bumped FSFW for bugfix in health service: No execution complete for targeted health announce + command. +- Removed matrix determinant calculation as part of the `MEKF`, which would take about + 300ms of runtime +- Resetting the `MEKF` now also actually resets its stored state +- Bumped FSFW for bugfix in destination handler: Better error handling and able to process + destination folder path. + +## Changed + +- Commented out commanding of actuators as part of the `AcsController` +- Collection sets of the `AcsController` now get updated before running the actual ACS + algorithm +- `GpsController` now always gets scheduled + +## Added + +- `ExecutableComIfDummy` class to have a dummy for classes like the RTD polling class. +- Added `AcsController` action command to confirm solar array deployment, which then deletes + two files +- Added `AcsController` action command to reset `MEKF` +- `GpsCtrlDummy` now initializes the `gpsSet` +- `RwDummy` now initializes with a non faulty state + # [v1.31.1] diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index f725027c..c979ff5e 100644 --- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp +++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 261 translations. + * @brief Auto-generated event translation file. Contains 263 translations. * @details - * Generated on: 2023-02-23 15:59:16 + * Generated on: 2023-02-24 16:57:00 */ #include "translateEvents.h" @@ -90,6 +90,8 @@ const char *CHANGE_OF_SETUP_PARAMETER_STRING = "CHANGE_OF_SETUP_PARAMETER"; const char *STORE_ERROR_STRING = "STORE_ERROR"; const char *MSG_QUEUE_ERROR_STRING = "MSG_QUEUE_ERROR"; const char *SERIALIZATION_ERROR_STRING = "SERIALIZATION_ERROR"; +const char *FILESTORE_ERROR_STRING = "FILESTORE_ERROR"; +const char *FILENAME_TOO_LARGE_ERROR_STRING = "FILENAME_TOO_LARGE_ERROR"; const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION"; const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY"; const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID"; @@ -434,6 +436,10 @@ const char *translateEvents(Event event) { return MSG_QUEUE_ERROR_STRING; case (10802): return SERIALIZATION_ERROR_STRING; + case (10803): + return FILESTORE_ERROR_STRING; + case (10804): + return FILENAME_TOO_LARGE_ERROR_STRING; case (11200): return SAFE_RATE_VIOLATION_STRING; case (11201): diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index 28d0f635..be60d130 100644 --- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp +++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 154 translations. - * Generated on: 2023-02-23 15:59:16 + * Generated on: 2023-02-24 16:57:00 */ #include "translateObjects.h" diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index 6dc7fceb..7647af8b 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -187,14 +187,12 @@ void scheduling::initTasks() { } #endif -#if OBSW_ADD_GPS_CTRL == 1 PeriodicTaskIF* gpsTask = factory->createPeriodicTask( "GPS_TASK", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); result = gpsTask->addComponent(objects::GPS_CONTROLLER); if (result != returnvalue::OK) { scheduling::printAddObjectError("GPS_CTRL", objects::GPS_CONTROLLER); } -#endif /* OBSW_ADD_GPS_CTRL */ #if OBSW_ADD_RW == 1 PeriodicTaskIF* rwPolling = factory->createPeriodicTask( @@ -378,9 +376,7 @@ void scheduling::initTasks() { #if OBSW_ADD_RW == 1 rwPolling->startTask(); #endif -#if OBSW_ADD_GPS_CTRL == 1 gpsTask->startTask(); -#endif acsSysTask->startTask(); if (not tcsSystemTask->isEmpty()) { tcsSystemTask->startTask(); diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h index edeb3765..4f2c948e 100644 --- a/common/config/eive/definitions.h +++ b/common/config/eive/definitions.h @@ -61,8 +61,8 @@ static constexpr uint32_t SCHED_BLOCK_1_SUS_READ_MS = 15; static constexpr uint32_t SCHED_BLOCK_2_SENSOR_READ_MS = 30; static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 42; static constexpr uint32_t SCHED_BLOCK_4_ACS_CTRL_MS = 45; -static constexpr uint32_t SCHED_BLOCK_5_ACTUATOR_MS = 50; -static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 90; +static constexpr uint32_t SCHED_BLOCK_5_ACTUATOR_MS = 55; +static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 95; static constexpr uint32_t SCHED_BLOCK_RTD = 150; static constexpr uint32_t SCHED_BLOCK_7_RW_READ_MS = 300; diff --git a/common/config/eive/resultClassIds.h b/common/config/eive/resultClassIds.h index e4dfb927..310d9e0b 100644 --- a/common/config/eive/resultClassIds.h +++ b/common/config/eive/resultClassIds.h @@ -35,14 +35,13 @@ enum commonClassIds : uint8_t { SA_DEPL_HANDLER, // SADPL MPSOC_RETURN_VALUES_IF, // MPSOCRTVIF SUPV_RETURN_VALUES_IF, // SPVRTVIF - ACS_KALMAN, // ACSKAL + ACS_CTRL, // ACSCTRL + ACS_MEKF, // ACSMEKF ACS_SAFE, // ACSSAF ACS_PTG, // ACSPTG ACS_DETUMBLE, // ACSDTB - ACS_MEKF, // ACSMEK COMMON_CLASS_ID_END // [EXPORT] : [END] }; - } #endif /* COMMON_CONFIG_COMMONCLASSIDS_H_ */ diff --git a/dummies/CMakeLists.txt b/dummies/CMakeLists.txt index 9040fbb0..41cb3328 100644 --- a/dummies/CMakeLists.txt +++ b/dummies/CMakeLists.txt @@ -20,11 +20,11 @@ target_sources( GyroL3GD20Dummy.cpp MgmLIS3MDLDummy.cpp PlPcduDummy.cpp + ExecutableComIfDummy.cpp ScexDummy.cpp CoreControllerDummy.cpp PlocMpsocDummy.cpp PlocSupervisorDummy.cpp helpers.cpp - RtdPollingDummy.cpp MgmRm3100Dummy.cpp Tmp1075Dummy.cpp) diff --git a/dummies/ExecutableComIfDummy.cpp b/dummies/ExecutableComIfDummy.cpp new file mode 100644 index 00000000..becb4a73 --- /dev/null +++ b/dummies/ExecutableComIfDummy.cpp @@ -0,0 +1,27 @@ +#include + +ExecutableComIfDummy::ExecutableComIfDummy(object_id_t objectId) : SystemObject(objectId) {} + +ReturnValue_t ExecutableComIfDummy::initializeInterface(CookieIF *cookie) { + return returnvalue::OK; +} + +ReturnValue_t ExecutableComIfDummy::sendMessage(CookieIF *cookie, const uint8_t *sendData, + size_t sendLen) { + return returnvalue::OK; +} + +ReturnValue_t ExecutableComIfDummy::getSendSuccess(CookieIF *cookie) { return returnvalue::OK; } + +ReturnValue_t ExecutableComIfDummy::requestReceiveMessage(CookieIF *cookie, size_t requestLen) { + return returnvalue::OK; +} + +ReturnValue_t ExecutableComIfDummy::performOperation(uint8_t operationCode) { + return returnvalue::OK; +} + +ReturnValue_t ExecutableComIfDummy::readReceivedMessage(CookieIF *cookie, uint8_t **buffer, + size_t *size) { + return returnvalue::OK; +} diff --git a/dummies/ExecutableComIfDummy.h b/dummies/ExecutableComIfDummy.h new file mode 100644 index 00000000..80e667d7 --- /dev/null +++ b/dummies/ExecutableComIfDummy.h @@ -0,0 +1,21 @@ +#ifndef DUMMIES_EXECUTABLECOMIFDUMMY_H_ +#define DUMMIES_EXECUTABLECOMIFDUMMY_H_ + +#include +#include +#include + +class ExecutableComIfDummy : public ExecutableObjectIF, + public DeviceCommunicationIF, + public SystemObject { + public: + ExecutableComIfDummy(object_id_t objectId); + ReturnValue_t performOperation(uint8_t operationCode = 0) override; + ReturnValue_t initializeInterface(CookieIF *cookie) override; + ReturnValue_t sendMessage(CookieIF *cookie, const uint8_t *sendData, size_t sendLen) override; + ReturnValue_t getSendSuccess(CookieIF *cookie) override; + ReturnValue_t requestReceiveMessage(CookieIF *cookie, size_t requestLen) override; + ReturnValue_t readReceivedMessage(CookieIF *cookie, uint8_t **buffer, size_t *size) override; +}; + +#endif /* DUMMIES_EXECUTABLECOMIFDUMMY_H_ */ diff --git a/dummies/GpsCtrlDummy.cpp b/dummies/GpsCtrlDummy.cpp index e69261b4..4ae8dc6f 100644 --- a/dummies/GpsCtrlDummy.cpp +++ b/dummies/GpsCtrlDummy.cpp @@ -1,6 +1,7 @@ #include "GpsCtrlDummy.h" -GpsCtrlDummy::GpsCtrlDummy(object_id_t objectId) : ExtendedControllerBase(objectId, 20) {} +GpsCtrlDummy::GpsCtrlDummy(object_id_t objectId) + : ExtendedControllerBase(objectId, 20), gpsSet(this) {} ReturnValue_t GpsCtrlDummy::handleCommandMessage(CommandMessage* message) { return returnvalue::OK; @@ -13,9 +14,23 @@ ReturnValue_t GpsCtrlDummy::checkModeCommand(Mode_t mode, Submode_t submode, return returnvalue::OK; } +LocalPoolDataSetBase* GpsCtrlDummy::getDataSetHandle(sid_t sid) { return &gpsSet; } + ReturnValue_t GpsCtrlDummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { + localDataPoolMap.emplace(GpsHyperion::ALTITUDE, new PoolEntry({537222.3469}, true)); + localDataPoolMap.emplace(GpsHyperion::LONGITUDE, new PoolEntry({-8.8579}, true)); + localDataPoolMap.emplace(GpsHyperion::LATITUDE, new PoolEntry({49.5952}, true)); + localDataPoolMap.emplace(GpsHyperion::SPEED, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::YEAR, new PoolEntry({2023}, true)); + localDataPoolMap.emplace(GpsHyperion::MONTH, new PoolEntry({5}, true)); + localDataPoolMap.emplace(GpsHyperion::DAY, new PoolEntry({16}, true)); + localDataPoolMap.emplace(GpsHyperion::HOURS, new PoolEntry({1}, true)); + localDataPoolMap.emplace(GpsHyperion::MINUTES, new PoolEntry({0}, true)); + localDataPoolMap.emplace(GpsHyperion::SECONDS, new PoolEntry({0}, true)); + localDataPoolMap.emplace(GpsHyperion::UNIX_SECONDS, new PoolEntry({1684191600}, true)); + localDataPoolMap.emplace(GpsHyperion::SATS_IN_USE, new PoolEntry()); + localDataPoolMap.emplace(GpsHyperion::SATS_IN_VIEW, new PoolEntry()); + localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry()); return returnvalue::OK; } - -LocalPoolDataSetBase* GpsCtrlDummy::getDataSetHandle(sid_t sid) { return nullptr; } diff --git a/dummies/GpsCtrlDummy.h b/dummies/GpsCtrlDummy.h index 128a9a85..4b7fc1bf 100644 --- a/dummies/GpsCtrlDummy.h +++ b/dummies/GpsCtrlDummy.h @@ -2,12 +2,15 @@ #define DUMMIES_GPSCTRLDUMMY_H_ #include +#include class GpsCtrlDummy : public ExtendedControllerBase { public: GpsCtrlDummy(object_id_t objectId); private: + GpsPrimaryDataset gpsSet; + ReturnValue_t handleCommandMessage(CommandMessage* message) override; void performControlOperation() override; ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, diff --git a/dummies/RtdPollingDummy.cpp b/dummies/RtdPollingDummy.cpp deleted file mode 100644 index 82eb3ec6..00000000 --- a/dummies/RtdPollingDummy.cpp +++ /dev/null @@ -1,5 +0,0 @@ -#include "RtdPollingDummy.h" - -RtdPollingDummy::RtdPollingDummy(object_id_t objectId) : SystemObject(objectId) {} - -ReturnValue_t RtdPollingDummy::performOperation(uint8_t operationCode) { return returnvalue::OK; } diff --git a/dummies/RtdPollingDummy.h b/dummies/RtdPollingDummy.h deleted file mode 100644 index b5ffb5f2..00000000 --- a/dummies/RtdPollingDummy.h +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef DUMMIES_RTDPOLLINGDUMMY_H_ -#define DUMMIES_RTDPOLLINGDUMMY_H_ - -#include -#include - -class RtdPollingDummy : public ExecutableObjectIF, public SystemObject { - public: - RtdPollingDummy(object_id_t objectId); - ReturnValue_t performOperation(uint8_t operationCode = 0) override; -}; - -#endif /* DUMMIES_RTDPOLLINGDUMMY_H_ */ diff --git a/dummies/RwDummy.cpp b/dummies/RwDummy.cpp index 54e7ac83..d90f3f7c 100644 --- a/dummies/RwDummy.cpp +++ b/dummies/RwDummy.cpp @@ -41,7 +41,7 @@ ReturnValue_t RwDummy::initializeLocalDataPool(localpool::DataPool &localDataPoo localDataPoolMap.emplace(rws::CURR_SPEED, new PoolEntry({0})); localDataPoolMap.emplace(rws::REFERENCE_SPEED, new PoolEntry({0})); - localDataPoolMap.emplace(rws::STATE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::STATE, new PoolEntry({1}, true)); localDataPoolMap.emplace(rws::CLC_MODE, new PoolEntry({0})); localDataPoolMap.emplace(rws::LAST_RESET_STATUS, new PoolEntry({0})); diff --git a/dummies/SusDummy.cpp b/dummies/SusDummy.cpp index c0aed6dd..7c271b21 100644 --- a/dummies/SusDummy.cpp +++ b/dummies/SusDummy.cpp @@ -37,7 +37,7 @@ ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool &localDataPo LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry({0}, 1, true)); localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, - new PoolEntry({0, 0, 0, 0, 0, 0}, true)); + new PoolEntry({2603, 781, 2760, 2048, 4056, 0}, true)); return returnvalue::OK; } diff --git a/dummies/helpers.cpp b/dummies/helpers.cpp index 8a49a592..344ee611 100644 --- a/dummies/helpers.cpp +++ b/dummies/helpers.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -46,7 +47,7 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio new CoreControllerDummy(objects::CORE_CONTROLLER); } if (cfg.addRtdComIFDummy) { - new RtdPollingDummy(objects::SPI_RTD_COM_IF); + new ExecutableComIfDummy(objects::SPI_RTD_COM_IF); } std::array rwIds = {objects::RW1, objects::RW2, objects::RW3, objects::RW4}; std::array rws; diff --git a/fsfw b/fsfw index d373c45d..216f603d 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit d373c45d36ef1c817e1a849afd91dbd81bdb5e32 +Subproject commit 216f603d62892356cb16e4ee4a35a59399c92ceb diff --git a/generators/bsp_hosted_events.csv b/generators/bsp_hosted_events.csv index 432e0828..6d26ed51 100644 --- a/generators/bsp_hosted_events.csv +++ b/generators/bsp_hosted_events.csv @@ -84,6 +84,8 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 10800;0x2a30;STORE_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h 10801;0x2a31;MSG_QUEUE_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h 10802;0x2a32;SERIALIZATION_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h +10803;0x2a33;FILESTORE_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h +10804;0x2a34;FILENAME_TOO_LARGE_ERROR;LOW;P1: Transaction step ID, P2: 0 for source file name, 1 for dest file name;fsfw/src/fsfw/cfdp/handler/defs.h 11200;0x2bc0;SAFE_RATE_VIOLATION;MEDIUM;No description;mission/acsDefs.h 11201;0x2bc1;SAFE_RATE_RECOVERY;MEDIUM;No description;mission/acsDefs.h 11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;No description;mission/acsDefs.h diff --git a/generators/bsp_hosted_returnvalues.csv b/generators/bsp_hosted_returnvalues.csv index 4c6d37e8..86c3f699 100644 --- a/generators/bsp_hosted_returnvalues.csv +++ b/generators/bsp_hosted_returnvalues.csv @@ -49,16 +49,17 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x4fa4;HEATER_MainSwitchSetTimeout;No description;164;HEATER_HANDLER;mission/devices/HeaterHandler.h 0x4fa5;HEATER_CommandAlreadyWaiting;No description;165;HEATER_HANDLER;mission/devices/HeaterHandler.h 0x60a0;CCSDS_CommandNotImplemented;Received action message with unknown action id;160;CCSDS_HANDLER;mission/tmtc/CcsdsIpCoreHandler.h -0x6a01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h -0x6b01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h -0x6c01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h -0x6902;ACSKAL_KalmanUninitialized;No description;2;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6903;ACSKAL_KalmanNoGyrData;No description;3;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6904;ACSKAL_KalmanNoModelVectors;No description;4;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6905;ACSKAL_KalmanNoSusMgmStrData;No description;5;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6906;ACSKAL_KalmanCovarianceInversionFailed;No description;6;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6907;ACSKAL_KalmanInitialized;No description;7;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6908;ACSKAL_KalmanRunning;No description;8;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6b01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h +0x6c01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h +0x6d01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h +0x6a02;ACSMEKF_MekfUninitialized;No description;2;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6a03;ACSMEKF_MekfNoGyrData;No description;3;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6a04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6a05;ACSMEKF_MekfNoSusMgmStrData;No description;5;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6a06;ACSMEKF_MekfCovarianceInversionFailed;No description;6;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6a07;ACSMEKF_MekfInitialized;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6a08;ACSMEKF_MekfRunning;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6900;ACSCTRL_FileDeletionFailed;No description;0;ACS_CTRL;mission/controller/AcsController.h 0x4500;HSPI_OpeningFileFailed;No description;0;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h 0x4501;HSPI_FullDuplexTransferFailed;No description;1;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h 0x4502;HSPI_HalfDuplexTransferFailed;No description;2;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index 432e0828..6d26ed51 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -84,6 +84,8 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 10800;0x2a30;STORE_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h 10801;0x2a31;MSG_QUEUE_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h 10802;0x2a32;SERIALIZATION_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h +10803;0x2a33;FILESTORE_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h +10804;0x2a34;FILENAME_TOO_LARGE_ERROR;LOW;P1: Transaction step ID, P2: 0 for source file name, 1 for dest file name;fsfw/src/fsfw/cfdp/handler/defs.h 11200;0x2bc0;SAFE_RATE_VIOLATION;MEDIUM;No description;mission/acsDefs.h 11201;0x2bc1;SAFE_RATE_RECOVERY;MEDIUM;No description;mission/acsDefs.h 11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;No description;mission/acsDefs.h diff --git a/generators/bsp_q7s_returnvalues.csv b/generators/bsp_q7s_returnvalues.csv index 6f215a94..4abc2ff7 100644 --- a/generators/bsp_q7s_returnvalues.csv +++ b/generators/bsp_q7s_returnvalues.csv @@ -49,16 +49,17 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x4fa4;HEATER_MainSwitchSetTimeout;No description;164;HEATER_HANDLER;mission/devices/HeaterHandler.h 0x4fa5;HEATER_CommandAlreadyWaiting;No description;165;HEATER_HANDLER;mission/devices/HeaterHandler.h 0x60a0;CCSDS_CommandNotImplemented;Received action message with unknown action id;160;CCSDS_HANDLER;mission/tmtc/CcsdsIpCoreHandler.h -0x6a01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h -0x6b01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h -0x6c01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h -0x6902;ACSKAL_KalmanUninitialized;No description;2;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6903;ACSKAL_KalmanNoGyrData;No description;3;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6904;ACSKAL_KalmanNoModelVectors;No description;4;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6905;ACSKAL_KalmanNoSusMgmStrData;No description;5;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6906;ACSKAL_KalmanCovarianceInversionFailed;No description;6;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6907;ACSKAL_KalmanInitialized;No description;7;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6908;ACSKAL_KalmanRunning;No description;8;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6b01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h +0x6c01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h +0x6d01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h +0x6a02;ACSMEKF_MekfUninitialized;No description;2;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6a03;ACSMEKF_MekfNoGyrData;No description;3;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6a04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6a05;ACSMEKF_MekfNoSusMgmStrData;No description;5;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6a06;ACSMEKF_MekfCovarianceInversionFailed;No description;6;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6a07;ACSMEKF_MekfInitialized;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6a08;ACSMEKF_MekfRunning;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6900;ACSCTRL_FileDeletionFailed;No description;0;ACS_CTRL;mission/controller/AcsController.h 0x4500;HSPI_OpeningFileFailed;No description;0;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h 0x4501;HSPI_FullDuplexTransferFailed;No description;1;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h 0x4502;HSPI_HalfDuplexTransferFailed;No description;2;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index f725027c..c979ff5e 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 261 translations. + * @brief Auto-generated event translation file. Contains 263 translations. * @details - * Generated on: 2023-02-23 15:59:16 + * Generated on: 2023-02-24 16:57:00 */ #include "translateEvents.h" @@ -90,6 +90,8 @@ const char *CHANGE_OF_SETUP_PARAMETER_STRING = "CHANGE_OF_SETUP_PARAMETER"; const char *STORE_ERROR_STRING = "STORE_ERROR"; const char *MSG_QUEUE_ERROR_STRING = "MSG_QUEUE_ERROR"; const char *SERIALIZATION_ERROR_STRING = "SERIALIZATION_ERROR"; +const char *FILESTORE_ERROR_STRING = "FILESTORE_ERROR"; +const char *FILENAME_TOO_LARGE_ERROR_STRING = "FILENAME_TOO_LARGE_ERROR"; const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION"; const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY"; const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID"; @@ -434,6 +436,10 @@ const char *translateEvents(Event event) { return MSG_QUEUE_ERROR_STRING; case (10802): return SERIALIZATION_ERROR_STRING; + case (10803): + return FILESTORE_ERROR_STRING; + case (10804): + return FILENAME_TOO_LARGE_ERROR_STRING; case (11200): return SAFE_RATE_VIOLATION_STRING; case (11201): diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index b2bf7f9b..93c4fb0f 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 159 translations. - * Generated on: 2023-02-23 15:59:16 + * Generated on: 2023-02-24 16:57:00 */ #include "translateObjects.h" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index f725027c..c979ff5e 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 261 translations. + * @brief Auto-generated event translation file. Contains 263 translations. * @details - * Generated on: 2023-02-23 15:59:16 + * Generated on: 2023-02-24 16:57:00 */ #include "translateEvents.h" @@ -90,6 +90,8 @@ const char *CHANGE_OF_SETUP_PARAMETER_STRING = "CHANGE_OF_SETUP_PARAMETER"; const char *STORE_ERROR_STRING = "STORE_ERROR"; const char *MSG_QUEUE_ERROR_STRING = "MSG_QUEUE_ERROR"; const char *SERIALIZATION_ERROR_STRING = "SERIALIZATION_ERROR"; +const char *FILESTORE_ERROR_STRING = "FILESTORE_ERROR"; +const char *FILENAME_TOO_LARGE_ERROR_STRING = "FILENAME_TOO_LARGE_ERROR"; const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION"; const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY"; const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID"; @@ -434,6 +436,10 @@ const char *translateEvents(Event event) { return MSG_QUEUE_ERROR_STRING; case (10802): return SERIALIZATION_ERROR_STRING; + case (10803): + return FILESTORE_ERROR_STRING; + case (10804): + return FILENAME_TOO_LARGE_ERROR_STRING; case (11200): return SAFE_RATE_VIOLATION_STRING; case (11201): diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index b2bf7f9b..93c4fb0f 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 159 translations. - * Generated on: 2023-02-23 15:59:16 + * Generated on: 2023-02-24 16:57:00 */ #include "translateObjects.h" diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 6b821a08..f3269285 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -1,9 +1,8 @@ #include "AcsController.h" #include - -#include "mission/acsDefs.h" -#include "mission/config/torquer.h" +#include +#include AcsController::AcsController(object_id_t objectId) : ExtendedControllerBase(objectId), @@ -46,6 +45,26 @@ ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { return result; } +ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, + const uint8_t *data, size_t size) { + switch (actionId) { + case SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL: { + ReturnValue_t result = guidance.solarArrayDeploymentComplete(); + if (result == returnvalue::FAILED) { + return FILE_DELETION_FAILED; + } + return HasActionsIF::EXECUTION_FINISHED; + } + case RESET_MEKF: { + navigation.resetMekf(&mekfData); + return HasActionsIF::EXECUTION_FINISHED; + } + default: { + return HasActionsIF::INVALID_ACTION_ID; + } + } +} + MessageQueueId_t AcsController::getCommandQueue() const { return commandQueue->getId(); } ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId, @@ -60,6 +79,25 @@ void AcsController::performControlOperation() { #if OBSW_THREAD_TRACING == 1 trace::threadTrace(opCounter, "ACS & TCS PST"); #endif + { + PoolReadGuard pg(&mgmDataRaw); + if (pg.getReadResult() == returnvalue::OK) { + copyMgmData(); + } + } + { + PoolReadGuard pg(&susDataRaw); + if (pg.getReadResult() == returnvalue::OK) { + copySusData(); + } + } + { + PoolReadGuard pg(&gyrDataRaw); + if (pg.getReadResult() == returnvalue::OK) { + copyGyrData(); + } + } + switch (internalState) { case InternalState::STARTUP: { initialCountdown.resetTimer(); @@ -95,25 +133,6 @@ void AcsController::performControlOperation() { default: break; } - - { - PoolReadGuard pg(&mgmDataRaw); - if (pg.getReadResult() == returnvalue::OK) { - copyMgmData(); - } - } - { - PoolReadGuard pg(&susDataRaw); - if (pg.getReadResult() == returnvalue::OK) { - copySusData(); - } - } - { - PoolReadGuard pg(&gyrDataRaw); - if (pg.getReadResult() == returnvalue::OK) { - copyGyrData(); - } - } } void AcsController::performSafe() { @@ -124,8 +143,8 @@ void AcsController::performSafe() { &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData); - if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && - result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { + if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && + result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { triggerEvent(acs::MEKF_INVALID_INFO); mekfInvalidFlag = true; @@ -139,7 +158,7 @@ void AcsController::performSafe() { // if MEKF is working double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; bool magMomMtqValid = false; - if (result == MultiplicativeKalmanFilter::KALMAN_RUNNING) { + if (result == MultiplicativeKalmanFilter::MEKF_RUNNING) { safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(), susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), @@ -189,8 +208,8 @@ void AcsController::performDetumble() { &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData); - if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && - result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { + if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && + result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { triggerEvent(acs::MEKF_INVALID_INFO); mekfInvalidFlag = true; @@ -236,8 +255,8 @@ void AcsController::performPointingCtrl() { &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData); - if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING && - result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) { + if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && + result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { triggerEvent(acs::MEKF_INVALID_INFO); mekfInvalidFlag = true; @@ -246,9 +265,10 @@ void AcsController::performPointingCtrl() { triggerEvent(acs::MEKF_INVALID_MODE_VIOLATION); } mekfInvalidCounter++; - commandActuators(0, 0, 0, acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], - cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], - acsParameters.rwHandlingParameters.rampTime); + // commandActuators(0, 0, 0, acsParameters.magnetorquesParameter.torqueDuration, + // cmdSpeedRws[0], + // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], + // acsParameters.rwHandlingParameters.rampTime); return; } else { mekfInvalidFlag = false; @@ -395,7 +415,7 @@ void AcsController::performPointingCtrl() { sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws); actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs); - updateCtrlValData(targetQuat, errorQuat, errorAngle); + updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], @@ -457,16 +477,20 @@ void AcsController::updateCtrlValData(double errAng) { ctrlValData.errQuat.setValid(false); ctrlValData.errAng.value = errAng; ctrlValData.errAng.setValid(true); + std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC, 3 * sizeof(double)); + ctrlValData.tgtRotRate.setValid(false); ctrlValData.setValidity(true, false); } } -void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng) { +void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng, + const double *tgtRotRate) { PoolReadGuard pg(&ctrlValData); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double)); std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double)); ctrlValData.errAng.value = errAng; + std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double)); ctrlValData.setValidity(true, true); } } @@ -477,6 +501,7 @@ void AcsController::disableCtrlValData() { std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double)); std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double)); ctrlValData.errAng.value = 0; + std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC, 3 * sizeof(double)); ctrlValData.setValidity(false, true); } } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 1b57a32a..6d31ae49 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -1,12 +1,17 @@ #ifndef MISSION_CONTROLLER_ACSCONTROLLER_H_ #define MISSION_CONTROLLER_ACSCONTROLLER_H_ +#include #include #include #include #include +#include +#include +#include #include #include +#include #include "acs/ActuatorCmd.h" #include "acs/Guidance.h" @@ -17,11 +22,6 @@ #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/trace.h" class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF { public: @@ -41,6 +41,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes 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}; @@ -58,7 +59,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes uint8_t detumbleCounter = 0; uint8_t multipleRwUnavailableCounter = 0; - bool mekfInvalidFlag = true; + bool mekfInvalidFlag = false; uint8_t mekfInvalidCounter = 0; int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; int16_t cmdDipolMtqs[3] = {0, 0, 0}; @@ -68,13 +69,23 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes #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; @@ -92,7 +103,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes 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); + void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng, + const double* tgtRotRate); void disableCtrlValData(); /* ACS Sensor Values */ diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index f1b538c8..11de10a3 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -1,8 +1,3 @@ -/******************************* - * EIVE Flight Software Framework (FSFW) - * (c) 2022 IRS, Uni Stuttgart - *******************************/ - #ifndef ACSPARAMETERS_H_ #define ACSPARAMETERS_H_ diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 031cd384..09f0be20 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -551,3 +551,19 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3 std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef, 3 * sizeof(double)); } + +ReturnValue_t Guidance::solarArrayDeploymentComplete() { + if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE)) { + std::remove(SD_0_SKEWED_PTG_FILE); + if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE)) { + return returnvalue::FAILED; + } + } + if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { + std::remove(SD_1_SKEWED_PTG_FILE); + if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { + return returnvalue::FAILED; + } + } + return returnvalue::OK; +} diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 65c9aa12..da9d429b 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -13,6 +13,7 @@ class Guidance { virtual ~Guidance(); void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]); + ReturnValue_t solarArrayDeploymentComplete(); // Function to get the target quaternion and refence rotation rate from gps position and // position of the ground station diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 5bae4624..a700c6a6 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -189,12 +189,12 @@ ReturnValue_t MultiplicativeKalmanFilter::init( initialCovarianceMatrix[5][4] = initGyroCov[2][1]; initialCovarianceMatrix[5][5] = initGyroCov[2][2]; updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED); - return KALMAN_INITIALIZED; + return MEKF_INITIALIZED; } else { // no initialisation possible, no valid measurements validInit = false; updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); - return KALMAN_UNINITIALIZED; + return MEKF_UNINITIALIZED; } } @@ -211,12 +211,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c int MDF = 0; // Matrix Dimension Factor if (!validGYRs_) { updateDataSetWithoutData(mekfData, MekfStatus::NO_GYR_DATA); - return KALMAN_NO_GYR_DATA; + return MEKF_NO_GYR_DATA; } // Check for Model Calculations else if (!validSSModel || !validMagModel) { updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS); - return KALMAN_NO_MODEL_VECTORS; + return MEKF_NO_MODEL_VECTORS; } // Check Measurements available from SS, MAG, STR if (validSS && validMagField_ && validSTR_) { @@ -854,7 +854,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c int inversionFailed = MathOperations::inverseMatrix(*residualCov, *invResidualCov, MDF); if (inversionFailed) { updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED); - return KALMAN_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed + return MEKF_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed } // [K = P * H' / (H * P * H' + R)] @@ -1085,16 +1085,17 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c MatrixOperations::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst); - return KALMAN_RUNNING; + return MEKF_RUNNING; } -void MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) { +ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) { double resetQuaternion[4] = {0, 0, 0, 1}; double resetCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double)); std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double)); updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); + return MEKF_UNINITIALIZED; } void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData, diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index dd02cf9f..47e1f807 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -1,17 +1,3 @@ -/* - * MultiplicativeKalmanFilter.h - * - * Created on: 4 Feb 2022 - * Author: Robin Marquardt - * - * @brief: This class handles the calculation of an estimated quaternion and the gyro bias by - * means of the spacecraft attitude sensors - * - * @note: A description of the used algorithms can be found in the bachelor thesis of Robin - * Marquardt - * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=500811 - */ - #ifndef MULTIPLICATIVEKALMANFILTER_H_ #define MULTIPLICATIVEKALMANFILTER_H_ @@ -22,6 +8,13 @@ #include "eive/resultClassIds.h" class MultiplicativeKalmanFilter { + /* @brief: This class handles the calculation of an estimated quaternion and the gyro bias by + * means of the spacecraft attitude sensors + * + * @note: A description of the used algorithms can be found in the bachelor thesis of Robin + * Marquardt + * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=500811 + */ public: /* @brief: Constructor * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters @@ -29,7 +22,7 @@ class MultiplicativeKalmanFilter { MultiplicativeKalmanFilter(AcsParameters *acsParameters_); virtual ~MultiplicativeKalmanFilter(); - void reset(acsctrl::MekfData *mekfData); + ReturnValue_t reset(acsctrl::MekfData *mekfData); /* @brief: init() - This function initializes the Kalman Filter and will provide the first * quaternion through the QUEST algorithm @@ -74,15 +67,15 @@ class MultiplicativeKalmanFilter { }; // resetting Mekf - static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN; - static constexpr ReturnValue_t KALMAN_UNINITIALIZED = returnvalue::makeCode(IF_KAL_ID, 2); - static constexpr ReturnValue_t KALMAN_NO_GYR_DATA = returnvalue::makeCode(IF_KAL_ID, 3); - static constexpr ReturnValue_t KALMAN_NO_MODEL_VECTORS = returnvalue::makeCode(IF_KAL_ID, 4); - static constexpr ReturnValue_t KALMAN_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_KAL_ID, 5); - static constexpr ReturnValue_t KALMAN_COVARIANCE_INVERSION_FAILED = - returnvalue::makeCode(IF_KAL_ID, 6); - static constexpr ReturnValue_t KALMAN_INITIALIZED = returnvalue::makeCode(IF_KAL_ID, 7); - static constexpr ReturnValue_t KALMAN_RUNNING = returnvalue::makeCode(IF_KAL_ID, 8); + static constexpr uint8_t IF_MEKF_ID = CLASS_ID::ACS_MEKF; + static constexpr ReturnValue_t MEKF_UNINITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 2); + static constexpr ReturnValue_t MEKF_NO_GYR_DATA = returnvalue::makeCode(IF_MEKF_ID, 3); + static constexpr ReturnValue_t MEKF_NO_MODEL_VECTORS = returnvalue::makeCode(IF_MEKF_ID, 4); + static constexpr ReturnValue_t MEKF_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_MEKF_ID, 5); + static constexpr ReturnValue_t MEKF_COVARIANCE_INVERSION_FAILED = + returnvalue::makeCode(IF_MEKF_ID, 6); + static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7); + static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8); private: /*Parameters*/ diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index c6310302..03446609 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -25,26 +25,25 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, sensorValues->strSet.caliQy.isValid() && sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid(); - if (kalmanInit) { - return multiplicativeKalmanFilter.mekfEst( + if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) { + mekfStatus = multiplicativeKalmanFilter.init( + mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), + susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), + susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), + mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData); + return mekfStatus; + } else { + mekfStatus = multiplicativeKalmanFilter.mekfEst( quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value, gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData); - } else { - ReturnValue_t result; - result = multiplicativeKalmanFilter.init( - mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), - susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), - susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), - mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData); - kalmanInit = true; - return result; + return mekfStatus; } } void Navigation::resetMekf(acsctrl::MekfData *mekfData) { - multiplicativeKalmanFilter.reset(mekfData); + mekfStatus = multiplicativeKalmanFilter.reset(mekfData); } diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index e054bfbf..cf9e81e3 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -22,7 +22,7 @@ class Navigation { private: MultiplicativeKalmanFilter multiplicativeKalmanFilter; AcsParameters acsParameters; - bool kalmanInit = false; + ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::MEKF_UNINITIALIZED; }; #endif /* ACS_NAVIGATION_H_ */ diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 6fa1ab8c..cdd29d8b 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -1,7 +1,3 @@ -/******************************* - * EIVE Flight Software - * (c) 2022 IRS, Uni Stuttgart - *******************************/ #ifndef SENSORPROCESSING_H_ #define SENSORPROCESSING_H_ diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index e4026300..85815ae5 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -1,9 +1,3 @@ -/* - * SensorValues.cpp - * - * Created on: 30 Mar 2022 - * Author: rooob - */ #include "SensorValues.h" #include diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index f56ad32e..3856789a 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -1,17 +1,9 @@ -/* - * SusConverter.cpp - * - * Created on: 17.01.2022 - * Author: Timon Schwarz - */ - #include "SusConverter.h" #include #include #include -#include //for atan2 - +#include #include bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) { diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index b3829827..046b0ca8 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -1,10 +1,3 @@ -/* - * SusConverter.h - * - * Created on: Sep 22, 2022 - * Author: marius - */ - #ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ #define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ @@ -28,8 +21,6 @@ class SusConverter { private: float alphaBetaRaw[2]; //[°] - // float coeffAlpha[9][10]; - // float coeffBeta[9][10]; float alphaBetaCalibrated[2]; //[°] float sunVectorSensorFrame[3]; //[-] diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index b8d1fa4d..f8537740 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -300,6 +300,7 @@ class MathOperations { } static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) { + /* do not use this. takes 300ms */ float det = 0; T1 matrix[size][size], submatrix[size - 1][size - 1]; for (uint8_t row = 0; row < size; row++) { @@ -329,9 +330,7 @@ class MathOperations { } static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) { - if (MathOperations::matrixDeterminant(inputMatrix, size) == 0) { - return 1; // Matrix is singular and not invertible - } + // Stopwatch stopwatch; T1 matrix[size][size], identity[size][size]; // reformat array to matrix for (uint8_t row = 0; row < size; row++) { @@ -346,7 +345,6 @@ class MathOperations { } // gauss-jordan algo // sort matrix such as no diag entry shall be 0 - // should not be needed as such a matrix has a det=0 for (uint8_t row = 0; row < size; row++) { if (matrix[row][row] == 0.0) { bool swaped = false; diff --git a/mission/devices/devicedefinitions/GPSDefinitions.h b/mission/devices/devicedefinitions/GPSDefinitions.h index 8acb77e9..80e2861a 100644 --- a/mission/devices/devicedefinitions/GPSDefinitions.h +++ b/mission/devices/devicedefinitions/GPSDefinitions.h @@ -67,6 +67,7 @@ class GpsPrimaryDataset : public StaticLocalDataSet<18> { private: friend class GpsHyperionLinuxController; + friend class GpsCtrlDummy; GpsPrimaryDataset(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, GpsHyperion::DATASET_ID) {} }; diff --git a/tmtc b/tmtc index da960b53..d47e9039 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit da960b534475128c09b5d5e68813c243170c333b +Subproject commit d47e9039a5286cd7bbe67679f09f095863911b7e