eive/eive-obsw
eive
/
eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests 1 Releases 117 Wiki Activity

Merge branch 'main' into smaller-ploc-tweaks
EIVE/eive-obsw/pipeline/pr-main Build started... Details

This commit is contained in:
Robin Müller 2024-01-29 10:38:54 +01:00
parent 561fe5aa3c c7ac5904f8
commit 16fa3d1e26
14 changed files with 130 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
CHANGELOG.md
View File

@ -16,6 +16,39 @@ 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.
# [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.
# [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)

36
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,14 +1396,18 @@ bool PlocMpsocHandler::handleHwStartup() {
return true;
#endif
if (powerState == PowerState::IDLE) {
if (supv::SUPV_ON) {
commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC);
supvTransitionCd.resetTimer();
powerState = PowerState::PENDING_STARTUP;
if (skipSupvCommandingToOn) {
powerState = PowerState::DONE;
} else {
triggerEvent(SUPV_NOT_ON, 1);
// Set back to OFF for now, failing the transition.
setMode(MODE_OFF);
if (supv::SUPV_ON) {
commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC);
supvTransitionCd.resetTimer();
powerState = PowerState::PENDING_STARTUP;
} else {
triggerEvent(SUPV_NOT_ON, 1);
// Set back to OFF for now, failing the transition.
setMode(MODE_OFF);
}
}
}
if (powerState == PowerState::SUPV_FAILED) {
@ -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