Merge pull request 'More buffer time' (#433) from bugfix_imtq_timing into develop
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Reviewed-on: #433
This commit is contained in:
commit
acae88c67c
@ -59,7 +59,7 @@ namespace spiSched {
|
||||
|
||||
static constexpr uint32_t SCHED_BLOCK_1_SUS_READ_MS = 15;
|
||||
static constexpr uint32_t SCHED_BLOCK_2_SENSOR_READ_MS = 30;
|
||||
static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 42;
|
||||
static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 43;
|
||||
static constexpr uint32_t SCHED_BLOCK_4_ACS_CTRL_MS = 45;
|
||||
static constexpr uint32_t SCHED_BLOCK_5_ACTUATOR_MS = 55;
|
||||
static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 105;
|
||||
|
@ -28,6 +28,8 @@ ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) {
|
||||
// Stopwatch watch;
|
||||
switch (currentRequest) {
|
||||
case imtq::RequestType::MEASURE_NO_ACTUATION: {
|
||||
// Measured to take 24 ms for debug and release builds.
|
||||
// Stopwatch watch;
|
||||
handleMeasureStep();
|
||||
break;
|
||||
}
|
||||
@ -47,6 +49,9 @@ void ImtqPollingTask::handleMeasureStep() {
|
||||
size_t replyLen = 0;
|
||||
uint8_t* replyPtr;
|
||||
ImtqRepliesDefault replies(replyBuf.data());
|
||||
// If some startup handling is added later, set configured after it was done once.
|
||||
replies.setConfigured();
|
||||
|
||||
// Can be used later to verify correct timing (e.g. all data has been read)
|
||||
clearReadFlagsDefault(replies);
|
||||
auto i2cCmdExecMeasure = [&](imtq::CC::CC cc) {
|
||||
@ -127,17 +132,12 @@ void ImtqPollingTask::handleMeasureStep() {
|
||||
if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
|
||||
return;
|
||||
}
|
||||
bool mgmMeasurementTooOld = false;
|
||||
// See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably
|
||||
// have old data. Which can be really bad for ACS. And everything.
|
||||
if ((replyPtr[2] >> 7) == 0) {
|
||||
sif::error << "IMTQ: MGM measurement too old" << std::endl;
|
||||
TaskFactory::delayTask(2);
|
||||
if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
|
||||
return;
|
||||
}
|
||||
if ((replyPtr[2] >> 7) == 0b1) {
|
||||
replyPtr[0] = false;
|
||||
}
|
||||
replyPtr[0] = false;
|
||||
mgmMeasurementTooOld = true;
|
||||
}
|
||||
|
||||
cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
|
||||
@ -149,7 +149,9 @@ void ImtqPollingTask::handleMeasureStep() {
|
||||
if (i2cCmdExecMeasure(imtq::CC::GET_CAL_MTM_MEASUREMENT) != returnvalue::OK) {
|
||||
return;
|
||||
}
|
||||
// sif::debug << "measure done" << std::endl;
|
||||
if(mgmMeasurementTooOld) {
|
||||
sif::error << "IMTQ: MGM measurement too old" << std::endl;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
@ -186,25 +188,22 @@ void ImtqPollingTask::handleActuateStep() {
|
||||
if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
|
||||
return;
|
||||
}
|
||||
bool measurementWasTooOld = false;
|
||||
// See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably
|
||||
// have old data. Which can be really bad for ACS. And everything.
|
||||
if ((replyPtr[2] >> 7) == 0) {
|
||||
sif::error << "IMTQ: MGM measurement too old" << std::endl;
|
||||
TaskFactory::delayTask(2);
|
||||
cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
|
||||
if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
|
||||
return;
|
||||
}
|
||||
if ((replyPtr[2] >> 7) == 0b1) {
|
||||
replyPtr[0] = false;
|
||||
}
|
||||
measurementWasTooOld = true;
|
||||
replyPtr[0] = false;
|
||||
}
|
||||
|
||||
cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
|
||||
if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
|
||||
return;
|
||||
}
|
||||
// sif::debug << "measure with torque done" << std::endl;
|
||||
|
||||
if(measurementWasTooOld) {
|
||||
sif::error << "IMTQ: MGM measurement too old" << std::endl;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
@ -223,7 +222,7 @@ ReturnValue_t ImtqPollingTask::initializeInterface(CookieIF* cookie) {
|
||||
|
||||
ReturnValue_t ImtqPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData,
|
||||
size_t sendLen) {
|
||||
const auto* imtqReq = reinterpret_cast<const ImtqRequest*>(sendData);
|
||||
const auto* imtqReq = reinterpret_cast<const imtq::Request*>(sendData);
|
||||
{
|
||||
MutexGuard mg(ipcLock);
|
||||
if (imtqReq->request == imtq::RequestType::ACTUATE) {
|
||||
|
@ -33,13 +33,12 @@ class ImtqPollingTask : public SystemObject,
|
||||
address_t i2cAddr = 0;
|
||||
uint32_t currentIntegrationTimeMs = 10;
|
||||
// Required in addition to integration time, otherwise old data might be read.
|
||||
static constexpr uint32_t MGM_READ_BUFFER_TIME_MS = 3;
|
||||
static constexpr uint32_t MGM_READ_BUFFER_TIME_MS = 6;
|
||||
bool ignoreNextActuateRequest = false;
|
||||
|
||||
imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE;
|
||||
int16_t dipoles[3] = {};
|
||||
uint16_t torqueDuration = 0;
|
||||
// uint8_t startActuateRawBuf[3] = {};
|
||||
|
||||
std::array<uint8_t, 32> cmdBuf;
|
||||
std::array<uint8_t, 524> replyBuf;
|
||||
|
@ -131,12 +131,21 @@ ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) {
|
||||
ImtqHandler::~ImtqHandler() = default;
|
||||
|
||||
void ImtqHandler::doStartUp() {
|
||||
updatePeriodicReply(true, imtq::cmdIds::REPLY_NO_TORQUE);
|
||||
updatePeriodicReply(true, imtq::cmdIds::REPLY_WITH_TORQUE);
|
||||
if (goToNormalMode) {
|
||||
setMode(MODE_NORMAL);
|
||||
} else {
|
||||
setMode(_MODE_TO_ON);
|
||||
if (internalState != InternalState::STARTUP) {
|
||||
commandExecuted = false;
|
||||
updatePeriodicReply(true, imtq::cmdIds::REPLY_NO_TORQUE);
|
||||
updatePeriodicReply(true, imtq::cmdIds::REPLY_WITH_TORQUE);
|
||||
internalState = InternalState::STARTUP;
|
||||
}
|
||||
if (internalState == InternalState::STARTUP) {
|
||||
if (commandExecuted) {
|
||||
if (goToNormalMode) {
|
||||
setMode(MODE_NORMAL);
|
||||
} else {
|
||||
setMode(_MODE_TO_ON);
|
||||
}
|
||||
commandExecuted = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -145,6 +154,8 @@ void ImtqHandler::doShutDown() {
|
||||
updatePeriodicReply(false, imtq::cmdIds::REPLY_WITH_TORQUE);
|
||||
specialRequestActive = false;
|
||||
firstReplyCycle = true;
|
||||
internalState = InternalState::NONE;
|
||||
commandExecuted = false;
|
||||
setMode(_MODE_POWER_DOWN);
|
||||
}
|
||||
|
||||
@ -162,8 +173,9 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
|
||||
*id = imtq::cmdIds::REQUEST;
|
||||
request.request = imtq::RequestType::DO_NOTHING;
|
||||
request.specialRequest = imtq::SpecialRequest::NONE;
|
||||
expectedReply = DeviceHandlerIF::NO_COMMAND_ID;
|
||||
rawPacket = reinterpret_cast<uint8_t*>(&request);
|
||||
rawPacketLen = sizeof(ImtqRequest);
|
||||
rawPacketLen = sizeof(imtq::Request);
|
||||
return returnvalue::OK;
|
||||
}
|
||||
}
|
||||
@ -171,6 +183,10 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
|
||||
}
|
||||
|
||||
ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
|
||||
if (internalState == InternalState::STARTUP) {
|
||||
*id = imtq::cmdIds::REQUEST;
|
||||
return buildCommandFromCommand(*id, nullptr, 0);
|
||||
}
|
||||
return NOTHING_TO_SEND;
|
||||
}
|
||||
|
||||
@ -183,7 +199,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
|
||||
expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
|
||||
specialRequestActive = true;
|
||||
rawPacket = reinterpret_cast<uint8_t*>(&request);
|
||||
rawPacketLen = sizeof(ImtqRequest);
|
||||
rawPacketLen = sizeof(imtq::Request);
|
||||
};
|
||||
switch (deviceCommand) {
|
||||
case (imtq::cmdIds::POS_X_SELF_TEST): {
|
||||
@ -219,7 +235,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
|
||||
request.specialRequest = imtq::SpecialRequest::NONE;
|
||||
expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
|
||||
rawPacket = reinterpret_cast<uint8_t*>(&request);
|
||||
rawPacketLen = sizeof(ImtqRequest);
|
||||
rawPacketLen = sizeof(imtq::Request);
|
||||
return returnvalue::OK;
|
||||
}
|
||||
case (imtq::cmdIds::START_ACTUATION_DIPOLE): {
|
||||
@ -258,7 +274,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
|
||||
torquer::TORQUEING = true;
|
||||
torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
|
||||
rawPacket = reinterpret_cast<uint8_t*>(&request);
|
||||
rawPacketLen = sizeof(ImtqRequest);
|
||||
rawPacketLen = sizeof(imtq::Request);
|
||||
return returnvalue::OK;
|
||||
}
|
||||
default:
|
||||
@ -298,13 +314,20 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
|
||||
ReturnValue_t result;
|
||||
ReturnValue_t status = returnvalue::OK;
|
||||
if (getMode() != MODE_NORMAL) {
|
||||
// Ignore replies during transitions.
|
||||
if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) {
|
||||
ImtqRepliesDefault replies(packet);
|
||||
if (replies.devWasConfigured() and internalState == InternalState::STARTUP) {
|
||||
commandExecuted = true;
|
||||
}
|
||||
}
|
||||
return returnvalue::OK;
|
||||
}
|
||||
// arrayprinter::print(packet, ImtqReplies::BASE_LEN);
|
||||
if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) {
|
||||
// sif::debug << "handle measure" << std::endl;
|
||||
ImtqRepliesDefault replies(packet);
|
||||
if (replies.devWasConfigured() and internalState == InternalState::STARTUP) {
|
||||
commandExecuted = true;
|
||||
}
|
||||
if (specialRequestActive) {
|
||||
if (replies.wasSpecialRequestRead()) {
|
||||
uint8_t* specialRequest = replies.getSpecialRequest();
|
||||
@ -347,7 +370,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
|
||||
}
|
||||
|
||||
if (not replies.wasGetRawMgmMeasurementRead() and not firstReplyCycle) {
|
||||
sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
|
||||
sif::warning << "IMTQ: Possible timing issue, raw MGM measurement was not read" << std::endl;
|
||||
}
|
||||
uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
|
||||
result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
|
||||
@ -358,7 +381,8 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
|
||||
}
|
||||
|
||||
if (not replies.wasCalibMgmMeasurementRead() and not firstReplyCycle) {
|
||||
sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
|
||||
sif::warning << "IMTQ: Possible timing issue, calib MGM measurement was not read"
|
||||
<< std::endl;
|
||||
}
|
||||
uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement();
|
||||
result = parseStatusByte(imtq::CC::GET_CAL_MTM_MEASUREMENT, calibMgmMeasurement);
|
||||
|
@ -83,6 +83,11 @@ class ImtqHandler : public DeviceHandlerBase {
|
||||
//! link between IMTQ and OBC.
|
||||
static const Event INVALID_ERROR_BYTE = MAKE_EVENT(8, severity::LOW);
|
||||
|
||||
enum class InternalState { NONE, STARTUP, SHUTDOWN } internalState = InternalState::NONE;
|
||||
bool commandExecuted = false;
|
||||
|
||||
imtq::Request request{};
|
||||
|
||||
imtq::StatusDataset statusSet;
|
||||
imtq::DipoleActuationSet dipoleSet;
|
||||
imtq::RawMtmMeasurementNoTorque rawMtmNoTorque;
|
||||
@ -123,8 +128,7 @@ class ImtqHandler : public DeviceHandlerBase {
|
||||
|
||||
power::Switch_t switcher = power::NO_SWITCH;
|
||||
|
||||
ImtqRequest request{};
|
||||
DeviceCommandId_t expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE;
|
||||
DeviceCommandId_t expectedReply = DeviceHandlerIF::NO_COMMAND_ID;
|
||||
bool goToNormalMode = false;
|
||||
bool debugMode = false;
|
||||
bool specialRequestActive = false;
|
||||
|
@ -10,18 +10,6 @@ class ImtqHandler;
|
||||
|
||||
namespace imtq {
|
||||
|
||||
enum ComStep : uint8_t {
|
||||
DHB_OP = 0,
|
||||
START_MEASURE_SEND = 1,
|
||||
START_MEASURE_GET = 2,
|
||||
READ_MEASURE_SEND = 3,
|
||||
READ_MEASURE_GET = 4,
|
||||
START_ACTUATE_SEND = 5,
|
||||
START_ACTUATE_GET = 6,
|
||||
READ_ACTUATE_SEND = 7,
|
||||
READ_ACTUATE_GET = 8,
|
||||
};
|
||||
|
||||
enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE, DO_NOTHING };
|
||||
|
||||
enum class SpecialRequest : uint8_t {
|
||||
@ -35,6 +23,26 @@ enum class SpecialRequest : uint8_t {
|
||||
GET_SELF_TEST_RESULT = 7
|
||||
};
|
||||
|
||||
struct Request {
|
||||
imtq::RequestType request = imtq::RequestType::MEASURE_NO_ACTUATION;
|
||||
imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE;
|
||||
uint8_t integrationTimeSel = 3;
|
||||
int16_t dipoles[3]{};
|
||||
uint16_t torqueDuration = 0;
|
||||
};
|
||||
|
||||
enum ComStep : uint8_t {
|
||||
DHB_OP = 0,
|
||||
START_MEASURE_SEND = 1,
|
||||
START_MEASURE_GET = 2,
|
||||
READ_MEASURE_SEND = 3,
|
||||
READ_MEASURE_GET = 4,
|
||||
START_ACTUATE_SEND = 5,
|
||||
START_ACTUATE_GET = 6,
|
||||
READ_ACTUATE_SEND = 7,
|
||||
READ_ACTUATE_GET = 8,
|
||||
};
|
||||
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::IMTQ_HANDLER;
|
||||
|
||||
static constexpr ReturnValue_t INVALID_COMMAND_CODE = MAKE_RETURN_CODE(0);
|
||||
@ -1124,25 +1132,21 @@ class NegZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
|
||||
|
||||
} // namespace imtq
|
||||
|
||||
struct ImtqRequest {
|
||||
imtq::RequestType request = imtq::RequestType::MEASURE_NO_ACTUATION;
|
||||
imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE;
|
||||
int16_t dipoles[3]{};
|
||||
uint16_t torqueDuration = 0;
|
||||
};
|
||||
|
||||
struct ImtqRepliesDefault {
|
||||
friend class ImtqPollingTask;
|
||||
|
||||
public:
|
||||
static constexpr size_t BASE_LEN =
|
||||
imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::SYSTEM_STATE + 1 +
|
||||
1 + imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::SYSTEM_STATE + 1 +
|
||||
+imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::RAW_MTM_MEASUREMENT + 1 +
|
||||
imtq::replySize::ENG_HK_DATA_REPLY + 1 + imtq::replySize::CAL_MTM_MEASUREMENT + 1;
|
||||
ImtqRepliesDefault(const uint8_t* rawData) : rawData(const_cast<uint8_t*>(rawData)) {
|
||||
initPointers();
|
||||
}
|
||||
|
||||
void setConfigured() { rawData[0] = true; }
|
||||
bool devWasConfigured() const { return rawData[0]; }
|
||||
|
||||
uint8_t* getCalibMgmMeasurement() const { return calibMgmMeasurement + 1; }
|
||||
bool wasCalibMgmMeasurementRead() const { return calibMgmMeasurement[0]; };
|
||||
|
||||
@ -1164,7 +1168,7 @@ struct ImtqRepliesDefault {
|
||||
|
||||
private:
|
||||
void initPointers() {
|
||||
swReset = rawData;
|
||||
swReset = rawData + 1;
|
||||
systemState = swReset + imtq::replySize::DEFAULT_MIN_LEN + 1;
|
||||
startMtmMeasurement = systemState + imtq::replySize::SYSTEM_STATE + 1;
|
||||
rawMgmMeasurement = startMtmMeasurement + imtq::replySize::DEFAULT_MIN_LEN + 1;
|
||||
@ -1172,6 +1176,7 @@ struct ImtqRepliesDefault {
|
||||
calibMgmMeasurement = engHk + imtq::replySize::ENG_HK_DATA_REPLY + 1;
|
||||
specialRequestReply = calibMgmMeasurement + imtq::replySize::CAL_MTM_MEASUREMENT + 1;
|
||||
}
|
||||
|
||||
uint8_t* rawData;
|
||||
uint8_t* swReset;
|
||||
uint8_t* systemState;
|
||||
|
Loading…
Reference in New Issue
Block a user