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

ACS Subsystem Init #228

Merged
muellerr merged 83 commits from mueller/acs-ss-init into develop 2022-11-02 10:34:40 +01:00
Conversation 1 Commits 83 Files Changed 56 +379 -130
12 changed files with 379 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 9b8d2f3097 - Show all commits

5
bsp_q7s/core/ObjectFactory.cpp
View File

@ -49,6 +49,8 @@
#if OBSW_TEST_LIBGPIOD == 1
#include "linux/boardtest/LibgpiodTest.h"
#endif
#include <mission/devices/ImtqHandler.h>
#include <sstream>
#include "fsfw/datapoollocal/LocalDataPoolManager.h"
@ -72,7 +74,6 @@
#include "mission/devices/BpxBatteryHandler.h"
#include "mission/devices/GyroADIS1650XHandler.h"
#include "mission/devices/HeaterHandler.h"
#include "mission/devices/IMTQHandler.h"
#include "mission/devices/Max31865PT1000Handler.h"
#include "mission/devices/P60DockHandler.h"
#include "mission/devices/PCDUHandler.h"
@ -897,7 +898,7 @@ void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher) {
void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher) {
I2cCookie* imtqI2cCookie =
new I2cCookie(addresses::IMTQ, IMTQ::MAX_REPLY_SIZE, q7s::I2C_DEFAULT_DEV);
auto imtqHandler = new IMTQHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie,
auto imtqHandler = new ImtqHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie,
pcdu::Switches::PDU1_CH3_MGT_5V);
imtqHandler->setPowerSwitcher(pwrSwitcher);
imtqHandler->connectModeTreeParent(satsystem::ACS_SUBSYSTEM);

2
dummies/ImtqDummy.cpp
View File

@ -1,6 +1,6 @@
#include "ImtqDummy.h"
#include <mission/devices/devicedefinitions/IMTQHandlerDefinitions.h>
#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
: DeviceHandlerBase(objectId, comif, comCookie) {}

18
linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
View File

@ -430,11 +430,21 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) {
uint32_t length = thisSequence->getPeriodMs();
static_cast<void>(length);
#if OBSW_ADD_MGT == 1
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_READ);
#endif
#if OBSW_ADD_BPX_BATTERY_HANDLER == 1
thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);

11
mission/controller/AcsController.cpp
View File

