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

added back self-test support

Browse Source
This commit is contained in:
Robin Müller
2023-02-20 02:32:48 +01:00
parent 641c069664
commit 2bacf1efa0
5 changed files with 293 additions and 225 deletions
Download Patch File Download Diff File Expand all files Collapse all files

198
mission/devices/ImtqHandler.cpp
View File

@ -69,7 +69,7 @@ void ImtqHandler::doShutDown() {
ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
switch (requestStep) {
case (imtq::RequestType::MEASURE): {
case (imtq::RequestType::MEASURE_WITH_ACTUATION): {
*id = imtq::cmdIds::REQUEST;
return buildCommandFromCommand(*id, nullptr, 0);
}
@ -88,40 +88,43 @@ ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t* commandData,
size_t commandDataLen) {
auto genericMeasureRequest = [&](imtq::SpecialRequest specialRequest) {
ImtqRequest request(commandBuffer, sizeof(commandBuffer));
request.setMeasureRequest(specialRequest);
rawPacket = commandBuffer;
rawPacketLen = ImtqRequest::REQUEST_LEN;
};
switch (deviceCommand) {
case (imtq::cmdIds::POS_X_SELF_TEST): {
// TODO: special request
genericMeasureRequest(imtq::SpecialRequest::DO_SELF_TEST_POS_X);
return returnvalue::OK;
}
case (imtq::cmdIds::NEG_X_SELF_TEST): {
// TODO: special request
genericMeasureRequest(imtq::SpecialRequest::DO_SELF_TEST_NEG_X);
return returnvalue::OK;
}
case (imtq::cmdIds::POS_Y_SELF_TEST): {
// TODO: special request
genericMeasureRequest(imtq::SpecialRequest::DO_SELF_TEST_POS_Y);
return returnvalue::OK;
}
case (imtq::cmdIds::NEG_Y_SELF_TEST): {
// TODO: special request
genericMeasureRequest(imtq::SpecialRequest::DO_SELF_TEST_NEG_Y);
return returnvalue::OK;
}
case (imtq::cmdIds::POS_Z_SELF_TEST): {
// TODO: special request
genericMeasureRequest(imtq::SpecialRequest::DO_SELF_TEST_POS_Z);
return returnvalue::OK;
}
case (imtq::cmdIds::NEG_Z_SELF_TEST): {
// TODO: special request
genericMeasureRequest(imtq::SpecialRequest::DO_SELF_TEST_NEG_Z);
return returnvalue::OK;
}
case (imtq::cmdIds::GET_SELF_TEST_RESULT): {
// TODO: special request
genericMeasureRequest(imtq::SpecialRequest::GET_SELF_TEST_RESULT);
return returnvalue::OK;
}
case (imtq::cmdIds::REQUEST): {
ImtqRequest request(commandBuffer, sizeof(commandBuffer));
request.setMeasureRequest(imtq::SpecialRequest::NONE);
rawPacket = commandBuffer;
rawPacketLen = ImtqRequest::REQUEST_LEN;
genericMeasureRequest(imtq::SpecialRequest::NONE);
return returnvalue::OK;
}
case (imtq::cmdIds::START_ACTUATION_DIPOLE): {
@ -168,6 +171,13 @@ void ImtqHandler::fillCommandAndReplyMap() {
insertInCommandMap(imtq::cmdIds::REQUEST);
insertInCommandMap(imtq::cmdIds::START_ACTUATION_DIPOLE);
insertInReplyMap(imtq::cmdIds::REPLY, 5, nullptr, 0, true);
insertInCommandMap(imtq::cmdIds::POS_X_SELF_TEST);
insertInCommandMap(imtq::cmdIds::NEG_X_SELF_TEST);
insertInCommandMap(imtq::cmdIds::POS_Y_SELF_TEST);
insertInCommandMap(imtq::cmdIds::NEG_Y_SELF_TEST);
insertInCommandMap(imtq::cmdIds::POS_Z_SELF_TEST);
insertInCommandMap(imtq::cmdIds::NEG_Z_SELF_TEST);
insertInCommandMap(imtq::cmdIds::GET_SELF_TEST_RESULT);
}
ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSize,
@ -187,72 +197,102 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
ReturnValue_t result;
ReturnValue_t status = returnvalue::OK;
// arrayprinter::print(packet, ImtqReplies::BASE_LEN);
if (requestStep == imtq::RequestType::MEASURE) {
if (requestStep == imtq::RequestType::MEASURE_WITH_ACTUATION) {
requestStep = imtq::RequestType::ACTUATE;
ImtqRepliesDefault replies(packet);
if (replies.wasEngHkRead()) {
uint8_t* engHkReply = replies.getEngHk();
result = parseStatusByte(imtq::CC::GET_ENG_HK_DATA, engHkReply);
if (result == returnvalue::OK) {
fillEngHkDataset(hkDatasetNoTorque, engHkReply);
} else {
if (replies.wasSpecialRequestRead()) {
uint8_t* specialRequest = replies.getSpecialRequest();
imtq::CC::CC cc = specialRequest[0];
result = parseStatusByte(cc, packet);
if (result != returnvalue::OK) {
status = result;
}
if (cc == imtq::CC::CC::GET_SELF_TEST_RESULT) {
handleSelfTestReply(specialRequest);
}
// For a special request, the other stuff was not read, so return here.
return status;
} else {
sif::warning << "IMTQ: Possible timing issue, special request was not read" << std::endl;
}
if (not replies.wasEngHkRead()) {
sif::warning << "IMTQ: Possible timing issue, ENG HK was not read" << std::endl;
}
// Still read it, even if it is old. Better than nothing
uint8_t* engHkReply = replies.getEngHk();
result = parseStatusByte(imtq::CC::GET_ENG_HK_DATA, engHkReply);
if (result == returnvalue::OK) {
fillEngHkDataset(hkDatasetNoTorque, engHkReply);
} else {
status = result;
}
if (replies.wasGetSystemStateRead()) {
uint8_t* sysStateReply = replies.getSystemState();
result = parseStatusByte(imtq::CC::GET_SYSTEM_STATE, sysStateReply);
if (result == returnvalue::OK) {
fillSystemStateIntoDataset(sysStateReply);
} else {
status = result;
}
if (not replies.wasGetSystemStateRead()) {
sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
}
if (replies.wasGetRawMgmMeasurementRead()) {
uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(rawMgmMeasurement);
} else {
status = result;
}
uint8_t* sysStateReply = replies.getSystemState();
result = parseStatusByte(imtq::CC::GET_SYSTEM_STATE, sysStateReply);
if (result == returnvalue::OK) {
fillSystemStateIntoDataset(sysStateReply);
} else {
status = result;
}
if (replies.wasCalibMgmMeasurementRead()) {
uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_CAL_MTM_MEASUREMENT, calibMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(calibMgmMeasurement);
} else {
status = result;
}
if (not replies.wasGetRawMgmMeasurementRead()) {
sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
}
uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(rawMgmMeasurement);
} else {
status = result;
}
if (not replies.wasCalibMgmMeasurementRead()) {
sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
}
uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_CAL_MTM_MEASUREMENT, calibMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(calibMgmMeasurement);
} else {
status = result;
}
} else {
requestStep = imtq::RequestType::MEASURE;
requestStep = imtq::RequestType::MEASURE_WITH_ACTUATION;
ImtqRepliesWithTorque replies(packet);
if (replies.wasDipoleActuationRead()) {
parseStatusByte(imtq::CC::START_ACTUATION_DIPOLE, replies.getDipoleActuation());
} else {
sif::warning << "IMTQ: Possible timing issue, start actuation dipole status was not read"
<< std::endl;
}
if (replies.wasGetRawMgmMeasurementRead()) {
uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(rawMgmMeasurement);
} else {
status = result;
}
if (not replies.wasGetRawMgmMeasurementRead()) {
sif::warning << "IMTQ: Possible timing issue, was MGM measurement with torque was not read"
<< std::endl;
}
if (replies.wasEngHkRead()) {
uint8_t* engHkReply = replies.getEngHk();
result = parseStatusByte(imtq::CC::GET_ENG_HK_DATA, engHkReply);
if (result != returnvalue::OK) {
return result;
} else {
status = result;
}
fillEngHkDataset(hkDatasetNoTorque, engHkReply);
uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(rawMgmMeasurement);
} else {
status = result;
}
if (not replies.wasEngHkRead()) {
sif::warning << "IMTQ: Possible timing issue, engineering HK with torque was not read"
<< std::endl;
}
uint8_t* engHkReply = replies.getEngHk();
result = parseStatusByte(imtq::CC::GET_ENG_HK_DATA, engHkReply);
if (result != returnvalue::OK) {
return result;
} else {
status = result;
}
fillEngHkDataset(hkDatasetNoTorque, engHkReply);
}
return status;
}
@ -705,42 +745,6 @@ void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* pa
void ImtqHandler::setToGoToNormal(bool enable) { this->goToNormalMode = enable; }
void ImtqHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) {
if (wiretappingMode == RAW) {
/* Data already sent in doGetRead() */
return;
}
DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId);
if (iter == deviceReplyMap.end()) {
sif::debug << "IMTQHandler::handleDeviceTM: Unknown reply id" << std::endl;
return;
}
MessageQueueId_t queueId = iter->second.command->second.sendReplyTo;
if (queueId == NO_COMMANDER) {
return;
}
ReturnValue_t result = actionHelper.reportData(queueId, replyId, data, dataSize);
if (result != returnvalue::OK) {
sif::debug << "IMTQHandler::handleDeviceTM: Failed to report data" << std::endl;
return;
}
}
void ImtqHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
// uint8_t tmData[6];
// /* Switching endianess of received dipole values */
// tmData[0] = *(packet + 3);
// tmData[1] = *(packet + 2);
// tmData[2] = *(packet + 5);
// tmData[3] = *(packet + 4);
// tmData[4] = *(packet + 7);
// tmData[5] = *(packet + 6);
// handleDeviceTM(tmData, sizeof(tmData), imtq::cmdIds::GET_COMMANDED_DIPOLE);
}
void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
PoolReadGuard rg(&calMtmMeasurementSet);
calMtmMeasurementSet.setValidity(true, true);

22
mission/devices/ImtqHandler.h
View File

@ -116,11 +116,7 @@ class ImtqHandler : public DeviceHandlerBase {
bool goToNormalMode = false;
bool debugMode = false;
imtq::RequestType requestStep = imtq::RequestType::MEASURE;
// enum class StartupStep { NONE, COMMAND_SELF_TEST, GET_SELF_TEST_RESULT };
// StartupStep startupStep = StartupStep::COMMAND_SELF_TEST;
imtq::RequestType requestStep = imtq::RequestType::MEASURE_WITH_ACTUATION;
/**
* @brief In case of a status reply to a single axis self test command, this function
@ -148,22 +144,6 @@ class ImtqHandler : public DeviceHandlerBase {
void fillSystemStateIntoDataset(const uint8_t* packet);
/**
* @brief This function sends a command reply to the requesting queue.
*
* @param data Pointer to the data to send.
* @param dataSize Size of the data to send.
* @param relplyId Reply id which will be inserted at the beginning of the action message.
*/
void handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId);
/**
* @brief This function handles the reply containing the commanded dipole.
*
* @param packet Pointer to the reply data.
*/
void handleGetCommandedDipoleReply(const uint8_t* packet);
/**
* @brief This function parses the reply containing the calibrated MTM measurement and writes
* the values to the appropriate dataset.

27
mission/devices/devicedefinitions/imtqHelpers.h
View File

@ -10,14 +10,17 @@ class ImtqHandler;
namespace imtq {
enum class RequestType : uint8_t { MEASURE, ACTUATE };
enum class RequestType : uint8_t { MEASURE_WITH_ACTUATION, ACTUATE };
enum class SpecialRequest : uint8_t {
NONE = 0,
DO_SELF_TEST_X = 1,
DO_SELF_TEST_Y = 2,
DO_SELF_TEST_Z = 3,
GET_SELF_TEST_RESULT = 4
DO_SELF_TEST_POS_X = 1,
DO_SELF_TEST_NEG_X = 2,
DO_SELF_TEST_POS_Y = 3,
DO_SELF_TEST_NEG_Y = 4,
DO_SELF_TEST_POS_Z = 5,
DO_SELF_TEST_NEG_Z = 6,
GET_SELF_TEST_RESULT = 7
};
static const uint8_t INTERFACE_ID = CLASS_ID::IMTQ_HANDLER;
@ -50,16 +53,6 @@ static const DeviceCommandId_t GET_SELF_TEST_RESULT = 0xD;
} // namespace cmdIds
// static const DeviceCommandId_t NONE = 0x0;
// static const DeviceCommandId_t GET_ENG_HK_DATA = 0x1;
// static const DeviceCommandId_t GET_COMMANDED_DIPOLE = 0x3;
///** Generates new measurement of the magnetic field */
// static const DeviceCommandId_t START_MTM_MEASUREMENT = 0x4;
///** Requests the calibrated magnetometer measurement */
// static const DeviceCommandId_t GET_CAL_MTM_MEASUREMENT = 0x5;
///** Requests the raw values measured by the built-in MTM XEN1210 */
// static const DeviceCommandId_t GET_RAW_MTM_MEASUREMENT = 0x6;
static const uint8_t POINTER_REG_SIZE = 1;
enum SetIds : uint32_t {
@ -1105,7 +1098,7 @@ struct ImtqRequest {
imtq::RequestType getRequestType() const { return static_cast<imtq::RequestType>(rawData[0]); }
void setMeasureRequest(imtq::SpecialRequest specialRequest) {
rawData[0] = static_cast<uint8_t>(imtq::RequestType::MEASURE);
rawData[0] = static_cast<uint8_t>(imtq::RequestType::MEASURE_WITH_ACTUATION);
rawData[1] = static_cast<uint8_t>(specialRequest);
}
@ -1148,7 +1141,7 @@ struct ImtqRequest {
private:
ImtqRequest(uint8_t* rawData, size_t maxLen) : rawData(rawData) {
if (rawData != nullptr) {
rawData[0] = static_cast<uint8_t>(imtq::RequestType::MEASURE);
rawData[0] = static_cast<uint8_t>(imtq::RequestType::MEASURE_WITH_ACTUATION);
}
}
uint8_t* rawData;