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

IMTQ Updates #306

Merged
muellerr merged 13 commits from mueller/imtq-updates into develop 2022-10-25 13:12:19 +02:00
Conversation 1 Commits 13 Files Changed 12 +63 -41
4 changed files with 63 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 386fc60441 - Show all commits

4
bsp_q7s/core/ObjectFactory.cpp
View File

@ -70,7 +70,7 @@
#include "mission/devices/BpxBatteryHandler.h" #include "mission/devices/BpxBatteryHandler.h"
#include "mission/devices/GyroADIS1650XHandler.h" #include "mission/devices/GyroADIS1650XHandler.h"
#include "mission/devices/HeaterHandler.h" #include "mission/devices/HeaterHandler.h"
#include "mission/devices/IMTQHandler.h" #include <mission/devices/ImtqHandler.h>
#include "mission/devices/Max31865PT1000Handler.h" #include "mission/devices/Max31865PT1000Handler.h"
#include "mission/devices/P60DockHandler.h" #include "mission/devices/P60DockHandler.h"
#include "mission/devices/PCDUHandler.h" #include "mission/devices/PCDUHandler.h"
@ -889,7 +889,7 @@ void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher) {
void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher) { void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher) {
I2cCookie* imtqI2cCookie = I2cCookie* imtqI2cCookie =
new I2cCookie(addresses::IMTQ, IMTQ::MAX_REPLY_SIZE, q7s::I2C_DEFAULT_DEV); 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); pcdu::Switches::PDU1_CH3_MGT_5V);
imtqHandler->setPowerSwitcher(pwrSwitcher); imtqHandler->setPowerSwitcher(pwrSwitcher);
static_cast<void>(imtqHandler); static_cast<void>(imtqHandler);

2
mission/devices/CMakeLists.txt
View File

