From 9d0e10eb1ed11b6f998a03dd22696af0a6e2c084 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 4 Oct 2022 13:45:13 +0200 Subject: [PATCH] fixed dummies, scheduled dummies as part of acsTask --- bsp_q7s/core/InitMission.cpp | 19 ++++++++ dummies/ImtqDummy.cpp | 2 + dummies/MgmLIS3MDLDummy.cpp | 2 +- dummies/SusDummy.cpp | 3 +- mission/controller/AcsController.cpp | 61 ++++++++++++++++++++---- mission/controller/AcsController.h | 9 +++- mission/controller/acs/AcsParameters.cpp | 15 +++--- mission/controller/acs/AcsParameters.h | 2 +- 8 files changed, 92 insertions(+), 21 deletions(-) diff --git a/bsp_q7s/core/InitMission.cpp b/bsp_q7s/core/InitMission.cpp index e60c2caf..5bac228b 100644 --- a/bsp_q7s/core/InitMission.cpp +++ b/bsp_q7s/core/InitMission.cpp @@ -139,6 +139,25 @@ void initmission::initTasks() { initmission::printAddObjectError("ACS_CTRL", objects::ACS_CONTROLLER); } #endif +#if OBSW_Q7S_EM == 1 + acsTask->addComponent(objects::MGM_0_LIS3_HANDLER); + acsTask->addComponent(objects::MGM_1_RM3100_HANDLER); + acsTask->addComponent(objects::MGM_2_LIS3_HANDLER); + acsTask->addComponent(objects::MGM_3_RM3100_HANDLER); + acsTask->addComponent(objects::IMTQ_HANDLER); + acsTask->addComponent(objects::SUS_0_N_LOC_XFYFZM_PT_XF); + acsTask->addComponent(objects::SUS_6_R_LOC_XFYBZM_PT_XF); + acsTask->addComponent(objects::SUS_1_N_LOC_XBYFZM_PT_XB); + acsTask->addComponent(objects::SUS_7_R_LOC_XBYBZM_PT_XB); + acsTask->addComponent(objects::SUS_2_N_LOC_XFYBZB_PT_YB); + acsTask->addComponent(objects::SUS_8_R_LOC_XBYBZB_PT_YB); + acsTask->addComponent(objects::SUS_3_N_LOC_XFYBZF_PT_YF); + acsTask->addComponent(objects::SUS_9_R_LOC_XBYBZB_PT_YF); + acsTask->addComponent(objects::SUS_4_N_LOC_XMYFZF_PT_ZF); + acsTask->addComponent(objects::SUS_10_N_LOC_XMYBZF_PT_ZF); + acsTask->addComponent(objects::SUS_5_N_LOC_XFYMZB_PT_ZB); + acsTask->addComponent(objects::SUS_11_R_LOC_XBYMZB_PT_ZB); +#endif PeriodicTaskIF* sysTask = factory->createPeriodicTask( "SYS_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); diff --git a/dummies/ImtqDummy.cpp b/dummies/ImtqDummy.cpp index 63d05c1e..8765e978 100644 --- a/dummies/ImtqDummy.cpp +++ b/dummies/ImtqDummy.cpp @@ -41,5 +41,7 @@ ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataP localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry({0.0,0.0,0.0})); localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry({0.0,0.0,0.0})); + localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry({0})); return returnvalue::OK; } diff --git a/dummies/MgmLIS3MDLDummy.cpp b/dummies/MgmLIS3MDLDummy.cpp index 9c521559..b89d74b8 100644 --- a/dummies/MgmLIS3MDLDummy.cpp +++ b/dummies/MgmLIS3MDLDummy.cpp @@ -41,6 +41,6 @@ uint32_t MgmLIS3MDLDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ReturnValue_t MgmLIS3MDLDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS, new PoolEntry({0.0})); - localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS, new PoolEntry({0.0,0.0,0.0})); + localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS, new PoolEntry({0.0, 0.0, 0.0})); return returnvalue::OK; } diff --git a/dummies/SusDummy.cpp b/dummies/SusDummy.cpp index 2a2a1bda..10368a9a 100644 --- a/dummies/SusDummy.cpp +++ b/dummies/SusDummy.cpp @@ -54,7 +54,8 @@ void SusDummy::performControlOperation() { ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry({0}, 1, true)); - localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, new PoolEntry({0})); + localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, + new PoolEntry({0, 0, 0, 0, 0, 0})); return returnvalue::OK; } diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 26f6a1ee..8c92d968 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -1,14 +1,17 @@ #include "AcsController.h" #include -#include +//#include AcsController::AcsController(object_id_t objectId) : ExtendedControllerBase(objectId, objects::NO_OBJECT), + /*sensorProcessing(&acsParameters), + navigation(&acsParameters), + actuatorCmd(&acsParameters), + detumble(&acsParameters), + detumbleCounter{0},*/ mgmData(this), - susData(this), - sensorProcessing(&acsParameters), - detumbleCounter{0} {} + susData(this) {} ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { return returnvalue::OK; @@ -28,13 +31,28 @@ void AcsController::performControlOperation() { return; } case InternalState::READY: { + if (mode != MODE_OFF) { + switch (submode) { + case SUBMODE_SAFE: + // performSafe(); + break; + + case SUBMODE_DETUMBLE: + // performDetumble(); + break; + + case SUBMODE_PTG_GS: + // performPointingCtrl(); + break; + } + } break; } default: break; } - /*{ + { PoolReadGuard pg(&mgmData); if (pg.getReadResult() == returnvalue::OK) { copyMgmData(); @@ -45,11 +63,15 @@ void AcsController::performControlOperation() { if (pg.getReadResult() == returnvalue::OK) { copySusData(); } - }*/ + } +} + +void AcsController::performSafe(){ + } void AcsController::performDetumble() { - ACS::SensorValues sensorValues; + /*ACS::SensorValues sensorValues; ACS::OutputValues outputValues; // sensorValues.read(); @@ -82,17 +104,36 @@ void AcsController::performDetumble() { if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { submode = SUBMODE_SAFE; detumbleCounter = 0; - } + }*/ +} + +void AcsController::performPointingCtrl(){ + } ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + // MGM 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); + // SUS + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11PoolVec); + poolManager.subscribeForRegularPeriodicPacket({mgmData.getSid(), false, 5.0}); return returnvalue::OK; } @@ -124,6 +165,10 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, return INVALID_MODE; } +void AcsController::modeChanged(Mode_t mode, Submode_t submode) {} + +void AcsController::announceMode(bool recursive) {} + void AcsController::copyMgmData() { { PoolReadGuard pg(&mgm0Lis3Set); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 21c77683..054f2b8f 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -27,16 +27,19 @@ class AcsController : public ExtendedControllerBase { static const Submode_t SUBMODE_PTG_NADIR = 5; protected: + + void performSafe(); void performDetumble(); + void performPointingCtrl(); private: - AcsParameters acsParameters; + /*AcsParameters acsParameters; SensorProcessing sensorProcessing; Navigation navigation; ActuatorCmd actuatorCmd; Detumble detumble; - uint8_t detumbleCounter; + uint8_t detumbleCounter;*/ enum class InternalState { STARTUP, INITIAL_DELAY, READY }; @@ -52,6 +55,8 @@ class AcsController : public ExtendedControllerBase { // 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); // MGMs acsctrl::MgmDataRaw mgmData; diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index c22e8ede..0b516c13 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -1,18 +1,17 @@ -#include #include "AcsParameters.h" +#include #include + #include AcsParameters::AcsParameters() {} AcsParameters::~AcsParameters() {} - /* -ReturnValue_t AcsParameters::getParameter(uint8_t domainId, - uint16_t parameterId, ParameterWrapper* parameterWrapper, - const ParameterWrapper *newValues, uint16_t startAtIndex) { - - return 0; - +ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint16_t parameterId, + ParameterWrapper* parameterWrapper, + const ParameterWrapper* newValues, + uint16_t startAtIndex) { + return returnvalue::OK; }*/ diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index cc1fd7b1..39f2c3c6 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -11,7 +11,7 @@ typedef unsigned char uint8_t; -class AcsParameters/*:public HasParametersIF*/{ +class AcsParameters /*: public HasParametersIF*/{ public: AcsParameters();