eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

First Version of ACS Controller #329

Merged
muellerr merged 106 commits from acs-ctrl-v1 into develop 2022-12-02 16:21:58 +01:00
Conversation 0 Commits 106 Files Changed 50 +92 -21
8 changed files with 92 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 9d0e10eb1e - Show all commits

19
bsp_q7s/core/InitMission.cpp
View File

@ -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);

2
dummies/ImtqDummy.cpp
View File

@ -41,5 +41,7 @@ ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataP
localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry<float>({0.0,0.0,0.0}));
localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry<float>({0.0,0.0,0.0}));
localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>({0}));
return returnvalue::OK;
}

2
dummies/MgmLIS3MDLDummy.cpp
View File

@ -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<float>({0.0}));
localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS, new PoolEntry<float>({0.0,0.0,0.0}));
localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS, new PoolEntry<float>({0.0, 0.0, 0.0}));
return returnvalue::OK;
}

3
dummies/SusDummy.cpp
View File

@ -54,7 +54,8 @@ void SusDummy::performControlOperation() {
ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) {
localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry<float>({0}, 1, true));
localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC,
new PoolEntry<uint16_t>({0, 0, 0, 0, 0, 0}));
return returnvalue::OK;
}

61
mission/controller/AcsController.cpp
View File

@ -1,14 +1,17 @@
#include "AcsController.h"
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/timemanager/Clock.h>
//#include <fsfw/timemanager/Clock.h>
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);

9
mission/controller/AcsController.h
View File

@ -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;

15
mission/controller/acs/AcsParameters.cpp
View File

@ -1,18 +1,17 @@
#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include "AcsParameters.h"
#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <stddef.h>
#include <cmath>
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;
}*/

2
mission/controller/acs/AcsParameters.h
View File

@ -11,7 +11,7 @@
typedef unsigned char uint8_t;
class AcsParameters/*:public HasParametersIF*/{
class AcsParameters /*: public HasParametersIF*/{
public:
AcsParameters();