@ -11,7 +11,7 @@ target_sources(
SyrlinksHkHandler.cpp SyrlinksHkHandler.cpp
Max31865PT1000Handler.cpp Max31865PT1000Handler.cpp
Max31865EiveHandler.cpp Max31865EiveHandler.cpp
IMTQHandler.cpp ImtqHandler.cpp
HeaterHandler.cpp HeaterHandler.cpp
RadiationSensorHandler.cpp RadiationSensorHandler.cpp
GyroADIS1650XHandler.cpp GyroADIS1650XHandler.cpp

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

@ -1,13 +1,16 @@
#include "IMTQHandler.h"
#include <fsfw/datapool/PoolReadGuard.h> #include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/CRC.h> #include <fsfw/globalfunctions/CRC.h>
#include <mission/devices/ImtqHandler.h>
#include <cmath> #include <cmath>
#include "OBSWConfig.h" #include "OBSWConfig.h"
IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, MutexIF* ImtqHandler::TORQUE_LOCK = nullptr;
bool ImtqHandler::TORQUEING = false;
Countdown ImtqHandler::TORQUE_COUNTDOWN = Countdown();
ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
power::Switch_t pwrSwitcher) power::Switch_t pwrSwitcher)
: DeviceHandlerBase(objectId, comIF, comCookie), : DeviceHandlerBase(objectId, comIF, comCookie),
engHkDataset(this), engHkDataset(this),
@ -24,11 +27,12 @@ IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC
if (comCookie == nullptr) { if (comCookie == nullptr) {
sif::error << "IMTQHandler: Invalid com cookie" << std::endl; sif::error << "IMTQHandler: Invalid com cookie" << std::endl;
} }
TORQUE_LOCK = MutexFactory::instance()->createMutex();
} }
IMTQHandler::~IMTQHandler() {} ImtqHandler::~ImtqHandler() {}
void IMTQHandler::doStartUp() { void ImtqHandler::doStartUp() {
if (goToNormalMode) { if (goToNormalMode) {
setMode(MODE_NORMAL); setMode(MODE_NORMAL);
} else { } else {
@ -36,9 +40,9 @@ 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) {
switch (communicationStep) { switch (communicationStep) {
case CommunicationStep::GET_ENG_HK_DATA: case CommunicationStep::GET_ENG_HK_DATA:
*id = IMTQ::GET_ENG_HK_DATA; *id = IMTQ::GET_ENG_HK_DATA;
@ -64,11 +68,11 @@ ReturnValue_t IMTQHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
return buildCommandFromCommand(*id, NULL, 0); return buildCommandFromCommand(*id, NULL, 0);
} }
ReturnValue_t IMTQHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
return NOTHING_TO_SEND; return NOTHING_TO_SEND;
} }
ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t* commandData, const uint8_t* commandData,
size_t commandDataLen) { size_t commandDataLen) {
switch (deviceCommand) { switch (deviceCommand) {
@ -174,7 +178,7 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
return returnvalue::FAILED; return returnvalue::FAILED;
} }
void IMTQHandler::fillCommandAndReplyMap() { void ImtqHandler::fillCommandAndReplyMap() {
this->insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY); 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::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::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
@ -197,7 +201,7 @@ void IMTQHandler::fillCommandAndReplyMap() {
IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT); 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) { DeviceCommandId_t* foundId, size_t* foundLen) {
ReturnValue_t result = returnvalue::OK; ReturnValue_t result = returnvalue::OK;
@ -243,7 +247,7 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSi
return result; 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; ReturnValue_t result = returnvalue::OK;
result = parseStatusByte(packet); result = parseStatusByte(packet);
@ -287,9 +291,9 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
return returnvalue::OK; 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()) { if (sid == engHkDataset.getSid()) {
return &engHkDataset; return &engHkDataset;
} else if (sid == calMtmMeasurementSet.getSid()) { } else if (sid == calMtmMeasurementSet.getSid()) {
@ -314,9 +318,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) { LocalDataPoolManager& poolManager) {
/** Entries of engineering housekeeping dataset */ /** Entries of engineering housekeeping dataset */
localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>({0})); localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>({0}));
@ -612,7 +616,7 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat
return returnvalue::OK; return returnvalue::OK;
} }
ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) { ReturnValue_t ImtqHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
DeviceCommandId_t commandId = getPendingCommand(); DeviceCommandId_t commandId = getPendingCommand();
switch (commandId) { switch (commandId) {
case IMTQ::POS_X_SELF_TEST: case IMTQ::POS_X_SELF_TEST:
@ -631,7 +635,7 @@ ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
return returnvalue::OK; 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; uint8_t cmdErrorField = *(packet + 1) & 0xF;
switch (cmdErrorField) { switch (cmdErrorField) {
case 0: case 0:
@ -662,7 +666,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); PoolReadGuard rg(&engHkDataset);
uint8_t offset = 2; uint8_t offset = 2;
engHkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset); engHkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
@ -709,9 +713,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) { if (wiretappingMode == RAW) {
/* Data already sent in doGetRead() */ /* Data already sent in doGetRead() */
return; return;
@ -735,7 +739,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]; uint8_t tmData[6];
/* Switching endianess of received dipole values */ /* Switching endianess of received dipole values */
tmData[0] = *(packet + 3); tmData[0] = *(packet + 3);
@ -747,7 +751,7 @@ void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
handleDeviceTM(tmData, sizeof(tmData), IMTQ::GET_COMMANDED_DIPOLE); 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); PoolReadGuard rg(&calMtmMeasurementSet);
calMtmMeasurementSet.setValidity(true, true); calMtmMeasurementSet.setValidity(true, true);
int8_t offset = 2; int8_t offset = 2;
@ -777,7 +781,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); PoolReadGuard rg(&rawMtmMeasurementSet);
unsigned int offset = 2; unsigned int offset = 2;
size_t deSerLen = 16; size_t deSerLen = 16;
@ -825,7 +829,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; uint16_t offset = 2;
checkErrorByte(*(packet + offset), *(packet + offset + 1)); checkErrorByte(*(packet + offset), *(packet + offset + 1));
@ -859,7 +863,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); PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2; uint16_t offset = 2;
@ -1071,7 +1075,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); PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2; uint16_t offset = 2;
@ -1283,7 +1287,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); PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2; uint16_t offset = 2;
@ -1495,7 +1499,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); PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2; uint16_t offset = 2;
@ -1707,7 +1711,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); PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2; uint16_t offset = 2;
@ -1919,7 +1923,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); PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2; uint16_t offset = 2;
@ -2131,9 +2135,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(""); std::string stepString("");
if (step < 8) { if (step < 8) {
stepString = makeStepString(step); stepString = makeStepString(step);
@ -2191,7 +2195,7 @@ void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) {
} }
} }
std::string IMTQHandler::makeStepString(const uint8_t step) { std::string ImtqHandler::makeStepString(const uint8_t step) {
std::string stepString(""); std::string stepString("");
switch (step) { switch (step) {
case IMTQ::SELF_TEST_STEPS::INIT: case IMTQ::SELF_TEST_STEPS::INIT:
@ -2226,7 +2230,7 @@ std::string IMTQHandler::makeStepString(const uint8_t step) {
return stepString; 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) { if (switcher != power::NO_SWITCH) {
*numberOfSwitches = 1; *numberOfSwitches = 1;
*switches = &switcher; *switches = &switcher;
@ -2234,3 +2238,11 @@ ReturnValue_t IMTQHandler::getSwitches(const uint8_t** switches, uint8_t* number
} }
return DeviceHandlerBase::NO_SWITCH; 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;
}

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

@ -13,11 +13,11 @@
* *
* @author J. Meier * @author J. Meier
*/ */
class IMTQHandler : public DeviceHandlerBase { class ImtqHandler : public DeviceHandlerBase {
public: public:
IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
power::Switch_t pwrSwitcher); power::Switch_t pwrSwitcher);
virtual ~IMTQHandler(); virtual ~ImtqHandler();
/** /**
* @brief Sets mode to MODE_NORMAL. Can be used for debugging. * @brief Sets mode to MODE_NORMAL. Can be used for debugging.
@ -206,6 +206,16 @@ class IMTQHandler : public DeviceHandlerBase {
void checkErrorByte(const uint8_t errorByte, const uint8_t step); void checkErrorByte(const uint8_t errorByte, const uint8_t step);
std::string makeStepString(const uint8_t step); std::string makeStepString(const uint8_t step);
static MutexIF* TORQUE_LOCK;
static bool TORQUEING;
static Countdown TORQUE_COUNTDOWN;
// 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;
static bool mgtIsTorqueing(dur_millis_t* remainingTorqueDuration);
}; };
#endif /* MISSION_DEVICES_IMTQHANDLER_H_ */ #endif /* MISSION_DEVICES_IMTQHANDLER_H_ */