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

re-use dipole set in IMTQ handler
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 10:32:17 +02:00
parent 42bb2f554f
commit ab273a8a59
5 changed files with 107 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

109
mission/devices/ImtqHandler.cpp
View File

@ -1,11 +1,29 @@
#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 "OBSWConfig.h"
#include <fsfw/datapoollocal/LocalPoolVariable.tpp>
MutexIF* ImtqHandler::TORQUE_LOCK = nullptr;
bool ImtqHandler::TORQUEING = false;
@ -56,22 +74,43 @@ 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;
communicationStep = CommunicationStep::GET_RAW_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: {
// TODO: Set correct ID if actuation is necessary
*id = IMTQ::START_ACTUATION_DIPOLE;
communicationStep = CommunicationStep::GET_ENG_HK_DATA;
break;
}
default:
sif::debug << "IMTQHandler::buildNormalDeviceCommand: Invalid communication step"
<< std::endl;
break;
}
return buildCommandFromCommand(*id, NULL, 0);
return buildCommandFromCommand(*id, nullptr, 0);
}
ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
@ -133,20 +172,27 @@ 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;
PoolReadGuard pg(&dipoleSet);
ReturnValue_t result;
// Commands override anything which was set in the software
if (commandBuffer != nullptr) {
result =
dipoleSet.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::NETWORK);
if (result != returnvalue::OK) {
return result;
}
}
result = buildDipoleActuationCommand();
if (result != returnvalue::OK) {
return result;
}
MutexGuard mg(TORQUE_LOCK);
TORQUEING = true;
TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
return result;
}
case (IMTQ::GET_ENG_HK_DATA): {
commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA;
@ -162,6 +208,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;
@ -184,6 +231,22 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
return returnvalue::FAILED;
}
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() {
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);
@ -700,7 +763,7 @@ void ImtqHandler::fillEngHkDataset(const uint8_t* packet) {
offset += 2;
size_t dummy = 2;
SerializeAdapter::deSerialize(&engHkDataset.mcuTemperature.value, packet + offset, &dummy,
SerializeIF::Endianness::LITTLE);
SerializeIF::Endianness::LITTLE);
engHkDataset.setValidity(true, true);
@ -2247,7 +2310,7 @@ ReturnValue_t ImtqHandler::getSwitches(const uint8_t** switches, uint8_t* number
return DeviceHandlerBase::NO_SWITCH;
}
bool ImtqHandler::mgtIsTorqueing(dur_millis_t *remainingTorqueDuration) {
bool ImtqHandler::mgtIsTorqueing(dur_millis_t* remainingTorqueDuration) {
MutexGuard mg(TORQUE_LOCK);
if (TORQUEING and remainingTorqueDuration != nullptr) {
*remainingTorqueDuration = TORQUE_COUNTDOWN.getRemainingMillis() + TORQUE_BUFFER_TIME_MS;