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

Merge branch 'smaller-ploc-tweaks' of https://egit.irs.uni-stuttgart.de/eive/eive-obsw into smaller-ploc-tweaks
Some checks are pending
EIVE/eive-obsw/pipeline/pr-main Build queued...
Details

Browse Source
This commit is contained in:
Robin Müller 2024-01-29 10:40:19 +01:00
commit 5c7f712bf2
Signed by: muellerr
GPG Key ID: A649FB78196E3849
14 changed files with 131 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
CHANGELOG.md
View File

@ -16,10 +16,44 @@ will consitute of a breaking change warranting a new major release:
# [unreleased]
## Added
- Added new parameter for MPSoC which allows to skip SUPV commanding.
## Changed
- Increased allowed mode transition time for PLOC SUPV.
## Fixed
- All action commands sent by the PLOC SUPV to itself will have no sender now.
# [v7.5.5] 2024-01-22
## Fixed
- Calculation of error quaternion was done with inverse of the required target quaternion.
# [v7.5.4] 2024-01-16
## Fixed
- Pointing strategy now actually uses fused rotation rate source instead of its valid flag.
- All datasets now get updated during pointing mode, even if the strategy is a fault one.
# [v7.5.3] 2023-12-19
## Fixed
- Set STR quaternions to invalid in device handler if the solution is not trustworthy.
# [v7.5.2] 2023-12-14
## Fixed
- Fixed faulty scaling within the QUEST algorithm.
>>>>>>> 16fa3d1e269cba5af4a22c7ba59e6a8e4a5980f5
# [v7.5.1] 2023-12-13
- `eive-tmtc` v5.12.1

2
CMakeLists.txt
View File

@ -11,7 +11,7 @@ cmake_minimum_required(VERSION 3.13)
set(OBSW_VERSION_MAJOR 7)
set(OBSW_VERSION_MINOR 5)
set(OBSW_VERSION_REVISION 1)
set(OBSW_VERSION_REVISION 5)
# set(CMAKE_VERBOSE TRUE)

22
linux/payload/PlocMpsocHandler.cpp
View File

