eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 4 Pull Requests 2 Releases 119 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 +107 -34
5 changed files with 107 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit ab273a8a59 - Show all commits

3
bsp_q7s/core/ObjectFactory.cpp
View File

@ -47,6 +47,8 @@
#if OBSW_TEST_LIBGPIOD == 1
#include "linux/boardtest/LibgpiodTest.h"
#endif
#include <mission/devices/ImtqHandler.h>
#include <sstream>
#include "fsfw/datapoollocal/LocalDataPoolManager.h"
@ -70,7 +72,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"

2
mission/controller/AcsController.h
View File

@ -71,8 +71,6 @@ class AcsController : public ExtendedControllerBase {
SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB),
};
// Initial delay to make sure all pool variables have been initialized their owners
Countdown initialCountdown = Countdown(INIT_DELAY);
};

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;

14
mission/devices/ImtqHandler.h
View File

@ -15,10 +15,14 @@
*/
class ImtqHandler : public DeviceHandlerBase {
public:
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);
/**
* @brief Sets mode to MODE_NORMAL. Can be used for debugging.
*/
@ -103,7 +107,12 @@ class ImtqHandler : public DeviceHandlerBase {
IMTQ::PosZSelfTestSet posZselfTestDataset;
IMTQ::NegZSelfTestSet negZselfTestDataset;
NormalPollingMode pollingMode = NormalPollingMode::BOTH;
PoolEntry<int32_t> mgmCalEntry = PoolEntry<int32_t>(3);
// 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;
@ -115,7 +124,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;
@ -197,6 +207,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.
*
@ -215,7 +226,6 @@ class ImtqHandler : public DeviceHandlerBase {
static constexpr dur_millis_t TORQUE_BUFFER_TIME_MS = 20;
static bool mgtIsTorqueing(dur_millis_t* remainingTorqueDuration);
};
#endif /* MISSION_DEVICES_IMTQHANDLER_H_ */

13
mission/devices/devicedefinitions/IMTQHandlerDefinitions.h
View File

@ -448,6 +448,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
@ -467,10 +472,6 @@ 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> {
@ -498,13 +499,13 @@ class DipoleActuationSet : public StaticLocalDataSet<4> {
zDipole = zDipole_;
currentTorqueDurationMs = currentTorqueDurationMs_;
}
private:
lp_var_t<uint16_t> xDipole = lp_var_t<uint16_t>(sid.objectId, DIPOLES_X, this);
lp_var_t<uint16_t> yDipole = lp_var_t<uint16_t>(sid.objectId, DIPOLES_Y, this);
lp_var_t<uint16_t> zDipole = lp_var_t<uint16_t>(sid.objectId, DIPOLES_Z, this);
lp_var_t<uint16_t> currentTorqueDurationMs =
lp_var_t<uint16_t>(sid.objectId, CURRENT_TORQUE_DURATION, this);
private:
bool newActuation = false;
};
/**