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

old code seems to work
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Robin Müller
2022-10-20 15:08:33 +02:00
parent ada1111252
commit 29a34256a7
12 changed files with 108 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
mission/devices/ImtqHandler.cpp
View File

@ -25,9 +25,7 @@
#include <cmath>
#include <fsfw/datapoollocal/LocalPoolVariable.tpp>
MutexIF* ImtqHandler::TORQUE_LOCK = nullptr;
bool ImtqHandler::TORQUEING = false;
Countdown ImtqHandler::TORQUE_COUNTDOWN = Countdown();
#include "mission/devices/torquer.h"
ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
power::Switch_t pwrSwitcher)
@ -46,7 +44,6 @@ ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC
if (comCookie == nullptr) {
sif::error << "IMTQHandler: Invalid com cookie" << std::endl;
}
TORQUE_LOCK = MutexFactory::instance()->createMutex();
}
ImtqHandler::~ImtqHandler() {}
@ -62,6 +59,7 @@ void ImtqHandler::doStartUp() {
void ImtqHandler::doShutDown() { setMode(_MODE_POWER_DOWN); }
ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
bool buildCommand = true;
// Depending on the normal polling mode configuration, 3-4 communication steps are recommended
switch (communicationStep) {
case CommunicationStep::GET_ENG_HK_DATA:
@ -99,9 +97,12 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
// If the dipole is not commanded but set by the ACS control algorithm,
// the dipoles will be set by the ACS controller directly using the dipole local pool set.
// This set has a flag to determine whether the ACS controller actually set any new input.
PoolReadGuard pg(&dipoleSet);
if (dipoleSet.newActuation) {
MutexGuard mg(torquer::lazyLock());
if (torquer::NEW_ACTUATION_FLAG) {
*id = IMTQ::START_ACTUATION_DIPOLE;
torquer::NEW_ACTUATION_FLAG = false;
} else {
buildCommand = false;
}
communicationStep = CommunicationStep::GET_ENG_HK_DATA;
break;
@ -111,7 +112,10 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
<< std::endl;
break;
}
return buildCommandFromCommand(*id, nullptr, 0);
if (buildCommand) {
return buildCommandFromCommand(*id, nullptr, 0);
}
return NOTHING_TO_SEND;
}
ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
@ -176,23 +180,27 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
if (commandData != nullptr && commandDataLen < 8) {
return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
}
PoolReadGuard pg(&dipoleSet);
ReturnValue_t result;
// Commands override anything which was set in the software
if (commandBuffer != nullptr) {
dipoleSet.setValidityBufferGeneration(false);
result =
dipoleSet.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::NETWORK);
dipoleSet.setValidityBufferGeneration(true);
if (result != returnvalue::OK) {
return result;
}
} else {
// Read set dipole values from local pool
PoolReadGuard pg(&dipoleSet);
}
result = buildDipoleActuationCommand();
if (result != returnvalue::OK) {
return result;
}
MutexGuard mg(TORQUE_LOCK);
TORQUEING = true;
TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
MutexGuard mg(torquer::lazyLock());
torquer::TORQUEING = true;
torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
return result;
}
case (IMTQ::GET_ENG_HK_DATA): {
@ -249,26 +257,22 @@ ReturnValue_t ImtqHandler::buildDipoleActuationCommand() {
}
void ImtqHandler::fillCommandAndReplyMap() {
this->insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr,
IMTQ::SIZE_SELF_TEST_RESULTS);
this->insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr,
IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset,
IMTQ::SIZE_ENG_HK_DATA_REPLY);
this->insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr,
IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY);
this->insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr,
IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet,
IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT);
this->insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet,
IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT);
insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr, IMTQ::SIZE_SELF_TEST_RESULTS);
insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset, IMTQ::SIZE_ENG_HK_DATA_REPLY);
insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr,
IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY);
insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet,
IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT);
insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet,
IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT);
}
ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSize,
@ -2315,11 +2319,3 @@ ReturnValue_t ImtqHandler::getSwitches(const uint8_t** switches, uint8_t* number
}
return DeviceHandlerBase::NO_SWITCH;
}
bool ImtqHandler::mgtIsTorqueing(dur_millis_t* remainingTorqueDuration) {
MutexGuard mg(TORQUE_LOCK);
if (TORQUEING and remainingTorqueDuration != nullptr) {
*remainingTorqueDuration = TORQUE_COUNTDOWN.getRemainingMillis() + TORQUE_BUFFER_TIME_MS;
}
return TORQUEING;
}