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

that was an evil bug
Some checks are pending
EIVE/eive-obsw/pipeline/pr-develop Build started...
Details

Browse Source
This commit is contained in:
Robin Müller
2023-03-04 17:06:23 +01:00
parent c8a2395d61
commit 4c1b79fd66
8 changed files with 70 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
mission/controller/AcsController.cpp
View File

@ -198,7 +198,7 @@ void AcsController::performSafe() {
updateCtrlValData(errAng);
updateActuatorCmdData(cmdDipolMtqs);
//commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2] /*500, 500, 500 */,
//commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2]/*500, 500, 500*/,
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// acsParameters.rwHandlingParameters.rampTime);
}

43
mission/devices/ImtqHandler.cpp
View File

@ -131,7 +131,8 @@ ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) {
ImtqHandler::~ImtqHandler() = default;
void ImtqHandler::doStartUp() {
updatePeriodicReply(true, imtq::cmdIds::REPLY);
updatePeriodicReply(true, imtq::cmdIds::REPLY_NO_TORQUE);
updatePeriodicReply(true, imtq::cmdIds::REPLY_WITH_TORQUE);
if (goToNormalMode) {
setMode(MODE_NORMAL);
} else {
@ -140,7 +141,8 @@ void ImtqHandler::doStartUp() {
}
void ImtqHandler::doShutDown() {
updatePeriodicReply(false, imtq::cmdIds::REPLY);
updatePeriodicReply(false, imtq::cmdIds::REPLY_NO_TORQUE);
updatePeriodicReply(false, imtq::cmdIds::REPLY_WITH_TORQUE);
specialRequestActive = false;
firstReplyCycle = true;
setMode(_MODE_POWER_DOWN);
@ -157,7 +159,12 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
return buildCommandFromCommand(*id, nullptr, 0);
}
default: {
return NOTHING_TO_SEND;
*id = imtq::cmdIds::REQUEST;
request.request = imtq::RequestType::DO_NOTHING;
request.specialRequest = imtq::SpecialRequest::NONE;
rawPacket = reinterpret_cast<uint8_t*>(&request);
rawPacketLen = sizeof(ImtqRequest);
return returnvalue::OK;
}
}
return NOTHING_TO_SEND;
@ -171,10 +178,9 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
const uint8_t* commandData,
size_t commandDataLen) {
auto genericSpecialRequest = [&](imtq::SpecialRequest specialRequest) {
// ImtqRequest request(commandBuffer, sizeof(commandBuffer));
request.request = imtq::RequestType::MEASURE_NO_ACTUATION;
request.specialRequest = specialRequest;
// request.setMeasureRequest(specialRequest);
expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
specialRequestActive = true;
rawPacket = reinterpret_cast<uint8_t*>(&request);
rawPacketLen = sizeof(ImtqRequest);
@ -211,17 +217,17 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
case (imtq::cmdIds::REQUEST): {
request.request = imtq::RequestType::MEASURE_NO_ACTUATION;
request.specialRequest = imtq::SpecialRequest::NONE;
expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
rawPacket = reinterpret_cast<uint8_t*>(&request);
rawPacketLen = sizeof(ImtqRequest);
return returnvalue::OK;
}
case (imtq::cmdIds::START_ACTUATION_DIPOLE): {
/* IMTQ expects low byte first */
// commandBuffer[0] = imtq::CC::START_ACTUATION_DIPOLE;
if (commandData != nullptr && commandDataLen < 8) {
return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
}
{
// Do this in any case to read values which might be commanded by the ACS controller.
PoolReadGuard pg(&dipoleSet);
// Commands override anything which was set in the software
if (commandData != nullptr) {
@ -237,6 +243,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
}
}
expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE;
request.request = imtq::RequestType::ACTUATE;
request.specialRequest = imtq::SpecialRequest::NONE;
std::memcpy(request.dipoles, dipoleSet.dipoles.value, sizeof(request.dipoles));
@ -263,7 +270,8 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
void ImtqHandler::fillCommandAndReplyMap() {
insertInCommandMap(imtq::cmdIds::REQUEST);
insertInCommandMap(imtq::cmdIds::START_ACTUATION_DIPOLE);
insertInReplyMap(imtq::cmdIds::REPLY, 5, nullptr, 0, true);
insertInReplyMap(imtq::cmdIds::REPLY_NO_TORQUE, 5, nullptr, 0, true);
insertInReplyMap(imtq::cmdIds::REPLY_WITH_TORQUE, 20, nullptr, 0, true);
insertInCommandMap(imtq::cmdIds::POS_X_SELF_TEST);
insertInCommandMap(imtq::cmdIds::NEG_X_SELF_TEST);
insertInCommandMap(imtq::cmdIds::POS_Y_SELF_TEST);
@ -280,7 +288,7 @@ ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSi
}
if (remainingSize > 0) {
*foundLen = remainingSize;
*foundId = imtq::cmdIds::REPLY;
*foundId = expectedReply;
return returnvalue::OK;
}
return returnvalue::FAILED;
@ -294,7 +302,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
return returnvalue::OK;
}
// arrayprinter::print(packet, ImtqReplies::BASE_LEN);
if (requestStep == imtq::RequestType::MEASURE_NO_ACTUATION) {
if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) {
// sif::debug << "handle measure" << std::endl;
ImtqRepliesDefault replies(packet);
if (specialRequestActive) {
@ -359,7 +367,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
} else {
status = result;
}
} else if (requestStep == imtq::RequestType::ACTUATE) {
} else if (expectedReply == imtq::cmdIds::REPLY_WITH_TORQUE) {
// sif::debug << "handle measure with torque" << std::endl;
ImtqRepliesWithTorque replies(packet);
if (replies.wasDipoleActuationRead()) {
@ -405,6 +413,8 @@ LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) {
return &hkDatasetNoTorque;
} else if (sid == dipoleSet.getSid()) {
return &dipoleSet;
} else if (sid == statusSet.getSid()) {
return &statusSet;
} else if (sid == hkDatasetWithTorque.getSid()) {
return &hkDatasetWithTorque;
} else if (sid == rawMtmWithTorque.getSid()) {
@ -747,6 +757,10 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat
subdp::DiagnosticsHkPeriodicParams(rawMtmWithTorque.getSid(), false, 10.0));
poolManager.subscribeForDiagPeriodicPacket(
subdp::DiagnosticsHkPeriodicParams(calMtmMeasurementSet.getSid(), false, 10.0));
poolManager.subscribeForRegularPeriodicPacket(
subdp::RegularHkPeriodicParams(statusSet.getSid(), false, 10.0));
poolManager.subscribeForDiagPeriodicPacket(
subdp::DiagnosticsHkPeriodicParams(dipoleSet.getSid(), false, 10.0));
return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager);
}
@ -793,10 +807,12 @@ ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t*
<< " has invalid parameter" << std::endl;
return imtq::PARAMETER_INVALID;
case 5:
sif::error << "IMTQ::parseStatusByte: CC 0x" << std::setw(2) << " unavailable" << std::endl;
sif::error << "IMTQ::parseStatusByte: CC 0x" << std::setw(2) << command << " unavailable"
<< std::endl;
return imtq::CC_UNAVAILABLE;
case 7:
sif::error << "IMTQ::parseStatusByte: IMTQ replied internal processing error" << std::endl;
sif::error << "IMTQ::parseStatusByte: Internal processing error for command 0x"
<< std::setw(2) << command << std::endl;
return imtq::INTERNAL_PROCESSING_ERROR;
default:
sif::error << "IMTQ::parseStatusByte: CMD error field for command 0x" << std::setw(2)
@ -930,6 +946,7 @@ void ImtqHandler::fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8
set.setValidity(true, true);
if (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::info << "Set ID: " << set.getSid().ownerSetId << std::endl;
sif::info << "IMTQ raw MTM measurement X: " << set.mtmRawNt[0] << " nT" << std::endl;
sif::info << "IMTQ raw MTM measurement Y: " << set.mtmRawNt[1] << " nT" << std::endl;
sif::info << "IMTQ raw MTM measurement Z: " << set.mtmRawNt[2] << " nT" << std::endl;

2
mission/devices/ImtqHandler.h
View File

@ -123,8 +123,8 @@ class ImtqHandler : public DeviceHandlerBase {
power::Switch_t switcher = power::NO_SWITCH;
// uint8_t commandBuffer[imtq::MAX_COMMAND_SIZE];
ImtqRequest request{};
DeviceCommandId_t expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE;
bool goToNormalMode = false;
bool debugMode = false;
bool specialRequestActive = false;

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

@ -53,7 +53,8 @@ static const ReturnValue_t UNEXPECTED_SELF_TEST_REPLY = MAKE_RETURN_CODE(0xA7);
namespace cmdIds {
static constexpr DeviceCommandId_t REQUEST = 0x70;
static constexpr DeviceCommandId_t REPLY = 0x71;
static constexpr DeviceCommandId_t REPLY_NO_TORQUE = 0x71;
static constexpr DeviceCommandId_t REPLY_WITH_TORQUE = 0x72;
static const DeviceCommandId_t START_ACTUATION_DIPOLE = 0x2;
static const DeviceCommandId_t POS_X_SELF_TEST = 0x7;
static const DeviceCommandId_t NEG_X_SELF_TEST = 0x8;