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...

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

View File

@ -16,10 +16,44 @@ will consitute of a breaking change warranting a new major release:
# [unreleased] # [unreleased]
## Added
- Added new parameter for MPSoC which allows to skip SUPV commanding.
## Changed
- Increased allowed mode transition time for PLOC SUPV.
## Fixed ## Fixed
- All action commands sent by the PLOC SUPV to itself will have no sender now. - 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 # [v7.5.1] 2023-12-13
- `eive-tmtc` v5.12.1 - `eive-tmtc` v5.12.1

View File

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

View File

@ -7,6 +7,7 @@
#include "OBSWConfig.h" #include "OBSWConfig.h"
#include "fsfw/datapool/PoolReadGuard.h" #include "fsfw/datapool/PoolReadGuard.h"
#include "fsfw/globalfunctions/CRC.h" #include "fsfw/globalfunctions/CRC.h"
#include "fsfw/parameters/HasParametersIF.h"
PlocMpsocHandler::PlocMpsocHandler(object_id_t objectId, object_id_t uartComIFid, PlocMpsocHandler::PlocMpsocHandler(object_id_t objectId, object_id_t uartComIFid,
CookieIF* comCookie, PlocMpsocSpecialComHelper* plocMPSoCHelper, CookieIF* comCookie, PlocMpsocSpecialComHelper* plocMPSoCHelper,
@ -1395,14 +1396,18 @@ bool PlocMpsocHandler::handleHwStartup() {
return true; return true;
#endif #endif
if (powerState == PowerState::IDLE) { if (powerState == PowerState::IDLE) {
if (supv::SUPV_ON) { if (skipSupvCommandingToOn) {
commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC); powerState = PowerState::DONE;
supvTransitionCd.resetTimer();
powerState = PowerState::PENDING_STARTUP;
} else { } else {
triggerEvent(SUPV_NOT_ON, 1); if (supv::SUPV_ON) {
// Set back to OFF for now, failing the transition. commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC);
setMode(MODE_OFF); 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) { if (powerState == PowerState::SUPV_FAILED) {
@ -1532,3 +1537,20 @@ ReturnValue_t PlocMpsocHandler::checkModeCommand(Mode_t commandedMode, Submode_t
} }
return DeviceHandlerBase::checkModeCommand(commandedMode, commandedSubmode, msToReachTheMode); 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);
}

View File

@ -201,6 +201,8 @@ class PlocMpsocHandler : public DeviceHandlerBase, public CommandsActionsIF {
PowerState powerState = PowerState::IDLE; PowerState powerState = PowerState::IDLE;
uint8_t skipSupvCommandingToOn = false;
/** /**
* @brief Handles events received from the PLOC MPSoC helper * @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); pwrctrl::EnablePl enablePl = pwrctrl::EnablePl(objects::POWER_CONTROLLER);
ReturnValue_t checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode, ReturnValue_t checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode,
uint32_t* msToReachTheMode) override; 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_ */ #endif /* BSP_Q7S_DEVICES_PLOC_PLOCMPSOCHANDLER_H_ */

View File

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

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; extern std::atomic_bool SUPV_ON;
static constexpr uint32_t INTER_COMMAND_DELAY = 20; static constexpr uint32_t INTER_COMMAND_DELAY = 20;
static constexpr uint32_t BOOT_TIMEOUT_MS = 4000; 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; static constexpr uint32_t MAX_TRANSITION_TIME_TO_OFF_MS = 1000;
namespace result { namespace result {

View File

@ -1162,7 +1162,7 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
break; break;
} }
case (startracker::REQ_SOLUTION): { case (startracker::REQ_SOLUTION): {
result = handleTm(packet, solutionSet, "REQ_SOLUTION"); result = handleSolution(packet);
break; break;
} }
case (startracker::REQ_CONTRAST): { case (startracker::REQ_CONTRAST): {
@ -2438,6 +2438,36 @@ ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDat
return result; 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 StarTrackerHandler::handleAutoBlobTm(const uint8_t* rawFrame) {
ReturnValue_t result = statusFieldCheck(rawFrame); ReturnValue_t result = statusFieldCheck(rawFrame);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {

View File

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

View File

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

View File

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

View File

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

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 targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
double errorQuat[4], double errorSatRotRate[3], double &errorAngle) { double errorQuat[4], double errorSatRotRate[3], double &errorAngle) {
// First calculate error quaternion between current and target orientation // 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 // Last calculate add rotation from reference quaternion
QuaternionOperations::multiply(refQuat, errorQuat, errorQuat); QuaternionOperations::multiply(refQuat, errorQuat, errorQuat);
// Keep scalar part of quaternion positive // Keep scalar part of quaternion positive

View File

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

2
tmtc

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