@ -7,6 +7,7 @@
#include "OBSWConfig.h"
#include "fsfw/datapool/PoolReadGuard.h"
#include "fsfw/globalfunctions/CRC.h"
#include "fsfw/parameters/HasParametersIF.h"
PlocMpsocHandler::PlocMpsocHandler(object_id_t objectId, object_id_t uartComIFid,
CookieIF* comCookie, PlocMpsocSpecialComHelper* plocMPSoCHelper,
@ -1395,6 +1396,9 @@ bool PlocMpsocHandler::handleHwStartup() {
return true;
#endif
if (powerState == PowerState::IDLE) {
if (skipSupvCommandingToOn) {
powerState = PowerState::DONE;
} else {
if (supv::SUPV_ON) {
commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC);
supvTransitionCd.resetTimer();
@ -1405,6 +1409,7 @@ bool PlocMpsocHandler::handleHwStartup() {
setMode(MODE_OFF);
}
}
}
if (powerState == PowerState::SUPV_FAILED) {
setMode(MODE_OFF);
powerState = PowerState::IDLE;
@ -1532,3 +1537,20 @@ ReturnValue_t PlocMpsocHandler::checkModeCommand(Mode_t commandedMode, Submode_t
}
return DeviceHandlerBase::checkModeCommand(commandedMode, commandedSubmode, msToReachTheMode);
}
ReturnValue_t PlocMpsocHandler::getParameter(uint8_t domainId, uint8_t uniqueId,
ParameterWrapper* parameterWrapper,
const ParameterWrapper* newValues,
uint16_t startAtIndex) {
if (uniqueId == mpsoc::ParamId::SKIP_SUPV_ON_COMMANDING) {
uint8_t value = 0;
newValues->getElement(&value);
if (value > 1) {
return HasParametersIF::INVALID_VALUE;
}
parameterWrapper->set(skipSupvCommandingToOn);
return returnvalue::OK;
}
return DeviceHandlerBase::getParameter(domainId, uniqueId, parameterWrapper, newValues,
startAtIndex);
}

5
linux/payload/PlocMpsocHandler.h
View File

@ -201,6 +201,8 @@ class PlocMpsocHandler : public DeviceHandlerBase, public CommandsActionsIF {
PowerState powerState = PowerState::IDLE;
uint8_t skipSupvCommandingToOn = false;
/**
* @brief Handles events received from the PLOC MPSoC helper
*/
@ -316,6 +318,9 @@ class PlocMpsocHandler : public DeviceHandlerBase, public CommandsActionsIF {
pwrctrl::EnablePl enablePl = pwrctrl::EnablePl(objects::POWER_CONTROLLER);
ReturnValue_t checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode,
uint32_t* msToReachTheMode) override;
ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId, ParameterWrapper* parameterWrapper,
const ParameterWrapper* newValues, uint16_t startAtIndex) override;
};
#endif /* BSP_Q7S_DEVICES_PLOC_PLOCMPSOCHANDLER_H_ */

2
linux/payload/plocMpsocHelpers.h
View File

@ -13,6 +13,8 @@
namespace mpsoc {
enum ParamId : uint8_t { SKIP_SUPV_ON_COMMANDING = 0x01 };
enum FileAccessModes : uint8_t {
// Opens a file, fails if the file does not exist.
OPEN_EXISTING = 0x00,

2
linux/payload/plocSupvDefs.h
View File

@ -49,7 +49,7 @@ static const Event SUPV_EXE_ACK_UNKNOWN_COMMAND = MAKE_EVENT(10, severity::LOW);
extern std::atomic_bool SUPV_ON;
static constexpr uint32_t INTER_COMMAND_DELAY = 20;
static constexpr uint32_t BOOT_TIMEOUT_MS = 4000;
static constexpr uint32_t MAX_TRANSITION_TIME_TO_ON_MS = BOOT_TIMEOUT_MS + 2000;
static constexpr uint32_t MAX_TRANSITION_TIME_TO_ON_MS = BOOT_TIMEOUT_MS + 3000;
static constexpr uint32_t MAX_TRANSITION_TIME_TO_OFF_MS = 1000;
namespace result {

32
mission/acs/str/StarTrackerHandler.cpp
View File

@ -1162,7 +1162,7 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
break;
}
case (startracker::REQ_SOLUTION): {
result = handleTm(packet, solutionSet, "REQ_SOLUTION");
result = handleSolution(packet);
break;
}
case (startracker::REQ_CONTRAST): {
@ -2438,6 +2438,36 @@ ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDat
return result;
}
ReturnValue_t StarTrackerHandler::handleSolution(const uint8_t* rawFrame) {
ReturnValue_t result = statusFieldCheck(rawFrame);
if (result != returnvalue::OK) {
return result;
}
PoolReadGuard pg(&solutionSet);
if (pg.getReadResult() != returnvalue::OK) {
return result;
}
const uint8_t* reply = rawFrame + TICKS_OFFSET;
solutionSet.setValidityBufferGeneration(false);
size_t sizeLeft = fullPacketLen;
result = solutionSet.deSerialize(&reply, &sizeLeft, SerializeIF::Endianness::LITTLE);
if (result != returnvalue::OK) {
sif::warning << "StarTrackerHandler::handleTm: Deserialization failed for solution set: 0x"
<< std::hex << std::setw(4) << result << std::dec << std::endl;
}
solutionSet.setValidityBufferGeneration(true);
solutionSet.setValidity(true, true);
solutionSet.caliQw.setValid(solutionSet.isTrustWorthy.value);
solutionSet.caliQx.setValid(solutionSet.isTrustWorthy.value);
solutionSet.caliQy.setValid(solutionSet.isTrustWorthy.value);
solutionSet.caliQz.setValid(solutionSet.isTrustWorthy.value);
solutionSet.trackQw.setValid(solutionSet.isTrustWorthy.value);
solutionSet.trackQx.setValid(solutionSet.isTrustWorthy.value);
solutionSet.trackQy.setValid(solutionSet.isTrustWorthy.value);
solutionSet.trackQz.setValid(solutionSet.isTrustWorthy.value);
return result;
}
ReturnValue_t StarTrackerHandler::handleAutoBlobTm(const uint8_t* rawFrame) {
ReturnValue_t result = statusFieldCheck(rawFrame);
if (result != returnvalue::OK) {

1
mission/acs/str/StarTrackerHandler.h
View File

@ -527,6 +527,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
ReturnValue_t handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset,
const char* context);
ReturnValue_t handleSolution(const uint8_t* rawFrame);
ReturnValue_t handleAutoBlobTm(const uint8_t* rawFrame);
ReturnValue_t handleMatchedCentroidTm(const uint8_t* rawFrame);
ReturnValue_t handleBlobTm(const uint8_t* rawFrame);

24
mission/controller/AcsController.cpp
View File

@ -403,14 +403,17 @@ void AcsController::performPointingCtrl() {
acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid,
attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(),
fusedRotRateData.rotRateSource.isValid(), useMekf);
fusedRotRateData.rotRateSource.value, useMekf);
if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL or
ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
ptgCtrlLostCounter++;
if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) {
triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
ptgCtrlLostCounter = 0;
}
updateCtrlValData(ptgCtrlStrat);
updateActuatorCmdData(ZERO_VEC4, cmdSpeedRws, ZERO_VEC3_INT16);
commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime);
@ -437,7 +440,7 @@ void AcsController::performPointingCtrl() {
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
break;
default:
sif::error << "AcsController: Invalid pointing mode strategy for performDetumble"
sif::error << "AcsController: Invalid pointing mode strategy for performPointingCtrl"
<< std::endl;
break;
}
@ -593,7 +596,7 @@ void AcsController::performPointingCtrl() {
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate, ptgCtrlStrat);
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
@ -671,7 +674,7 @@ void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
}
}
void AcsController::updateCtrlValData(uint8_t safeModeStrat) {
void AcsController::updateCtrlValData(acs::ControlModeStrategy ctrlStrat) {
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
@ -682,13 +685,13 @@ void AcsController::updateCtrlValData(uint8_t safeModeStrat) {
ctrlValData.errAng.setValid(false);
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
ctrlValData.tgtRotRate.setValid(false);
ctrlValData.safeStrat.value = safeModeStrat;
ctrlValData.safeStrat.value = ctrlStrat;
ctrlValData.safeStrat.setValid(true);
ctrlValData.setValidity(true, false);
}
}
void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
void AcsController::updateCtrlValData(double errAng, acs::ControlModeStrategy ctrlStrat) {
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
@ -699,21 +702,22 @@ void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
ctrlValData.errAng.setValid(true);
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
ctrlValData.tgtRotRate.setValid(false);
ctrlValData.safeStrat.value = safeModeStrat;
ctrlValData.safeStrat.value = ctrlStrat;
ctrlValData.safeStrat.setValid(true);
ctrlValData.setValidity(true, false);
}
}
void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng,
const double *tgtRotRate) {
const double *tgtRotRate,
acs::ControlModeStrategy ctrlStrat) {
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng;
std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
ctrlValData.safeStrat.value = acs::ControlModeStrategy::CTRL_OFF;
ctrlValData.safeStrat.value = ctrlStrat;
ctrlValData.setValidity(true, true);
}
}

