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

Merge branch 'develop' into eggert/acs
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-02-07 10:08:20 +01:00
parent b9ed485d60 5f81b0866a
commit 6567fd1023
35 changed files with 626 additions and 338 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
mission/controller/AcsController.cpp
View File

@@ -2,6 +2,7 @@
#include <fsfw/datapool/PoolReadGuard.h>
#include "mission/acsDefs.h"
#include "mission/config/torquer.h"
AcsController::AcsController(object_id_t objectId)
@@ -64,17 +65,17 @@ void AcsController::performControlOperation() {
case InternalState::READY: {
if (mode != MODE_OFF) {
switch (submode) {
case SUBMODE_SAFE:
case acs::SAFE:
performSafe();
break;
case SUBMODE_DETUMBLE:
case acs::DETUMBLE:
performDetumble();
break;
case SUBMODE_IDLE:
case SUBMODE_PTG_TARGET:
case SUBMODE_PTG_TARGET_GS:
case SUBMODE_PTG_NADIR:
case SUBMODE_PTG_INERTIAL:
case acs::IDLE:
case acs::PTG_TARGET:
case acs::PTG_TARGET_GS:
case acs::PTG_TARGET_NADIR:
case acs::PTG_TARGET_INERTIAL:
performPointingCtrl();
break;
}
@@ -168,17 +169,18 @@ void AcsController::performSafe() {
detumbleCounter = 0;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
submode = SUBMODE_DETUMBLE;
detumbleCounter = 0;
triggerEvent(SAFE_RATE_VIOLATION);
// Triggers detubmle mode transition in subsystem
triggerEvent(acs::SAFE_RATE_VIOLATION);
}
{
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(int32_t));
int32_t zeroVec[4] = {0, 0, 0, 0};
std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetTorque.setValid(false);
std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t));
actuatorCmdData.mtqTargetDipole.setValid(true);
@@ -225,8 +227,9 @@ void AcsController::performDetumble() {
detumbleCounter = 0;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
submode = SUBMODE_SAFE;
detumbleCounter = 0;
// Triggers safe mode transition in subsystem
triggerEvent(acs::SAFE_RATE_RECOVERY);
}
int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
@@ -436,8 +439,8 @@ void AcsController::performPointingCtrl() {
// }
}
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) {
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
// MGM Raw
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1VecRaw);
@@ -523,7 +526,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool& localD
return returnvalue::OK;
}
LocalPoolDataSetBase* AcsController::getDataSetHandle(sid_t sid) {
LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
switch (sid.ownerSetId) {
case acsctrl::MGM_SENSOR_DATA:
return &mgmDataRaw;
@@ -552,7 +555,7 @@ LocalPoolDataSetBase* AcsController::getDataSetHandle(sid_t sid) {
}
ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t* msToReachTheMode) {
uint32_t *msToReachTheMode) {
if (mode == MODE_OFF) {
if (submode == SUBMODE_NONE) {
return returnvalue::OK;
@@ -569,6 +572,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() {
ACS::SensorValues sensorValues;
{
@@ -616,7 +623,6 @@ void AcsController::copyMgmData() {
}
void AcsController::copySusData() {
ACS::SensorValues sensorValues;
{
PoolReadGuard pg(&sensorValues.susSets[0]);
if (pg.getReadResult() == returnvalue::OK) {