diff --git a/CHANGELOG.md b/CHANGELOG.md index b88823b5..5ffdcbc8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,10 @@ list yields a list of all related PRs for each release. # [unreleased] +# [v1.13.0] + +- Added first version of ACS Controller with gathers MGM data in a set + # [v1.12.1] 05.07.2022 - Disable periodic TCS controller HK generation by default diff --git a/bsp_q7s/core/InitMission.cpp b/bsp_q7s/core/InitMission.cpp index f2090666..80f5fc9e 100644 --- a/bsp_q7s/core/InitMission.cpp +++ b/bsp_q7s/core/InitMission.cpp @@ -133,6 +133,11 @@ void initmission::initTasks() { } #endif /* OBSW_ADD_ACS_HANDLERS */ + acsTask->addComponent(objects::ACS_CONTROLLER); + if (result != HasReturnvaluesIF::RETURN_OK) { + initmission::printAddObjectError("ACS_CTRL", objects::ACS_CONTROLLER); + } + PeriodicTaskIF* sysTask = factory->createPeriodicTask( "SYS_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); static_cast(sysTask); diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 56c0a62a..acce5dfb 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -63,4 +63,5 @@ void ObjectFactory::produce(void* args) { createMiscComponents(); createThermalController(); + createAcsController(); } diff --git a/common/config/commonObjects.h b/common/config/commonObjects.h index 4a4594f4..e0efbce7 100644 --- a/common/config/commonObjects.h +++ b/common/config/commonObjects.h @@ -18,7 +18,7 @@ enum commonObjects : uint32_t { /* 0x43 ('C') for Controllers */ THERMAL_CONTROLLER = 0x43400001, - ACS_CONTROLLER = 0x43100002, + ACS_CONTROLLER = 0x43000002, CORE_CONTROLLER = 0x43000003, /* 0x44 ('D') for device handlers */ diff --git a/fsfw b/fsfw index fdcfd89e..d9759581 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit fdcfd89ed271118ddb2daf0de3db5d69bdbc95d4 +Subproject commit d975958120bd151dabdc450a104e9cc8069ff509 diff --git a/linux/ObjectFactory.cpp b/linux/ObjectFactory.cpp index e8ba7947..494b0baa 100644 --- a/linux/ObjectFactory.cpp +++ b/linux/ObjectFactory.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -324,6 +325,8 @@ void ObjectFactory::createThermalController() { new ThermalController(objects::THERMAL_CONTROLLER, objects::NO_OBJECT); } +void ObjectFactory::createAcsController() { new AcsController(objects::ACS_CONTROLLER); } + void ObjectFactory::gpioChecker(ReturnValue_t result, std::string output) { if (result != HasReturnvaluesIF::RETURN_OK) { sif::error << "ObjectFactory: Adding GPIOs failed for " << output << std::endl; diff --git a/linux/ObjectFactory.h b/linux/ObjectFactory.h index a592e818..c88a9eb4 100644 --- a/linux/ObjectFactory.h +++ b/linux/ObjectFactory.h @@ -1,7 +1,7 @@ #pragma once -#include #include +#include #include @@ -19,5 +19,6 @@ void createRtdComponents(std::string spiDev, GpioIF* gpioComIF, PowerSwitchIF* p void gpioChecker(ReturnValue_t result, std::string output); void createThermalController(); +void createAcsController(); } // namespace ObjectFactory diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp new file mode 100644 index 00000000..4aea53a0 --- /dev/null +++ b/mission/controller/AcsController.cpp @@ -0,0 +1,96 @@ +#include +#include "AcsController.h" + +AcsController::AcsController(object_id_t objectId) + : ExtendedControllerBase(objectId, objects::NO_OBJECT), + mgmData(this) {} + +ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { + return HasReturnvaluesIF::RETURN_OK; +} + +void AcsController::performControlOperation() { + switch (internalState) { + case InternalState::STARTUP: { + initialCountdown.resetTimer(); + internalState = InternalState::INITIAL_DELAY; + return; + } + case InternalState::INITIAL_DELAY: { + if (initialCountdown.hasTimedOut()) { + internalState = InternalState::READY; + } + return; + } + case InternalState::READY: { + break; + } + default: + break; + } + + { + PoolReadGuard pg(&mgmData); + if (pg.getReadResult() == RETURN_OK) { + copyMgmData(); + } + } +} + +ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmPoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus); + poolManager.subscribeForPeriodicPacket(mgmData.getSid(), false, 5.0, false); + return HasReturnvaluesIF::RETURN_OK; +} + +LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { + if (sid == mgmData.getSid()) { + return &mgmData; + } + return nullptr; +} + +ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t *msToReachTheMode) { + return HasReturnvaluesIF::RETURN_OK; +} + +void AcsController::copyMgmData() { + { + PoolReadGuard pg(&mgm0Lis3Set); + if(pg.getReadResult() == RETURN_OK) { + std::memcpy(mgmData.mgm0Lis3.value, mgm0Lis3Set.fieldStrengths.value, 3 * sizeof(float)); + } + } + { + PoolReadGuard pg(&mgm1Rm3100Set); + if(pg.getReadResult() == RETURN_OK) { + std::memcpy(mgmData.mgm1Rm3100.value, mgm1Rm3100Set.fieldStrengths.value, 3 * sizeof(float)); + } + } + { + PoolReadGuard pg(&mgm2Lis3Set); + if(pg.getReadResult() == RETURN_OK) { + std::memcpy(mgmData.mgm2Lis3.value, mgm2Lis3Set.fieldStrengths.value, 3 * sizeof(float)); + } + } + { + PoolReadGuard pg(&mgm3Rm3100Set); + if(pg.getReadResult() == RETURN_OK) { + std::memcpy(mgmData.mgm3Rm3100.value, mgm3Rm3100Set.fieldStrengths.value, 3 * sizeof(float)); + } + } + { + PoolReadGuard pg(&imtqMgmSet); + if(pg.getReadResult() == RETURN_OK) { + std::memcpy(mgmData.imtqCal.value, imtqMgmSet.mgmXyz.value, 3 * sizeof(int32_t)); + mgmData.actuationCalStatus.value = imtqMgmSet.coilActuationStatus.value; + } + } +} diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h new file mode 100644 index 00000000..8dbe38aa --- /dev/null +++ b/mission/controller/AcsController.h @@ -0,0 +1,55 @@ +#ifndef MISSION_CONTROLLER_ACSCONTROLLER_H_ +#define MISSION_CONTROLLER_ACSCONTROLLER_H_ + +#include +#include "controllerdefinitions/AcsCtrlDefinitions.h" +#include +#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" +#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" +#include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h" + +class AcsController : public ExtendedControllerBase { + public: + static constexpr dur_millis_t INIT_DELAY = 500; + + AcsController(object_id_t objectId); + + private: + 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; + + // MGMs + acsctrl::MgmData 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::CalibratedMtmMeasurementSet imtqMgmSet = IMTQ::CalibratedMtmMeasurementSet(objects::IMTQ_HANDLER); + + PoolEntry mgm0PoolVec = PoolEntry(3); + PoolEntry mgm1PoolVec = PoolEntry(3); + PoolEntry mgm2PoolVec = PoolEntry(3); + PoolEntry mgm3PoolVec = PoolEntry(3); + PoolEntry imtqMgmPoolVec = PoolEntry(3); + PoolEntry imtqCalActStatus = PoolEntry(); + void copyMgmData(); + + // Initial delay to make sure all pool variables have been initialized their owners + Countdown initialCountdown = Countdown(INIT_DELAY); + // Sun Sensors +}; + +#endif /* MISSION_CONTROLLER_ACSCONTROLLER_H_ */ diff --git a/mission/controller/CMakeLists.txt b/mission/controller/CMakeLists.txt index 816a560a..84ff7f9e 100644 --- a/mission/controller/CMakeLists.txt +++ b/mission/controller/CMakeLists.txt @@ -1,3 +1,4 @@ if(TGT_BSP MATCHES "arm/q7s" OR TGT_BSP MATCHES "") - target_sources(${LIB_EIVE_MISSION} PRIVATE ThermalController.cpp) + target_sources(${LIB_EIVE_MISSION} PRIVATE ThermalController.cpp + AcsController.cpp) endif() diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h new file mode 100644 index 00000000..f2913943 --- /dev/null +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -0,0 +1,48 @@ +#ifndef MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ +#define MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ + +#include +#include +#include + +namespace acsctrl { + +enum SetIds : uint32_t { + MGM_SENSOR_DATA +}; + +enum PoolIds : lp_id_t { + MGM_0_LIS3_UT, + MGM_1_RM3100_UT, + MGM_2_LIS3_UT, + MGM_3_RM3100_UT, + MGM_IMTQ_CAL_NT, + MGM_IMTQ_CAL_ACT_STATUS +}; + + +static constexpr uint8_t MGM_SET_ENTRIES = 10; + +/** + * @brief This dataset can be used to store the collected temperatures of all temperature sensors + */ +class MgmData : public StaticLocalDataSet { +public: + MgmData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MGM_SENSOR_DATA) {} + + // The ACS board measurement are in floating point uT + lp_vec_t mgm0Lis3 = lp_vec_t(sid.objectId, MGM_0_LIS3_UT, this); + lp_vec_t mgm1Rm3100 = lp_vec_t(sid.objectId, MGM_1_RM3100_UT, this); + lp_vec_t mgm2Lis3 = lp_vec_t(sid.objectId, MGM_2_LIS3_UT, this); + lp_vec_t mgm3Rm3100 = lp_vec_t(sid.objectId, MGM_3_RM3100_UT, this); + // The IMTQ measurements are in integer nT + lp_vec_t imtqCal = lp_vec_t(sid.objectId, MGM_IMTQ_CAL_NT, this); + lp_var_t actuationCalStatus = lp_var_t(sid.objectId, + MGM_IMTQ_CAL_ACT_STATUS, this); +private: + +}; + +} + +#endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ */ diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/IMTQHandler.cpp index 994ca84c..ea7a3177 100644 --- a/mission/devices/IMTQHandler.cpp +++ b/mission/devices/IMTQHandler.cpp @@ -331,9 +331,7 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry({0})); /** Entries of calibrated MTM measurement dataset */ - localDataPoolMap.emplace(IMTQ::MTM_CAL_X, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::MTM_CAL_Y, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::MTM_CAL_Z, new PoolEntry({0})); + localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, &mgmCalEntry); localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry({0})); /** Entries of raw MTM measurement dataset */ @@ -749,14 +747,15 @@ void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) { void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) { PoolReadGuard rg(&calMtmMeasurementSet); + calMtmMeasurementSet.setValidity(true, true); int8_t offset = 2; - calMtmMeasurementSet.mtmXnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | + calMtmMeasurementSet.mgmXyz[0] = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; - calMtmMeasurementSet.mtmYnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | + calMtmMeasurementSet.mgmXyz[1] = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; - calMtmMeasurementSet.mtmZnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | + calMtmMeasurementSet.mgmXyz[2] = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; calMtmMeasurementSet.coilActuationStatus = (*(packet + offset + 3) << 24) | @@ -764,11 +763,11 @@ void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) { (*(packet + offset + 1) << 8) | (*(packet + offset)); if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 - sif::info << "IMTQ calibrated MTM measurement X: " << calMtmMeasurementSet.mtmXnT << " nT" + sif::info << "IMTQ calibrated MTM measurement X: " << calMtmMeasurementSet.mgmXyz[0] << " nT" << std::endl; - sif::info << "IMTQ calibrated MTM measurement Y: " << calMtmMeasurementSet.mtmYnT << " nT" + sif::info << "IMTQ calibrated MTM measurement Y: " << calMtmMeasurementSet.mgmXyz[1] << " nT" << std::endl; - sif::info << "IMTQ calibrated MTM measurement Z: " << calMtmMeasurementSet.mtmZnT << " nT" + sif::info << "IMTQ calibrated MTM measurement Z: " << calMtmMeasurementSet.mgmXyz[2] << " nT" << std::endl; sif::info << "IMTQ coil actuation status during MTM measurement: " << (unsigned int)calMtmMeasurementSet.coilActuationStatus.value << std::endl; diff --git a/mission/devices/IMTQHandler.h b/mission/devices/IMTQHandler.h index 1cd5d0f4..481d554c 100644 --- a/mission/devices/IMTQHandler.h +++ b/mission/devices/IMTQHandler.h @@ -99,6 +99,8 @@ class IMTQHandler : public DeviceHandlerBase { IMTQ::PosZSelfTestSet posZselfTestDataset; IMTQ::NegZSelfTestSet negZselfTestDataset; + PoolEntry mgmCalEntry = PoolEntry(3); + power::Switch_t switcher = power::NO_SWITCH; uint8_t commandBuffer[IMTQ::MAX_COMMAND_SIZE]; diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h index 382fa106..e6661f84 100644 --- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h @@ -115,9 +115,7 @@ enum IMTQPoolIds : lp_id_t { COIL_Y_TEMPERATURE, COIL_Z_TEMPERATURE, MCU_TEMPERATURE, - MTM_CAL_X, - MTM_CAL_Y, - MTM_CAL_Z, + MGM_CAL_NT, ACTUATION_CAL_STATUS, MTM_RAW_X, MTM_RAW_Y, @@ -408,9 +406,7 @@ class CalibratedMtmMeasurementSet : public StaticLocalDataSet mtmXnT = lp_var_t(sid.objectId, MTM_CAL_X, this); - lp_var_t mtmYnT = lp_var_t(sid.objectId, MTM_CAL_Y, this); - lp_var_t mtmZnT = lp_var_t(sid.objectId, MTM_CAL_Z, this); + lp_vec_t mgmXyz = lp_vec_t(sid.objectId, MGM_CAL_NT); /** 1 if coils were actuating during measurement otherwise 0 */ lp_var_t coilActuationStatus = lp_var_t(sid.objectId, ACTUATION_CAL_STATUS, this); diff --git a/mission/tmtc/CMakeLists.txt b/mission/tmtc/CMakeLists.txt index a952629a..0a931d0f 100644 --- a/mission/tmtc/CMakeLists.txt +++ b/mission/tmtc/CMakeLists.txt @@ -1,5 +1,2 @@ -target_sources(${LIB_EIVE_MISSION} PRIVATE - CCSDSHandler.cpp - VirtualChannel.cpp - TmFunnel.cpp -) +target_sources(${LIB_EIVE_MISSION} PRIVATE CCSDSHandler.cpp VirtualChannel.cpp + TmFunnel.cpp) diff --git a/mission/utility/CMakeLists.txt b/mission/utility/CMakeLists.txt index e1162ef2..128356bf 100644 --- a/mission/utility/CMakeLists.txt +++ b/mission/utility/CMakeLists.txt @@ -1,5 +1,2 @@ -target_sources(${LIB_EIVE_MISSION} PRIVATE - Timestamp.cpp - ProgressPrinter.cpp - Filenaming.cpp -) +target_sources(${LIB_EIVE_MISSION} PRIVATE Timestamp.cpp ProgressPrinter.cpp + Filenaming.cpp) diff --git a/tmtc b/tmtc index 3fa70080..ca9f85de 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 3fa700804e0592db4e9a5667de2a81323cac5dc7 +Subproject commit ca9f85de1b51e29e3c0cccd527d736083e374f7f