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]
|
# [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
|
# [v7.5.1] 2023-12-13
|
||||||
|
|
||||||
- `eive-tmtc` v5.12.1
|
- `eive-tmtc` v5.12.1
|
||||||
|
@ -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)
|
||||||
|
|
||||||
|
@ -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);
|
||||||
|
}
|
||||||
|
@ -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_ */
|
||||||
|
@ -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,
|
||||||
|
@ -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 {
|
||||||
|
@ -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) {
|
||||||
|
@ -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);
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -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);
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
2
tmtc
@ -1 +1 @@
|
|||||||
Subproject commit 747ad34eec5baa5199de49a1330687508c991550
|
Subproject commit bcdd12caf05b6a874b0d3ac2b9436c4061545312
|
Loading…
Reference in New Issue
Block a user