@ -2,6 +2,8 @@
#include <fsfw/datapool/PoolReadGuard.h>
#include "mission/devices/torquer.h"
AcsController::AcsController(object_id_t objectId)
: ExtendedControllerBase(objectId), mgmData(this) {}
@ -29,6 +31,15 @@ void AcsController::performControlOperation() {
break;
}
{
// TODO: Calculate actuator output
// PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true;
// TODO: Insert correct values here
// dipoleSet.setDipoles(500, 500, 500, 150);
}
{
PoolReadGuard pg(&mgmData);
if (pg.getReadResult() == returnvalue::OK) {

4
mission/controller/AcsController.h
View File

@ -7,8 +7,8 @@
#include "eive/objects.h"
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
class AcsController : public ExtendedControllerBase {
public:
@ -20,7 +20,6 @@ class AcsController : public ExtendedControllerBase {
enum class InternalState { STARTUP, INITIAL_DELAY, READY };
InternalState internalState = InternalState::STARTUP;
ReturnValue_t handleCommandMessage(CommandMessage* message) override;
void performControlOperation() override;
@ -44,6 +43,7 @@ class AcsController : public ExtendedControllerBase {
RM3100::Rm3100PrimaryDataset mgm3Rm3100Set =
RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER);
IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_HANDLER);
IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
PoolEntry<float> mgm0PoolVec = PoolEntry<float>(3);
PoolEntry<float> mgm1PoolVec = PoolEntry<float>(3);

2
mission/controller/ThermalController.cpp
View File

@ -8,9 +8,9 @@
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
#include <mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h>
#include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
#include <mission/devices/devicedefinitions/IMTQHandlerDefinitions.h>
#include <mission/devices/devicedefinitions/RwDefinitions.h>
#include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
#include <mission/devices/devicedefinitions/payloadPcduDefinitions.h>
#include <objects/systemObjectList.h>

5
mission/devices/CMakeLists.txt
View File

@ -11,7 +11,7 @@ target_sources(
SyrlinksHkHandler.cpp
Max31865PT1000Handler.cpp
Max31865EiveHandler.cpp
IMTQHandler.cpp
ImtqHandler.cpp
HeaterHandler.cpp
RadiationSensorHandler.cpp
GyroADIS1650XHandler.cpp
@ -20,6 +20,7 @@ target_sources(
SusHandler.cpp
PayloadPcduHandler.cpp
SolarArrayDeploymentHandler.cpp
ScexDeviceHandler.cpp)
ScexDeviceHandler.cpp
torquer.cpp)
add_subdirectory(devicedefinitions)

255
mission/devices/IMTQHandler.cpp → mission/devices/ImtqHandler.cpp
View File

@ -1,18 +1,41 @@
#include "IMTQHandler.h"
#include <bits/stdint-intn.h>
#include <commonConfig.h>
#include <fsfw/datapool/PoolEntry.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/datapoollocal/LocalDataPoolManager.h>
#include <fsfw/datapoollocal/LocalPoolVariable.h>
#include <fsfw/datapoollocal/ProvidesDataPoolSubscriptionIF.h>
#include <fsfw/datapoollocal/localPoolDefinitions.h>
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <fsfw/ipc/MutexFactory.h>
#include <fsfw/ipc/MutexGuard.h>
#include <fsfw/ipc/messageQueueDefinitions.h>
#include <fsfw/modes/ModeMessage.h>
#include <fsfw/objectmanager/SystemObjectIF.h>
#include <fsfw/power/definitions.h>
#include <fsfw/returnvalues/returnvalue.h>
#include <fsfw/serialize/SerializeAdapter.h>
#include <fsfw/serialize/SerializeIF.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw/timemanager/Countdown.h>
#include <fsfw/timemanager/clockDefinitions.h>
#include <mission/devices/ImtqHandler.h>
#include <cmath>
#include <fsfw/datapoollocal/LocalPoolVariable.tpp>
#include "OBSWConfig.h"
#include "mission/devices/torquer.h"
IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
static constexpr bool ACTUATION_WIRETAPPING = false;
ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
power::Switch_t pwrSwitcher)
: DeviceHandlerBase(objectId, comIF, comCookie),
engHkDataset(this),
calMtmMeasurementSet(this),
rawMtmMeasurementSet(this),
dipoleSet(*this),
posXselfTestDataset(this),
negXselfTestDataset(this),
posYselfTestDataset(this),
@ -25,9 +48,9 @@ IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC
}
}
IMTQHandler::~IMTQHandler() {}
ImtqHandler::~ImtqHandler() = default;
void IMTQHandler::doStartUp() {
void ImtqHandler::doStartUp() {
if (goToNormalMode) {
setMode(MODE_NORMAL);
} else {
@ -35,9 +58,11 @@ void IMTQHandler::doStartUp() {
}
}
void IMTQHandler::doShutDown() { setMode(_MODE_POWER_DOWN); }
void ImtqHandler::doShutDown() { setMode(_MODE_POWER_DOWN); }
ReturnValue_t IMTQHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
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:
*id = IMTQ::GET_ENG_HK_DATA;
@ -45,29 +70,61 @@ ReturnValue_t IMTQHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
break;
case CommunicationStep::START_MTM_MEASUREMENT:
*id = IMTQ::START_MTM_MEASUREMENT;
communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT;
break;
case CommunicationStep::GET_CAL_MTM_MEASUREMENT:
*id = IMTQ::GET_CAL_MTM_MEASUREMENT;
if (pollingMode == NormalPollingMode::BOTH or
pollingMode == NormalPollingMode::UNCALIBRATED) {
communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT;
} else {
communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT;
}
break;
case CommunicationStep::GET_RAW_MTM_MEASUREMENT:
if (integrationTimeCd.getRemainingMillis() > 0) {
TaskFactory::delayTask(integrationTimeCd.getRemainingMillis());
}
*id = IMTQ::GET_RAW_MTM_MEASUREMENT;
if (pollingMode == NormalPollingMode::BOTH) {
communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT;
} else {
communicationStep = CommunicationStep::DIPOLE_ACTUATION;
}
break;
case CommunicationStep::GET_CAL_MTM_MEASUREMENT:
if (integrationTimeCd.getRemainingMillis() > 0) {
TaskFactory::delayTask(integrationTimeCd.getRemainingMillis());
}
*id = IMTQ::GET_CAL_MTM_MEASUREMENT;
communicationStep = CommunicationStep::DIPOLE_ACTUATION;
break;
case CommunicationStep::DIPOLE_ACTUATION: {
// 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.
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;
}
default:
sif::debug << "IMTQHandler::buildNormalDeviceCommand: Invalid communication step"
<< std::endl;
break;
}
return buildCommandFromCommand(*id, NULL, 0);
}
ReturnValue_t IMTQHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
if (buildCommand) {
return buildCommandFromCommand(*id, nullptr, 0);
}
return NOTHING_TO_SEND;
}
ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
return NOTHING_TO_SEND;
}
ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t* commandData,
size_t commandDataLen) {
switch (deviceCommand) {
@ -122,20 +179,36 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
case (IMTQ::START_ACTUATION_DIPOLE): {
/* IMTQ expects low byte first */
commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
if (commandData == nullptr) {
if (commandData != nullptr && commandDataLen < 8) {
return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
}
commandBuffer[1] = commandData[1];
commandBuffer[2] = commandData[0];
commandBuffer[3] = commandData[3];
commandBuffer[4] = commandData[2];
commandBuffer[5] = commandData[5];
commandBuffer[6] = commandData[4];
commandBuffer[7] = commandData[7];
commandBuffer[8] = commandData[6];
rawPacket = commandBuffer;
rawPacketLen = 9;
return returnvalue::OK;
ReturnValue_t result;
// Commands override anything which was set in the software
if (commandData != 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);
}
if (ACTUATION_WIRETAPPING) {
sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value
<< ", y = " << dipoleSet.yDipole.value << ", z = " << dipoleSet.zDipole.value
<< ", duration = " << dipoleSet.currentTorqueDurationMs.value << std::endl;
}
result = buildDipoleActuationCommand();
if (result != returnvalue::OK) {
return result;
}
MutexGuard mg(torquer::lazyLock());
torquer::TORQUEING = true;
torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
return result;
}
case (IMTQ::GET_ENG_HK_DATA): {
commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA;
@ -151,6 +224,7 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
}
case (IMTQ::START_MTM_MEASUREMENT): {
commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT;
integrationTimeCd.resetTimer();
rawPacket = commandBuffer;
rawPacketLen = 1;
return returnvalue::OK;
@ -173,30 +247,42 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
return returnvalue::FAILED;
}
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,
ReturnValue_t ImtqHandler::buildDipoleActuationCommand() {
commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
uint8_t* serPtr = commandBuffer + 1;
size_t serSize = 1;
dipoleSet.setValidityBufferGeneration(false);
ReturnValue_t result = dipoleSet.serialize(&serPtr, &serSize, sizeof(commandBuffer),
SerializeIF::Endianness::LITTLE);
dipoleSet.setValidityBufferGeneration(true);
if (result != returnvalue::OK) {
return result;
}
rawPacket = commandBuffer;
rawPacketLen = 9;
return result;
}
void ImtqHandler::fillCommandAndReplyMap() {
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);
this->insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr,
IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet,
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);
this->insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet,
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,
ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSize,
DeviceCommandId_t* foundId, size_t* foundLen) {
ReturnValue_t result = returnvalue::OK;
@ -233,8 +319,15 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSi
*foundLen = IMTQ::SIZE_SELF_TEST_RESULTS;
*foundId = IMTQ::GET_SELF_TEST_RESULT;
break;
case (IMTQ::CC::PAST_AVAILABLE_RESPONSE_BYTES): {
sif::warning << "IMTQHandler::scanForReply: Read 0xFF command byte, reading past available "
"bytes. Keep 1 ms delay between I2C send and read" << std::endl;
result = IGNORE_REPLY_DATA;
break;
}
default:
sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl;
sif::debug << "IMTQHandler::scanForReply: Reply with length " << remainingSize
<< "contains invalid command code " << static_cast<int>(*start) << std::endl;
result = IGNORE_REPLY_DATA;
break;
}
@ -242,7 +335,7 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSi
return result;
}
ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
ReturnValue_t result = returnvalue::OK;
result = parseStatusByte(packet);
@ -286,9 +379,9 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
return returnvalue::OK;
}
void IMTQHandler::setNormalDatapoolEntriesInvalid() {}
void ImtqHandler::setNormalDatapoolEntriesInvalid() {}
LocalPoolDataSetBase* IMTQHandler::getDataSetHandle(sid_t sid) {
LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) {
if (sid == engHkDataset.getSid()) {
return &engHkDataset;
} else if (sid == calMtmMeasurementSet.getSid()) {
@ -313,9 +406,9 @@ LocalPoolDataSetBase* IMTQHandler::getDataSetHandle(sid_t sid) {
}
}
uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; }
uint32_t ImtqHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; }
ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) {
/** Entries of engineering housekeeping dataset */
localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>({0}));
@ -330,6 +423,11 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat
localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(IMTQ::DIPOLES_X, &dipoleXEntry);
localDataPoolMap.emplace(IMTQ::DIPOLES_Y, &dipoleYEntry);
localDataPoolMap.emplace(IMTQ::DIPOLES_Z, &dipoleZEntry);
localDataPoolMap.emplace(IMTQ::CURRENT_TORQUE_DURATION, &torqueDurationEntry);
/** Entries of calibrated MTM measurement dataset */
localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, &mgmCalEntry);
localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
@ -611,7 +709,7 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat
return returnvalue::OK;
}
ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
ReturnValue_t ImtqHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
DeviceCommandId_t commandId = getPendingCommand();
switch (commandId) {
case IMTQ::POS_X_SELF_TEST:
@ -630,7 +728,7 @@ ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
return returnvalue::OK;
}
ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) {
ReturnValue_t ImtqHandler::parseStatusByte(const uint8_t* packet) {
uint8_t cmdErrorField = *(packet + 1) & 0xF;
switch (cmdErrorField) {
case 0:
@ -661,7 +759,7 @@ ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) {
}
}
void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
void ImtqHandler::fillEngHkDataset(const uint8_t* packet) {
PoolReadGuard rg(&engHkDataset);
uint8_t offset = 2;
engHkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
@ -687,7 +785,9 @@ void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
offset += 2;
engHkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
offset += 2;
engHkDataset.mcuTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
size_t dummy = 2;
SerializeAdapter::deSerialize(&engHkDataset.mcuTemperature.value, packet + offset, &dummy,
SerializeIF::Endianness::LITTLE);
engHkDataset.setValidity(true, true);
@ -708,9 +808,9 @@ void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
}
}
void IMTQHandler::setToGoToNormal(bool enable) { this->goToNormalMode = enable; }
void ImtqHandler::setToGoToNormal(bool enable) { this->goToNormalMode = enable; }
void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) {
void ImtqHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) {
if (wiretappingMode == RAW) {
/* Data already sent in doGetRead() */
return;
@ -734,7 +834,7 @@ void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCom
}
}
void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
void ImtqHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
uint8_t tmData[6];
/* Switching endianess of received dipole values */
tmData[0] = *(packet + 3);
@ -746,7 +846,7 @@ void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
handleDeviceTM(tmData, sizeof(tmData), IMTQ::GET_COMMANDED_DIPOLE);
}
void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
PoolReadGuard rg(&calMtmMeasurementSet);
calMtmMeasurementSet.setValidity(true, true);
int8_t offset = 2;
@ -776,7 +876,7 @@ void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
}
}
void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) {
void ImtqHandler::fillRawMtmDataset(const uint8_t* packet) {
PoolReadGuard rg(&rawMtmMeasurementSet);
unsigned int offset = 2;
size_t deSerLen = 16;
@ -824,7 +924,7 @@ void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) {
}
}
void IMTQHandler::handleSelfTestReply(const uint8_t* packet) {
void ImtqHandler::handleSelfTestReply(const uint8_t* packet) {
uint16_t offset = 2;
checkErrorByte(*(packet + offset), *(packet + offset + 1));
@ -858,7 +958,7 @@ void IMTQHandler::handleSelfTestReply(const uint8_t* packet) {
}
}
void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) {
void ImtqHandler::handlePositiveXSelfTestReply(const uint8_t* packet) {
PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2;
@ -1070,7 +1170,7 @@ void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) {
}
}
void IMTQHandler::handleNegativeXSelfTestReply(const uint8_t* packet) {
void ImtqHandler::handleNegativeXSelfTestReply(const uint8_t* packet) {
PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2;
@ -1282,7 +1382,7 @@ void IMTQHandler::handleNegativeXSelfTestReply(const uint8_t* packet) {
}
}
void IMTQHandler::handlePositiveYSelfTestReply(const uint8_t* packet) {
void ImtqHandler::handlePositiveYSelfTestReply(const uint8_t* packet) {
PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2;
@ -1494,7 +1594,7 @@ void IMTQHandler::handlePositiveYSelfTestReply(const uint8_t* packet) {
}
}
void IMTQHandler::handleNegativeYSelfTestReply(const uint8_t* packet) {
void ImtqHandler::handleNegativeYSelfTestReply(const uint8_t* packet) {
PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2;
@ -1706,7 +1806,7 @@ void IMTQHandler::handleNegativeYSelfTestReply(const uint8_t* packet) {
}
}
void IMTQHandler::handlePositiveZSelfTestReply(const uint8_t* packet) {
void ImtqHandler::handlePositiveZSelfTestReply(const uint8_t* packet) {
PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2;
@ -1918,7 +2018,7 @@ void IMTQHandler::handlePositiveZSelfTestReply(const uint8_t* packet) {
}
}
void IMTQHandler::handleNegativeZSelfTestReply(const uint8_t* packet) {
void ImtqHandler::handleNegativeZSelfTestReply(const uint8_t* packet) {
PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2;
@ -2130,9 +2230,9 @@ void IMTQHandler::handleNegativeZSelfTestReply(const uint8_t* packet) {
}
}
void IMTQHandler::setDebugMode(bool enable) { this->debugMode = enable; }
void ImtqHandler::setDebugMode(bool enable) { this->debugMode = enable; }
void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) {
void ImtqHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) {
std::string stepString("");
if (step < 8) {
stepString = makeStepString(step);
@ -2190,7 +2290,12 @@ void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) {
}
}
std::string IMTQHandler::makeStepString(const uint8_t step) {
void ImtqHandler::doSendRead() {
TaskFactory::delayTask(1);
DeviceHandlerBase::doSendRead();
}
std::string ImtqHandler::makeStepString(const uint8_t step) {
std::string stepString("");
switch (step) {
case IMTQ::SELF_TEST_STEPS::INIT:
@ -2225,7 +2330,7 @@ std::string IMTQHandler::makeStepString(const uint8_t step) {
return stepString;
}
ReturnValue_t IMTQHandler::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) {
ReturnValue_t ImtqHandler::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) {
if (switcher != power::NO_SWITCH) {
*numberOfSwitches = 1;
*switches = &switcher;

29
mission/devices/IMTQHandler.h → mission/devices/ImtqHandler.h
View File

@ -2,7 +2,7 @@
#define MISSION_DEVICES_IMTQHANDLER_H_
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
#include <mission/devices/devicedefinitions/IMTQHandlerDefinitions.h>
#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
#include <string.h>
#include "events/subsystemIdRanges.h"
@ -13,12 +13,17 @@
*
* @author J. Meier
*/
class IMTQHandler : public DeviceHandlerBase {
class ImtqHandler : public DeviceHandlerBase {
public:
IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
power::Switch_t pwrSwitcher);
virtual ~IMTQHandler();
enum NormalPollingMode { UNCALIBRATED = 0, CALIBRATED = 1, BOTH = 2 };
ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
power::Switch_t pwrSwitcher);
virtual ~ImtqHandler();
void setPollingMode(NormalPollingMode pollMode);
void doSendRead() override;
/**
* @brief Sets mode to MODE_NORMAL. Can be used for debugging.
*/
@ -95,6 +100,7 @@ class IMTQHandler : public DeviceHandlerBase {
IMTQ::EngHkDataset engHkDataset;
IMTQ::CalibratedMtmMeasurementSet calMtmMeasurementSet;
IMTQ::RawMtmMeasurementSet rawMtmMeasurementSet;
IMTQ::DipoleActuationSet dipoleSet;
IMTQ::PosXSelfTestSet posXselfTestDataset;
IMTQ::NegXSelfTestSet negXselfTestDataset;
IMTQ::PosYSelfTestSet posYselfTestDataset;
@ -102,7 +108,16 @@ class IMTQHandler : public DeviceHandlerBase {
IMTQ::PosZSelfTestSet posZselfTestDataset;
IMTQ::NegZSelfTestSet negZselfTestDataset;
NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED;
PoolEntry<int32_t> mgmCalEntry = PoolEntry<int32_t>(3);
PoolEntry<int16_t> dipoleXEntry = PoolEntry<int16_t>(0, false);
PoolEntry<int16_t> dipoleYEntry = PoolEntry<int16_t>(0, false);
PoolEntry<int16_t> dipoleZEntry = PoolEntry<int16_t>(0, false);
PoolEntry<uint16_t> torqueDurationEntry = PoolEntry<uint16_t>(0, false);
// Hardcoded to default integration time of 10 ms.
// SHOULDDO: Support for other integration times
Countdown integrationTimeCd = Countdown(10);
power::Switch_t switcher = power::NO_SWITCH;
@ -114,7 +129,8 @@ class IMTQHandler : public DeviceHandlerBase {
GET_ENG_HK_DATA,
START_MTM_MEASUREMENT,
GET_CAL_MTM_MEASUREMENT,
GET_RAW_MTM_MEASUREMENT
GET_RAW_MTM_MEASUREMENT,
DIPOLE_ACTUATION
};
CommunicationStep communicationStep = CommunicationStep::GET_ENG_HK_DATA;
@ -196,6 +212,7 @@ class IMTQHandler : public DeviceHandlerBase {
void handlePositiveZSelfTestReply(const uint8_t* packet);
void handleNegativeZSelfTestReply(const uint8_t* packet);
ReturnValue_t buildDipoleActuationCommand();
/**
* @brief This function checks the error byte of a self test measurement.
*

121
mission/devices/devicedefinitions/IMTQHandlerDefinitions.h → mission/devices/devicedefinitions/imtqHandlerDefinitions.h
View File

@ -1,8 +1,11 @@
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
class ImtqHandler;
namespace IMTQ {
static const DeviceCommandId_t NONE = 0x0;
@ -27,15 +30,18 @@ static const uint8_t GET_TEMP_REPLY_SIZE = 2;
static const uint8_t CFGR_CMD_SIZE = 3;
static const uint8_t POINTER_REG_SIZE = 1;
static const uint32_t ENG_HK_DATA_SET_ID = 1;
static const uint32_t CAL_MTM_SET = 2;
static const uint32_t RAW_MTM_SET = 3;
static const uint32_t POS_X_TEST_DATASET = 4;
static const uint32_t NEG_X_TEST_DATASET = 5;
static const uint32_t POS_Y_TEST_DATASET = 6;
static const uint32_t NEG_Y_TEST_DATASET = 7;
static const uint32_t POS_Z_TEST_DATASET = 8;
static const uint32_t NEG_Z_TEST_DATASET = 9;
enum SetIds : uint32_t {
ENG_HK = 1,
CAL_MGM = 2,
RAW_MGM = 3,
POS_X_TEST = 4,
NEG_X_TEST = 5,
POS_Y_TEST = 6,
NEG_Y_TEST = 7,
POS_Z_TEST = 8,
NEG_Z_TEST = 9,
DIPOLES = 10
};
static const uint8_t SIZE_ENG_HK_COMMAND = 1;
static const uint8_t SIZE_STATUS_REPLY = 2;
@ -80,6 +86,7 @@ static const uint8_t GET_COMMANDED_DIPOLE = 0x46;
static const uint8_t GET_RAW_MTM_MEASUREMENT = 0x42;
static const uint8_t GET_CAL_MTM_MEASUREMENT = 0x43;
static const uint8_t GET_SELF_TEST_RESULT = 0x47;
static const uint8_t PAST_AVAILABLE_RESPONSE_BYTES = 0xff;
}; // namespace CC
namespace SELF_TEST_AXIS {
@ -103,7 +110,7 @@ static const uint8_t Z_NEGATIVE = 0x6;
static const uint8_t FINA = 0x7;
} // namespace SELF_TEST_STEPS
enum IMTQPoolIds : lp_id_t {
enum PoolIds : lp_id_t {
DIGITAL_VOLTAGE_MV,
ANALOG_VOLTAGE_MV,
DIGITAL_CURRENT,
@ -119,6 +126,10 @@ enum IMTQPoolIds : lp_id_t {
ACTUATION_CAL_STATUS,
MTM_RAW,
ACTUATION_RAW_STATUS,
DIPOLES_X,
DIPOLES_Y,
DIPOLES_Z,
CURRENT_TORQUE_DURATION,
INIT_POS_X_ERR,
INIT_POS_X_RAW_MAG_X,
@ -375,9 +386,9 @@ enum IMTQPoolIds : lp_id_t {
class EngHkDataset : public StaticLocalDataSet<ENG_HK_SET_POOL_ENTRIES> {
public:
EngHkDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, ENG_HK_DATA_SET_ID) {}
EngHkDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, IMTQ::SetIds::ENG_HK) {}
EngHkDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, ENG_HK_DATA_SET_ID)) {}
EngHkDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::ENG_HK)) {}
lp_var_t<uint16_t> digitalVoltageMv = lp_var_t<uint16_t>(sid.objectId, DIGITAL_VOLTAGE_MV, this);
lp_var_t<uint16_t> analogVoltageMv = lp_var_t<uint16_t>(sid.objectId, ANALOG_VOLTAGE_MV, this);
@ -398,13 +409,14 @@ class EngHkDataset : public StaticLocalDataSet<ENG_HK_SET_POOL_ENTRIES> {
*/
class CalibratedMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
public:
CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CAL_MTM_SET) {}
CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::SetIds::CAL_MGM) {}
CalibratedMtmMeasurementSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, CAL_MTM_SET)) {}
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::CAL_MGM)) {}
/** The unit of all measurements is nT */
lp_vec_t<int32_t, 3> mgmXyz = lp_vec_t<int32_t, 3>(sid.objectId, MGM_CAL_NT);
lp_vec_t<int32_t, 3> mgmXyz = lp_vec_t<int32_t, 3>(sid.objectId, MGM_CAL_NT, this);
/** 1 if coils were actuating during measurement otherwise 0 */
lp_var_t<uint8_t> coilActuationStatus =
lp_var_t<uint8_t>(sid.objectId, ACTUATION_CAL_STATUS, this);
@ -415,9 +427,11 @@ class CalibratedMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRI
*/
class RawMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
public:
RawMtmMeasurementSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, RAW_MTM_SET) {}
RawMtmMeasurementSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::SetIds::RAW_MGM) {}
RawMtmMeasurementSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, RAW_MTM_SET)) {}
RawMtmMeasurementSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::RAW_MGM)) {}
/** The unit of all measurements is nT */
lp_vec_t<float, 3> mtmRawNt = lp_vec_t<float, 3>(sid.objectId, MTM_RAW, this);
@ -437,6 +451,11 @@ class CommandDipolePacket : public SerialLinkedListAdapter<SerializeIF> {
public:
CommandDipolePacket() { setLinks(); }
SerializeElement<uint16_t> xDipole;
SerializeElement<uint16_t> yDipole;
SerializeElement<uint16_t> zDipole;
SerializeElement<uint16_t> duration;
private:
/**
* @brief Constructor
@ -456,10 +475,46 @@ class CommandDipolePacket : public SerialLinkedListAdapter<SerializeIF> {
yDipole.setNext(&zDipole);
zDipole.setNext(&duration);
}
SerializeElement<uint16_t> xDipole;
SerializeElement<uint16_t> yDipole;
SerializeElement<uint16_t> zDipole;
SerializeElement<uint16_t> duration;
};
class DipoleActuationSet : public StaticLocalDataSet<4> {
friend class ::ImtqHandler;
public:
DipoleActuationSet(HasLocalDataPoolIF& owner)
: StaticLocalDataSet(&owner, IMTQ::SetIds::DIPOLES) {}
DipoleActuationSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::DIPOLES)) {}
// Refresh torque command without changing any of the set dipoles.
void refreshTorqueing(uint16_t durationMs_) { currentTorqueDurationMs = durationMs_; }
void setDipoles(uint16_t xDipole_, uint16_t yDipole_, uint16_t zDipole_,
uint16_t currentTorqueDurationMs_) {
if (xDipole.value != xDipole_) {
}
xDipole = xDipole_;
if (yDipole.value != yDipole_) {
}
yDipole = yDipole_;
if (zDipole.value != zDipole_) {
}
zDipole = zDipole_;
currentTorqueDurationMs = currentTorqueDurationMs_;
}
void getDipoles(uint16_t& xDipole_, uint16_t& yDipole_, uint16_t& zDipole_) {
xDipole_ = xDipole.value;
yDipole_ = yDipole.value;
zDipole_ = zDipole.value;
}
private:
lp_var_t<int16_t> xDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_X, this);
lp_var_t<int16_t> yDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_Y, this);
lp_var_t<int16_t> zDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_Z, this);
lp_var_t<uint16_t> currentTorqueDurationMs =
lp_var_t<uint16_t>(sid.objectId, CURRENT_TORQUE_DURATION, this);
};
/**
@ -479,10 +534,10 @@ class CommandDipolePacket : public SerialLinkedListAdapter<SerializeIF> {
class PosXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
PosXSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::POS_X_TEST_DATASET) {}
: StaticLocalDataSet(owner, IMTQ::SetIds::POS_X_TEST) {}
PosXSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::POS_X_TEST_DATASET)) {}
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_X_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_POS_X_ERR, this);
@ -556,10 +611,10 @@ class PosXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
class NegXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
NegXSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::NEG_X_TEST_DATASET) {}
: StaticLocalDataSet(owner, IMTQ::SetIds::NEG_X_TEST) {}
NegXSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_X_TEST_DATASET)) {}
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_X_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_NEG_X_ERR, this);
@ -633,10 +688,10 @@ class NegXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
class PosYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
PosYSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::POS_Y_TEST_DATASET) {}
: StaticLocalDataSet(owner, IMTQ::SetIds::POS_Y_TEST) {}
PosYSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::POS_Y_TEST_DATASET)) {}
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_Y_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_POS_Y_ERR, this);
@ -710,10 +765,10 @@ class PosYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
class NegYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
NegYSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::NEG_Y_TEST_DATASET) {}
: StaticLocalDataSet(owner, IMTQ::SetIds::NEG_Y_TEST) {}
NegYSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_Y_TEST_DATASET)) {}
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_Y_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_NEG_Y_ERR, this);
@ -787,10 +842,10 @@ class NegYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
class PosZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
PosZSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::POS_Z_TEST_DATASET) {}
: StaticLocalDataSet(owner, IMTQ::SetIds::POS_Z_TEST) {}
PosZSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::POS_Z_TEST_DATASET)) {}
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_Z_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_POS_Z_ERR, this);
@ -864,10 +919,10 @@ class PosZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
class NegZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
NegZSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::NEG_Z_TEST_DATASET) {}
: StaticLocalDataSet(owner, IMTQ::SetIds::NEG_Z_TEST) {}
NegZSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_Z_TEST_DATASET)) {}
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_Z_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_NEG_Z_ERR, this);

27
mission/devices/torquer.cpp Normal file
View File

@ -0,0 +1,27 @@
#include "torquer.h"
#include <fsfw/ipc/MutexGuard.h>
MutexIF* TORQUE_LOCK = nullptr;
namespace torquer {
bool TORQUEING = false;
bool NEW_ACTUATION_FLAG = false;
Countdown TORQUE_COUNTDOWN = Countdown();
bool mgtIsTorqueing(dur_millis_t* remainingTorqueDuration) {
if (TORQUEING and remainingTorqueDuration != nullptr) {
*remainingTorqueDuration = TORQUE_COUNTDOWN.getRemainingMillis() + TORQUE_BUFFER_TIME_MS;
}
return TORQUEING;
}
MutexIF* lazyLock() {
if (TORQUE_LOCK == nullptr) {
TORQUE_LOCK = MutexFactory::instance()->createMutex();
}
return TORQUE_LOCK;
}
} // namespace torquer

22
mission/devices/torquer.h Normal file
View File

@ -0,0 +1,22 @@
#ifndef MISSION_DEVICES_TOQUER_H_
#define MISSION_DEVICES_TOQUER_H_
#include <fsfw/ipc/MutexIF.h>
#include <fsfw/timemanager/Countdown.h>
namespace torquer {
// Additional buffer time to accont for time until I2C command arrives and ramp up / ramp down
// time of the MGT
static constexpr dur_millis_t TORQUE_BUFFER_TIME_MS = 20;
MutexIF* lazyLock();
extern bool TORQUEING;
extern bool NEW_ACTUATION_FLAG;
extern Countdown TORQUE_COUNTDOWN;
bool mgtIsTorqueing(dur_millis_t* remainingTorqueDuration);
} // namespace torquer
#endif /* MISSION_DEVICES_TOQUER_H_ */