7
mission/controller/AcsController.h
View File

@ -52,6 +52,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void performPointingCtrl();
private:
static constexpr int16_t ZERO_VEC3_INT16[3] = {0, 0, 0};
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
static constexpr double RW_OFF_TORQUE[4] = {0, 0, 0, 0};
@ -143,10 +144,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void updateActuatorCmdData(const int16_t* mtqTargetDipole);
void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed,
const int16_t* mtqTargetDipole);
void updateCtrlValData(uint8_t safeModeStrat);
void updateCtrlValData(double errAng, uint8_t safeModeStrat);
void updateCtrlValData(acs::ControlModeStrategy ctrlStrat);
void updateCtrlValData(double errAng, acs::ControlModeStrategy ctrlStrat);
void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng,
const double* tgtRotRate);
const double* tgtRotRate, acs::ControlModeStrategy cStrat);
ReturnValue_t updateTle(const uint8_t* line1, const uint8_t* line2, bool fromFile);
ReturnValue_t writeTleToFs(const uint8_t* tle);

10
mission/controller/acs/AttitudeEstimation.cpp
View File

@ -77,24 +77,22 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
qBI[3] = (gamma + alpha) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
// Rotational Vector Part
VectorOperations<double>::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3);
VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(helperSum, beta, qRotVecPt1, 3);
VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 3);
VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 4);
QuaternionOperations::normalize(qBI, qBI);
} else {
// Scalar Part
qBI[3] = (beta) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
// Rotational Vector Part
VectorOperations<double>::mulScalar(helperCross, beta, qRotVecPt0, 3);
VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(helperSum, gamma - alpha, qRotVecPt1, 3);
VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 4);
QuaternionOperations::normalize(qBI, qBI);
}
// Low Pass

4
mission/controller/acs/Guidance.cpp
View File

@ -418,7 +418,9 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
double errorQuat[4], double errorSatRotRate[3], double &errorAngle) {
// First calculate error quaternion between current and target orientation
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
double invTargetQuat[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(targetQuat, invTargetQuat);
QuaternionOperations::multiply(currentQuat, invTargetQuat, errorQuat);
// Last calculate add rotation from reference quaternion
QuaternionOperations::multiply(refQuat, errorQuat, errorQuat);
// Keep scalar part of quaternion positive

3
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -21,9 +21,8 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
return acs::ControlModeStrategy::PTGCTRL_STR;
} else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
return acs::ControlModeStrategy::PTGCTRL_QUEST;
} else {
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,

2
tmtc

@ -1 +1 @@
Subproject commit 747ad34eec5baa5199de49a1330687508c991550
Subproject commit bcdd12caf05b6a874b0d3ac2b9436c4061545312