Merge branch 'main' into smaller-ploc-tweaks
Some checks are pending
EIVE/eive-obsw/pipeline/pr-main Build started...
Some checks are pending
EIVE/eive-obsw/pipeline/pr-main Build started...
This commit is contained in:
commit
16fa3d1e26
33
CHANGELOG.md
33
CHANGELOG.md
@ -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
|
||||
|
@ -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)
|
||||
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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_ */
|
||||
|
@ -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,
|
||||
|
@ -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 {
|
||||
|
@ -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) {
|
||||
|
@ -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);
|
||||
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
@ -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);
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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
2
tmtc
@ -1 +1 @@
|
||||
Subproject commit 747ad34eec5baa5199de49a1330687508c991550
|
||||
Subproject commit bcdd12caf05b6a874b0d3ac2b9436c4061545312
|
Loading…
Reference in New Issue
Block a user