eive-obsw/mission/controller/AcsController.cpp

1164 lines
51 KiB
C++
Raw Normal View History

2022-08-15 09:58:18 +02:00
#include "AcsController.h"
2022-08-12 12:29:28 +02:00
2023-11-29 16:59:03 +01:00
AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMountedIF &sdcMan)
: ExtendedControllerBase(objectId),
2023-03-26 15:28:00 +02:00
enableHkSets(enableHkSets),
2023-11-29 16:59:03 +01:00
sdcMan(sdcMan),
2023-11-23 16:56:36 +01:00
attitudeEstimation(&acsParameters),
2023-07-20 11:36:44 +02:00
fusedRotationEstimation(&acsParameters),
guidance(&acsParameters),
2022-10-20 11:07:45 +02:00
safeCtrl(&acsParameters),
ptgCtrl(&acsParameters),
parameterHelper(this),
mgmDataRaw(this),
mgmDataProcessed(this),
susDataRaw(this),
susDataProcessed(this),
gyrDataRaw(this),
gyrDataProcessed(this),
gpsDataProcessed(this),
2023-11-23 16:56:36 +01:00
attitudeEstimationData(this),
ctrlValData(this),
actuatorCmdData(this),
2023-08-07 10:43:00 +02:00
fusedRotRateData(this),
fusedRotRateSourcesData(this) {}
2022-08-12 12:29:28 +02:00
2023-02-17 11:11:48 +01:00
ReturnValue_t AcsController::initialize() {
ReturnValue_t result = parameterHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
return ExtendedControllerBase::initialize();
}
2023-02-08 13:40:03 +01:00
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
ReturnValue_t result = actionHelper.handleActionMessage(message);
if (result == returnvalue::OK) {
return result;
}
result = parameterHelper.handleParameterMessage(message);
if (result == returnvalue::OK) {
return result;
}
return result;
}
2023-02-22 16:10:30 +01:00
ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
const uint8_t *data, size_t size) {
switch (actionId) {
2023-02-22 17:10:42 +01:00
case SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL: {
2023-02-23 11:26:43 +01:00
ReturnValue_t result = guidance.solarArrayDeploymentComplete();
if (result == returnvalue::FAILED) {
return FILE_DELETION_FAILED;
}
2023-02-22 17:10:42 +01:00
return HasActionsIF::EXECUTION_FINISHED;
}
case RESET_MEKF: {
2023-11-23 16:56:36 +01:00
navigation.resetMekf(&attitudeEstimationData);
2023-02-22 16:10:30 +01:00
return HasActionsIF::EXECUTION_FINISHED;
}
case RESTORE_MEKF_NONFINITE_RECOVERY: {
mekfLost = false;
return HasActionsIF::EXECUTION_FINISHED;
}
2023-08-07 10:43:00 +02:00
case UPDATE_TLE: {
if (size != 69 * 2) {
return INVALID_PARAMETERS;
}
2023-08-23 15:21:40 +02:00
ReturnValue_t result = updateTle(data, data + 69, false);
2023-08-07 10:43:00 +02:00
if (result != returnvalue::OK) {
return result;
}
2023-08-23 15:21:40 +02:00
result = writeTleToFs(data);
if (result != returnvalue::OK) {
return result;
2023-08-07 10:43:00 +02:00
}
return HasActionsIF::EXECUTION_FINISHED;
}
2023-12-01 11:42:27 +01:00
case (READ_TLE): {
uint8_t tle[69 * 2] = {};
uint8_t line2[69] = {};
ReturnValue_t result = readTleFromFs(tle, line2);
if (result != returnvalue::OK) {
return result;
}
std::memcpy(tle + 69, line2, 69);
actionHelper.reportData(commandedBy, actionId, tle, 69 * 2);
return EXECUTION_FINISHED;
}
2023-02-22 16:10:30 +01:00
default: {
return HasActionsIF::INVALID_ACTION_ID;
}
}
}
MessageQueueId_t AcsController::getCommandQueue() const { return commandQueue->getId(); }
ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId,
2023-02-08 13:40:03 +01:00
ParameterWrapper *parameterWrapper,
const ParameterWrapper *newValues,
uint16_t startAtIndex) {
return acsParameters.getParameter(domainId, parameterId, parameterWrapper, newValues,
startAtIndex);
2022-08-15 09:58:18 +02:00
}
2022-08-12 12:29:28 +02:00
2022-08-15 10:50:19 +02:00
void AcsController::performControlOperation() {
2023-02-14 10:59:35 +01:00
#if OBSW_THREAD_TRACING == 1
trace::threadTrace(opCounter, "ACS & TCS PST");
#endif
{
PoolReadGuard pg(&mgmDataRaw);
if (pg.getReadResult() == returnvalue::OK) {
copyMgmData();
}
}
{
PoolReadGuard pg(&susDataRaw);
if (pg.getReadResult() == returnvalue::OK) {
copySusData();
}
}
{
PoolReadGuard pg(&gyrDataRaw);
if (pg.getReadResult() == returnvalue::OK) {
copyGyrData();
}
}
2022-08-15 10:50:19 +02:00
switch (internalState) {
case InternalState::STARTUP: {
initialCountdown.resetTimer();
internalState = InternalState::INITIAL_DELAY;
return;
}
case InternalState::INITIAL_DELAY: {
if (initialCountdown.hasTimedOut()) {
2023-08-23 15:21:40 +02:00
uint8_t line1[69] = {};
uint8_t line2[69] = {};
readTleFromFs(line1, line2);
updateTle(line1, line2, true);
2022-08-15 10:50:19 +02:00
internalState = InternalState::READY;
}
return;
}
case InternalState::READY: {
if (mode != MODE_OFF) {
performAttitudeControl();
}
2022-08-15 10:50:19 +02:00
break;
}
default:
break;
}
2023-01-23 16:34:52 +01:00
}
void AcsController::performAttitudeControl() {
Clock::getClock_timeval(&timeAbsolute);
Clock::getClockMonotonic(&timeRelative);
2023-01-23 16:34:52 +01:00
2023-10-16 13:26:56 +02:00
if (timeRelative.tv_sec != 0 and oldTimeRelative.tv_sec != 0) {
timeDelta = timevalOperations::toDouble(timeRelative - oldTimeRelative);
}
oldTimeRelative = timeRelative;
ReturnValue_t result = navigation.useSpg4(timeAbsolute, &gpsDataProcessed);
2023-08-09 13:41:25 +02:00
if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
triggerEvent(acs::TLE_TOO_OLD);
tleTooOldFlag = true;
} else if (result != Sgp4Propagator::TLE_TOO_OLD) {
tleTooOldFlag = false;
}
2023-10-16 13:26:56 +02:00
sensorProcessing.process(timeAbsolute, timeDelta, &sensorValues, &mgmDataProcessed,
&susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
2023-11-23 16:56:36 +01:00
attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData);
fusedRotationEstimation.estimateFusedRotationRate(
&susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
2023-11-24 11:30:27 +01:00
&attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
2023-08-09 13:41:25 +02:00
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
2023-11-23 16:56:36 +01:00
&susDataProcessed, &attitudeEstimationData, &acsParameters);
2023-11-14 16:57:55 +01:00
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
2023-11-23 16:56:36 +01:00
triggerEvent(acs::MEKF_INVALID_INFO,
static_cast<uint32_t>(attitudeEstimationData.mekfStatus.value));
2023-11-14 16:57:55 +01:00
mekfInvalidFlag = true;
}
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
2023-11-23 16:56:36 +01:00
navigation.resetMekf(&attitudeEstimationData);
2023-11-14 16:57:55 +01:00
mekfLost = true;
}
} else if (mekfInvalidFlag) {
triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false;
}
switch (mode) {
case acs::SAFE:
switch (submode) {
case SUBMODE_NONE:
performSafe();
break;
case acs::DETUMBLE:
performDetumble();
break;
}
break;
case acs::PTG_IDLE:
case acs::PTG_TARGET:
case acs::PTG_TARGET_GS:
case acs::PTG_NADIR:
case acs::PTG_INERTIAL:
performPointingCtrl();
break;
}
}
2023-03-24 15:18:52 +01:00
void AcsController::performSafe() {
2023-03-24 14:52:12 +01:00
// get desired satellite rate, sun direction to align to and inertia
2023-04-14 11:37:23 +02:00
double sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir);
2023-03-24 14:52:12 +01:00
2023-01-23 16:34:52 +01:00
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
2023-11-02 16:59:09 +01:00
acs::ControlModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
2023-03-24 14:52:12 +01:00
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf,
2023-07-19 15:30:54 +02:00
acsParameters.safeModeControllerParameters.useGyr,
acsParameters.safeModeControllerParameters.dampingDuringEclipse);
2023-03-24 14:52:12 +01:00
switch (safeCtrlStrat) {
2023-11-02 16:59:09 +01:00
case (acs::ControlModeStrategy::SAFECTRL_MEKF):
2023-11-23 16:56:36 +01:00
safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value,
attitudeEstimationData.satRotRateMekf.value,
susDataProcessed.sunIjkModel.value, attitudeEstimationData.quatMekf.value,
sunTargetDir, magMomMtq, errAng);
2023-03-15 10:06:24 +01:00
safeCtrlFailureFlag = false;
2023-03-17 16:16:53 +01:00
safeCtrlFailureCounter = 0;
2023-03-24 14:52:12 +01:00
break;
2023-11-02 16:59:09 +01:00
case (acs::ControlModeStrategy::SAFECTRL_GYR):
2023-07-19 13:24:37 +02:00
safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
2023-07-19 15:30:54 +02:00
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
2023-03-24 14:52:12 +01:00
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
2023-11-02 16:59:09 +01:00
case (acs::ControlModeStrategy::SAFECTRL_SUSMGM):
safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value,
fusedRotRateData.rotRateParallel.value,
fusedRotRateData.rotRateOrthogonal.value,
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
2023-07-19 13:24:37 +02:00
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
2023-11-02 16:59:09 +01:00
case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR):
2023-07-19 15:30:54 +02:00
safeCtrl.safeRateDampingGyr(mgmDataProcessed.mgmVecTot.value,
gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq,
errAng);
2023-03-24 14:52:12 +01:00
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
2023-11-02 16:59:09 +01:00
case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value,
fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq,
errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
2023-11-02 16:59:09 +01:00
case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING):
2023-04-11 18:56:10 +02:00
errAng = NAN;
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
2023-11-14 13:22:35 +01:00
case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL):
2023-04-06 10:47:26 +02:00
safeCtrlFailure(1, 0);
2023-03-24 14:52:12 +01:00
break;
2023-11-14 13:22:35 +01:00
case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL):
2023-04-06 10:47:26 +02:00
safeCtrlFailure(0, 1);
2023-03-24 14:52:12 +01:00
break;
2023-04-27 09:35:38 +02:00
default:
sif::error << "AcsController: Invalid safe mode strategy for performSafe" << std::endl;
break;
2022-10-20 11:07:45 +02:00
}
2023-04-27 10:33:43 +02:00
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
2022-10-20 11:07:45 +02:00
2023-02-22 14:50:38 +01:00
// detumble check and switch
2023-07-20 14:22:45 +02:00
if (acsParameters.safeModeControllerParameters.useMekf) {
2023-11-23 16:56:36 +01:00
if (attitudeEstimationData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) >
2023-07-20 14:22:45 +02:00
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
}
} else if (acsParameters.safeModeControllerParameters.useGyr) {
if (gyrDataProcessed.gyrVecTot.isValid() and
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
}
} else if (fusedRotRateData.rotRateTotal.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) >
2023-01-23 16:34:52 +01:00
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
} else if (detumbleCounter > 0) {
detumbleCounter -= 1;
2023-01-23 16:34:52 +01:00
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
detumbleCounter = 0;
2023-02-03 14:31:22 +01:00
// Triggers detumble mode transition in subsystem
triggerEvent(acs::SAFE_RATE_VIOLATION);
2023-03-10 15:21:36 +01:00
startTransition(mode, acs::SafeSubmode::DETUMBLE);
2023-01-23 16:34:52 +01:00
}
updateCtrlValData(errAng, safeCtrlStrat);
2023-04-27 10:33:43 +02:00
updateActuatorCmdData(cmdDipoleMtqs);
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
2023-04-14 16:50:02 +02:00
acsParameters.magnetorquerParameter.torqueDuration);
2023-01-23 16:34:52 +01:00
}
2022-08-12 12:29:28 +02:00
2023-01-23 16:34:52 +01:00
void AcsController::performDetumble() {
2023-11-02 16:59:09 +01:00
acs::ControlModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
2023-04-14 11:38:05 +02:00
mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
mgmDataProcessed.mgmVecTotDerivative.isValid(),
acsParameters.detumbleParameter.useFullDetumbleLaw);
2023-01-23 16:34:52 +01:00
double magMomMtq[3] = {0, 0, 0};
2023-04-06 10:47:26 +02:00
switch (safeCtrlStrat) {
2023-11-02 16:59:09 +01:00
case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL):
2023-04-06 10:47:26 +02:00
detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value,
2023-04-14 11:38:05 +02:00
magMomMtq, acsParameters.detumbleParameter.gainFull);
2023-04-06 10:47:26 +02:00
break;
2023-11-02 16:59:09 +01:00
case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
2023-04-06 10:47:26 +02:00
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value,
2023-04-14 11:38:05 +02:00
magMomMtq, acsParameters.detumbleParameter.gainBdot);
2023-04-06 10:47:26 +02:00
break;
2023-11-14 13:22:35 +01:00
case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL):
2023-04-06 10:47:26 +02:00
safeCtrlFailure(1, 0);
break;
2023-11-14 13:22:35 +01:00
case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL):
2023-04-06 10:47:26 +02:00
safeCtrlFailure(0, 1);
break;
2023-04-27 09:35:38 +02:00
default:
sif::error << "AcsController: Invalid safe mode strategy for performDetumble" << std::endl;
break;
2023-04-06 10:47:26 +02:00
}
2023-04-27 10:33:43 +02:00
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
2023-01-23 16:34:52 +01:00
2023-07-20 14:43:43 +02:00
if (acsParameters.safeModeControllerParameters.useMekf) {
2023-11-23 16:56:36 +01:00
if (attitudeEstimationData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) <
2023-08-02 10:22:15 +02:00
acsParameters.detumbleParameter.omegaDetumbleEnd) {
2023-07-20 14:43:43 +02:00
detumbleCounter++;
}
} else if (acsParameters.safeModeControllerParameters.useGyr) {
if (gyrDataProcessed.gyrVecTot.isValid() and
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
2023-08-02 10:22:15 +02:00
acsParameters.detumbleParameter.omegaDetumbleEnd) {
2023-07-20 14:43:43 +02:00
detumbleCounter++;
}
} else if (fusedRotRateData.rotRateTotal.isValid() and
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
2023-08-02 10:22:15 +02:00
acsParameters.detumbleParameter.omegaDetumbleEnd) {
2023-01-23 16:34:52 +01:00
detumbleCounter++;
} else if (detumbleCounter > 0) {
detumbleCounter -= 1;
2023-01-23 16:34:52 +01:00
}
2023-07-20 14:43:43 +02:00
2023-01-23 16:34:52 +01:00
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
detumbleCounter = 0;
2023-02-03 14:21:36 +01:00
// Triggers safe mode transition in subsystem
triggerEvent(acs::SAFE_RATE_RECOVERY);
2023-03-10 15:21:36 +01:00
startTransition(mode, acs::SafeSubmode::DEFAULT);
2023-01-23 16:34:52 +01:00
}
updateCtrlValData(safeCtrlStrat);
2023-04-27 10:33:43 +02:00
updateActuatorCmdData(cmdDipoleMtqs);
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
2023-04-14 16:50:02 +02:00
acsParameters.magnetorquerParameter.torqueDuration);
2023-01-23 16:34:52 +01:00
}
2023-01-23 16:34:52 +01:00
void AcsController::performPointingCtrl() {
2023-11-23 16:56:36 +01:00
bool strValid = (sensorValues.strSet.caliQw.isValid() and sensorValues.strSet.caliQx.isValid() and
sensorValues.strSet.caliQy.isValid() and sensorValues.strSet.caliQz.isValid());
2023-11-24 11:30:27 +01:00
uint8_t useMekf = false;
switch (mode) {
case acs::PTG_IDLE:
useMekf = acsParameters.idleModeControllerParameters.useMekf;
break;
case acs::PTG_TARGET:
useMekf = acsParameters.targetModeControllerParameters.useMekf;
break;
case acs::PTG_TARGET_GS:
useMekf = acsParameters.gsTargetModeControllerParameters.useMekf;
break;
case acs::PTG_NADIR:
useMekf = acsParameters.nadirModeControllerParameters.useMekf;
break;
case acs::PTG_INERTIAL:
useMekf = acsParameters.inertialModeControllerParameters.useMekf;
break;
}
acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid,
attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(),
2024-01-16 15:51:37 +01:00
fusedRotRateData.rotRateSource.value, useMekf);
2023-11-23 16:56:36 +01:00
2024-01-16 16:35:45 +01:00
if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL or
ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
2023-11-24 11:30:27 +01:00
ptgCtrlLostCounter++;
if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) {
triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
ptgCtrlLostCounter = 0;
}
updateCtrlValData(ptgCtrlStrat);
updateActuatorCmdData(ZERO_VEC4, cmdSpeedRws, ZERO_VEC3_INT16);
2023-11-24 11:30:27 +01:00
commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime);
return;
2023-11-24 11:32:15 +01:00
} else {
ptgCtrlLostCounter = 0;
2023-11-24 11:30:27 +01:00
}
2023-11-15 16:54:54 +01:00
double quatBI[4] = {0, 0, 0, 0}, rotRateB[3] = {0, 0, 0};
switch (ptgCtrlStrat) {
case acs::ControlModeStrategy::PTGCTRL_MEKF:
std::memcpy(quatBI, attitudeEstimationData.quatMekf.value, sizeof(quatBI));
std::memcpy(rotRateB, attitudeEstimationData.satRotRateMekf.value, sizeof(rotRateB));
break;
case acs::ControlModeStrategy::PTGCTRL_STR:
quatBI[0] = sensorValues.strSet.caliQx.value;
quatBI[1] = sensorValues.strSet.caliQy.value;
quatBI[2] = sensorValues.strSet.caliQz.value;
quatBI[3] = sensorValues.strSet.caliQw.value;
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
break;
case acs::ControlModeStrategy::PTGCTRL_QUEST:
std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI));
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
break;
2023-11-27 10:37:40 +01:00
default:
sif::error << "AcsController: Invalid pointing mode strategy for performPointingCtrl"
2023-11-27 10:37:40 +01:00
<< std::endl;
break;
}
2023-01-23 16:34:52 +01:00
uint8_t enableAntiStiction = true;
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
if (result == returnvalue::FAILED) {
2023-06-05 09:43:31 +02:00
if (multipleRwUnavailableCounter >=
acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
triggerEvent(acs::MULTIPLE_RW_INVALID);
2023-06-05 16:08:53 +02:00
multipleRwUnavailableCounter = 0;
}
2023-02-22 14:50:38 +01:00
multipleRwUnavailableCounter++;
return;
} else {
multipleRwUnavailableCounter = 0;
}
2023-01-23 16:34:52 +01:00
2023-02-17 15:57:07 +01:00
// Variables required for guidance
2023-02-20 16:00:17 +01:00
double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1},
errorAngle = 0, errorSatRotRate[3] = {0, 0, 0};
2023-03-10 17:31:12 +01:00
// Variables required for setting actuators
double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0},
mgtDpDes[3] = {0, 0, 0};
2023-06-02 17:41:25 +02:00
switch (mode) {
case acs::PTG_IDLE:
2023-10-16 13:26:56 +02:00
guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat,
2023-06-02 17:41:25 +02:00
targetSatRotRate);
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
rwTrqNs);
2023-01-23 16:34:52 +01:00
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
2023-03-10 17:31:12 +01:00
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
2023-01-23 16:34:52 +01:00
ptgCtrl.ptgDesaturation(
2023-02-20 15:59:01 +01:00
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
2023-02-20 15:59:01 +01:00
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
2023-01-23 16:34:52 +01:00
break;
case acs::PTG_TARGET:
2023-10-16 13:26:56 +02:00
guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat,
targetSatRotRate);
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
2023-11-23 16:56:36 +01:00
acsParameters.targetModeControllerParameters.quatRef,
acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
2023-01-23 16:34:52 +01:00
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
rwTrqNs);
2023-01-23 16:34:52 +01:00
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
2023-03-10 17:31:12 +01:00
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
2023-01-23 16:34:52 +01:00
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
2023-02-20 15:59:01 +01:00
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
2023-01-23 16:34:52 +01:00
break;
case acs::PTG_TARGET_GS:
2023-10-16 13:26:56 +02:00
guidance.targetQuatPtgGs(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
2023-02-20 15:59:01 +01:00
susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
2023-01-23 16:34:52 +01:00
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
rwTrqNs);
2023-01-23 16:34:52 +01:00
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
2023-03-10 17:31:12 +01:00
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
2023-01-23 16:34:52 +01:00
ptgCtrl.ptgDesaturation(
2023-02-28 15:01:07 +01:00
&acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
2023-02-28 15:01:07 +01:00
enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
2023-01-23 16:34:52 +01:00
break;
case acs::PTG_NADIR:
2023-10-16 13:26:56 +02:00
guidance.targetQuatPtgNadirThreeAxes(
timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate);
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
2023-11-23 16:56:36 +01:00
acsParameters.nadirModeControllerParameters.quatRef,
2023-02-20 16:20:54 +01:00
acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle);
2023-02-20 15:59:01 +01:00
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
2023-01-23 16:34:52 +01:00
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
rwTrqNs);
2023-01-23 16:34:52 +01:00
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
2023-03-10 17:31:12 +01:00
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
2023-01-23 16:34:52 +01:00
ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
2023-02-20 15:59:01 +01:00
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
2023-01-23 16:34:52 +01:00
break;
case acs::PTG_INERTIAL:
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
2023-06-02 17:41:25 +02:00
sizeof(targetQuat));
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
2023-11-23 16:56:36 +01:00
acsParameters.inertialModeControllerParameters.quatRef,
acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
2023-01-23 16:34:52 +01:00
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
rwTrqNs);
2023-01-23 16:34:52 +01:00
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
2023-03-10 17:31:12 +01:00
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
2023-01-23 16:34:52 +01:00
ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
2023-02-20 15:59:01 +01:00
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
2023-01-23 16:34:52 +01:00
break;
default:
2023-04-27 09:35:38 +02:00
sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;
break;
2023-01-23 16:34:52 +01:00
}
actuatorCmd.cmdSpeedToRws(
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
2023-04-27 10:19:07 +02:00
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
2023-03-10 17:31:12 +01:00
if (enableAntiStiction) {
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
}
2023-04-27 10:33:43 +02:00
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
2023-01-23 16:34:52 +01:00
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate, ptgCtrlStrat);
2023-04-27 10:33:43 +02:00
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
2023-04-14 16:50:02 +02:00
acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime);
2023-02-10 11:27:25 +01:00
}
2023-04-06 10:47:26 +02:00
void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) {
if (not safeCtrlFailureFlag) {
triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure);
safeCtrlFailureFlag = true;
}
safeCtrlFailureCounter++;
if (safeCtrlFailureCounter > 150) {
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
}
}
ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
uint16_t dipoleTorqueDuration) {
{
PoolReadGuard pg(&dipoleSet);
MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT,
torquer::LOCK_CTX);
torquer::NEW_ACTUATION_FLAG = true;
dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration);
}
return returnvalue::OK;
}
2023-02-10 11:27:25 +01:00
ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
uint16_t dipoleTorqueDuration, int32_t rw1Speed,
int32_t rw2Speed, int32_t rw3Speed, int32_t rw4Speed,
uint16_t rampTime) {
{
PoolReadGuard pg(&dipoleSet);
2023-03-02 15:32:12 +01:00
MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT,
torquer::LOCK_CTX);
2023-02-10 11:27:25 +01:00
torquer::NEW_ACTUATION_FLAG = true;
dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration);
}
{
PoolReadGuard pg(&rw1SpeedSet);
rw1SpeedSet.setRwSpeed(rw1Speed, rampTime);
}
{
PoolReadGuard pg(&rw2SpeedSet);
rw2SpeedSet.setRwSpeed(rw2Speed, rampTime);
}
{
PoolReadGuard pg(&rw3SpeedSet);
rw3SpeedSet.setRwSpeed(rw3Speed, rampTime);
}
{
PoolReadGuard pg(&rw4SpeedSet);
rw4SpeedSet.setRwSpeed(rw4Speed, rampTime);
}
return returnvalue::OK;
2023-01-23 16:34:52 +01:00
}
2023-02-23 18:34:28 +01:00
void AcsController::updateActuatorCmdData(const int16_t *mtqTargetDipole) {
2023-02-23 18:39:14 +01:00
updateActuatorCmdData(RW_OFF_TORQUE, RW_OFF_SPEED, mtqTargetDipole);
2023-02-17 11:11:48 +01:00
}
2023-02-23 18:34:28 +01:00
void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
const int32_t *rwTargetSpeed,
const int16_t *mtqTargetDipole) {
2023-02-23 18:39:14 +01:00
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t));
actuatorCmdData.setValidity(true, true);
2023-02-17 11:11:48 +01:00
}
}
void AcsController::updateCtrlValData(acs::ControlModeStrategy ctrlStrat) {
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double));
ctrlValData.errQuat.setValid(false);
ctrlValData.errAng.value = 0;
ctrlValData.errAng.setValid(false);
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
ctrlValData.tgtRotRate.setValid(false);
ctrlValData.safeStrat.value = ctrlStrat;
ctrlValData.safeStrat.setValid(true);
ctrlValData.setValidity(true, false);
}
}
void AcsController::updateCtrlValData(double errAng, acs::ControlModeStrategy ctrlStrat) {
2023-02-23 18:34:28 +01:00
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
2023-02-23 18:34:28 +01:00
ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double));
2023-02-23 18:34:28 +01:00
ctrlValData.errQuat.setValid(false);
ctrlValData.errAng.value = errAng;
ctrlValData.errAng.setValid(true);
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
ctrlValData.tgtRotRate.setValid(false);
ctrlValData.safeStrat.value = ctrlStrat;
ctrlValData.safeStrat.setValid(true);
2023-02-23 18:34:28 +01:00
ctrlValData.setValidity(true, false);
}
}
void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng,
const double *tgtRotRate,
acs::ControlModeStrategy ctrlStrat) {
2023-02-23 18:39:14 +01:00
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 = ctrlStrat;
2023-02-23 18:39:14 +01:00
ctrlValData.setValidity(true, true);
}
}
2022-08-15 09:58:18 +02:00
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
2023-01-23 16:34:52 +01:00
// MGM Raw
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmVecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus);
2023-04-04 19:18:53 +02:00
poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 10.0});
2023-01-23 16:34:52 +01:00
// MGM Processed
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_VEC, &mgm2VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_VEC, &mgm3VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_4_VEC, &mgm4VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer);
localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf);
2023-04-04 19:18:53 +02:00
poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), enableHkSets, 10.0});
2023-01-23 16:34:52 +01:00
// SUS Raw
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11ValRaw);
2023-04-04 19:18:53 +02:00
poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 10.0});
2023-01-23 16:34:52 +01:00
// SUS Processed
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_VEC, &sus2VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_VEC, &sus3VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_VEC, &sus4VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_VEC, &sus5VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_VEC, &sus6VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_VEC, &sus7VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_VEC, &sus8VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_VEC, &sus9VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_VEC, &sus10VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_VEC, &sus11VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT, &susVecTot);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT_DERIVATIVE, &susVecTotDer);
localDataPoolMap.emplace(acsctrl::PoolIds::SUN_IJK_MODEL, &sunIjk);
2023-04-04 19:18:53 +02:00
poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), enableHkSets, 10.0});
2023-01-23 16:34:52 +01:00
// GYR Raw
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_ADIS, &gyr0VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_L3, &gyr1VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_ADIS, &gyr2VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_L3, &gyr3VecRaw);
2023-04-04 19:18:53 +02:00
poolManager.subscribeForDiagPeriodicPacket({gyrDataRaw.getSid(), false, 10.0});
2023-01-23 16:34:52 +01:00
// GYR Processed
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_VEC, &gyr0VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_VEC, &gyr1VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_VEC, &gyr2VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_VEC, &gyr3VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_VEC_TOT, &gyrVecTot);
2023-04-04 19:18:53 +02:00
poolManager.subscribeForDiagPeriodicPacket({gyrDataProcessed.getSid(), enableHkSets, 10.0});
2023-01-23 16:34:52 +01:00
// GPS Processed
localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude);
2023-01-23 16:34:52 +01:00
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
2023-08-07 13:36:12 +02:00
localDataPoolMap.emplace(acsctrl::PoolIds::SOURCE, &gpsSource);
2023-04-04 19:18:53 +02:00
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0});
2023-11-23 16:56:36 +01:00
// Attitude Estimation
2023-01-23 16:34:52 +01:00
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
2023-02-21 16:02:26 +01:00
localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
2023-11-23 16:56:36 +01:00
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_QUEST, &quatQuest);
poolManager.subscribeForDiagPeriodicPacket({attitudeEstimationData.getSid(), enableHkSets, 10.0});
2023-01-23 16:34:52 +01:00
// Ctrl Values
localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat);
2023-01-23 16:34:52 +01:00
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_ROT_RATE, &tgtRotRate);
2023-04-04 19:18:53 +02:00
poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), enableHkSets, 10.0});
2023-01-23 16:34:52 +01:00
// Actuator CMD
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed);
localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole);
2023-04-04 19:18:53 +02:00
poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 10.0});
// Fused Rot Rate
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
2023-11-23 16:56:36 +01:00
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource);
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0});
2023-11-23 16:56:36 +01:00
// Fused Rot Rate Sources
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL_SUSMGM, &rotRateOrthogonalSusMgm);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL_SUSMGM, &rotRateParallelSusMgm);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_SUSMGM, &rotRateTotalSusMgm);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_QUEST, &rotRateTotalQuest);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_STR, &rotRateTotalStr);
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateSourcesData.getSid(), false, 10.0});
2023-01-23 16:34:52 +01:00
return returnvalue::OK;
}
2022-08-16 11:56:17 +02:00
LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
2023-01-23 16:34:52 +01:00
switch (sid.ownerSetId) {
case acsctrl::MGM_SENSOR_DATA:
return &mgmDataRaw;
case acsctrl::MGM_PROCESSED_DATA:
return &mgmDataProcessed;
case acsctrl::SUS_SENSOR_DATA:
return &susDataRaw;
case acsctrl::SUS_PROCESSED_DATA:
return &susDataProcessed;
case acsctrl::GYR_SENSOR_DATA:
return &gyrDataRaw;
case acsctrl::GYR_PROCESSED_DATA:
return &gyrDataProcessed;
case acsctrl::GPS_PROCESSED_DATA:
return &gpsDataProcessed;
case acsctrl::MEKF_DATA:
2023-11-23 16:56:36 +01:00
return &attitudeEstimationData;
2023-01-23 16:34:52 +01:00
case acsctrl::CTRL_VAL_DATA:
return &ctrlValData;
case acsctrl::ACTUATOR_CMD_DATA:
return &actuatorCmdData;
2023-07-21 09:51:14 +02:00
case acsctrl::FUSED_ROTATION_RATE_DATA:
return &fusedRotRateData;
2023-12-04 18:07:22 +01:00
case acsctrl::FUSED_ROTATION_RATE_SOURCES_DATA:
return &fusedRotRateSourcesData;
2023-01-23 16:34:52 +01:00
default:
return nullptr;
}
return nullptr;
}
ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
2022-08-15 09:58:18 +02:00
uint32_t *msToReachTheMode) {
2023-01-23 16:34:52 +01:00
if (mode == MODE_OFF) {
if (submode == SUBMODE_NONE) {
return returnvalue::OK;
} else {
return INVALID_SUBMODE;
}
} else if (not((mode < acs::AcsMode::SAFE) or (mode > acs::AcsMode::PTG_INERTIAL))) {
if (mode == acs::AcsMode::SAFE) {
2023-03-10 13:17:46 +01:00
if (not((submode == SUBMODE_NONE) or (submode == acs::SafeSubmode::DETUMBLE))) {
return INVALID_SUBMODE;
} else {
return returnvalue::OK;
}
} else if (not(submode == SUBMODE_NONE)) {
2023-01-23 16:34:52 +01:00
return INVALID_SUBMODE;
} else {
return returnvalue::OK;
}
}
2023-01-23 16:34:52 +01:00
return INVALID_MODE;
}
2022-08-15 10:50:19 +02:00
2023-02-13 01:26:30 +01:00
void AcsController::modeChanged(Mode_t mode, Submode_t submode) {
return ExtendedControllerBase::modeChanged(mode, submode);
}
2023-02-13 01:26:30 +01:00
void AcsController::announceMode(bool recursive) {
2023-02-13 10:03:16 +01:00
const char *modeStr = "UNKNOWN";
if (mode == HasModesIF::MODE_OFF) {
modeStr = "OFF";
2023-03-10 12:46:26 +01:00
} else {
2023-03-10 13:17:46 +01:00
modeStr = acs::getModeStr(static_cast<acs::AcsMode>(mode));
2023-03-10 12:46:26 +01:00
}
const char *submodeStr = "UNKNOWN";
if (submode == HasModesIF::SUBMODE_NONE) {
submodeStr = "NONE";
2023-03-10 13:17:46 +01:00
}
if (mode == acs::AcsMode::SAFE) {
acs::SafeSubmode safeSubmode = static_cast<acs::SafeSubmode>(this->submode);
if (safeSubmode == acs::SafeSubmode::DETUMBLE) {
submodeStr = "DETUMBLE";
}
}
2023-02-13 10:03:16 +01:00
sif::info << "ACS controller is now in " << modeStr << " mode with " << submodeStr << " submode"
<< std::endl;
2023-02-13 01:26:30 +01:00
return ExtendedControllerBase::announceMode(recursive);
}
2023-01-23 16:34:52 +01:00
void AcsController::copyMgmData() {
{
PoolReadGuard pg(&sensorValues.mgm0Lis3Set);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmDataRaw.mgm0Lis3.value, sensorValues.mgm0Lis3Set.fieldStrengths.value,
3 * sizeof(float));
mgmDataRaw.mgm0Lis3.setValid(sensorValues.mgm0Lis3Set.fieldStrengths.isValid());
2022-08-15 11:19:08 +02:00
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.mgm1Rm3100Set);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmDataRaw.mgm1Rm3100.value, sensorValues.mgm1Rm3100Set.fieldStrengths.value,
3 * sizeof(float));
mgmDataRaw.mgm1Rm3100.setValid(sensorValues.mgm1Rm3100Set.fieldStrengths.isValid());
2022-08-15 11:19:08 +02:00
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.mgm2Lis3Set);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmDataRaw.mgm2Lis3.value, sensorValues.mgm2Lis3Set.fieldStrengths.value,
3 * sizeof(float));
mgmDataRaw.mgm2Lis3.setValid(sensorValues.mgm2Lis3Set.fieldStrengths.isValid());
2022-08-15 11:19:08 +02:00
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.mgm3Rm3100Set);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmDataRaw.mgm3Rm3100.value, sensorValues.mgm3Rm3100Set.fieldStrengths.value,
3 * sizeof(float));
mgmDataRaw.mgm3Rm3100.setValid(sensorValues.mgm3Rm3100Set.fieldStrengths.isValid());
2022-08-15 11:19:08 +02:00
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.imtqMgmSet);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmDataRaw.imtqRaw.value, sensorValues.imtqMgmSet.mtmRawNt.value,
3 * sizeof(float));
mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid());
mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value;
mgmDataRaw.actuationCalStatus.setValid(sensorValues.imtqMgmSet.coilActuationStatus.isValid());
2022-08-15 11:19:08 +02:00
}
}
2023-01-23 16:34:52 +01:00
}
2022-09-28 15:27:51 +02:00
2023-01-23 16:34:52 +01:00
void AcsController::copySusData() {
{
PoolReadGuard pg(&sensorValues.susSets[0]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus0.setValid(sensorValues.susSets[0].channels.isValid());
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.susSets[1]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataRaw.sus1.value, sensorValues.susSets[1].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus1.setValid(sensorValues.susSets[1].channels.isValid());
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.susSets[2]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataRaw.sus2.value, sensorValues.susSets[2].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus2.setValid(sensorValues.susSets[2].channels.isValid());
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.susSets[3]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataRaw.sus3.value, sensorValues.susSets[3].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus3.setValid(sensorValues.susSets[3].channels.isValid());
2022-09-28 15:27:51 +02:00
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.susSets[4]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataRaw.sus4.value, sensorValues.susSets[4].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus4.setValid(sensorValues.susSets[4].channels.isValid());
2022-09-28 15:27:51 +02:00
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.susSets[5]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataRaw.sus5.value, sensorValues.susSets[5].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus5.setValid(sensorValues.susSets[5].channels.isValid());
2022-09-28 15:27:51 +02:00
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.susSets[6]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataRaw.sus6.value, sensorValues.susSets[6].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus6.setValid(sensorValues.susSets[6].channels.isValid());
2022-09-28 15:27:51 +02:00
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.susSets[7]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataRaw.sus7.value, sensorValues.susSets[7].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus7.setValid(sensorValues.susSets[7].channels.isValid());
2022-09-28 15:27:51 +02:00
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.susSets[8]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataRaw.sus8.value, sensorValues.susSets[8].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus8.setValid(sensorValues.susSets[8].channels.isValid());
2022-09-28 15:27:51 +02:00
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.susSets[9]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataRaw.sus9.value, sensorValues.susSets[9].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus9.setValid(sensorValues.susSets[9].channels.isValid());
2022-09-28 15:27:51 +02:00
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.susSets[10]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataRaw.sus10.value, sensorValues.susSets[10].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus10.setValid(sensorValues.susSets[10].channels.isValid());
2022-09-28 15:27:51 +02:00
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.susSets[11]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataRaw.sus11.value, sensorValues.susSets[11].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus11.setValid(sensorValues.susSets[11].channels.isValid());
2022-09-28 15:27:51 +02:00
}
}
2023-01-23 16:34:52 +01:00
}
2023-08-23 15:21:40 +02:00
ReturnValue_t AcsController::updateTle(const uint8_t *line1, const uint8_t *line2, bool fromFile) {
ReturnValue_t result = navigation.updateTle(line1, line2);
if (result != returnvalue::OK) {
if (not fromFile) {
uint8_t fileLine1[69] = {};
uint8_t fileLine2[69] = {};
readTleFromFs(fileLine1, fileLine2);
navigation.updateTle(fileLine1, fileLine2);
}
return result;
}
return returnvalue::OK;
}
ReturnValue_t AcsController::writeTleToFs(const uint8_t *tle) {
2023-11-29 16:59:03 +01:00
auto mntPrefix = sdcMan.getCurrentMountPrefix();
if (mntPrefix == nullptr or !sdcMan.isSdCardUsable(std::nullopt)) {
return returnvalue::FAILED;
}
std::string path = mntPrefix + TLE_FILE;
2023-08-23 15:21:40 +02:00
// Clear existing TLE from file
std::ofstream tleFile(path.c_str(), std::ofstream::out | std::ofstream::trunc);
if (tleFile.is_open()) {
2023-09-15 13:34:16 +02:00
tleFile.write(reinterpret_cast<const char *>(tle), 69);
tleFile << "\n";
2023-09-15 13:34:16 +02:00
tleFile.write(reinterpret_cast<const char *>(tle + 69), 69);
2023-08-23 15:21:40 +02:00
} else {
2023-11-29 16:59:03 +01:00
return WRITE_FILE_FAILED;
2023-08-23 15:21:40 +02:00
}
tleFile.close();
return returnvalue::OK;
}
ReturnValue_t AcsController::readTleFromFs(uint8_t *line1, uint8_t *line2) {
2023-11-29 16:59:03 +01:00
auto mntPrefix = sdcMan.getCurrentMountPrefix();
if (mntPrefix == nullptr or !sdcMan.isSdCardUsable(std::nullopt)) {
return returnvalue::FAILED;
}
std::string path = mntPrefix + TLE_FILE;
std::error_code e;
if (std::filesystem::exists(path, e)) {
// Read existing TLE from file
std::fstream tleFile = std::fstream(path.c_str(), std::fstream::in);
if (tleFile.is_open()) {
std::string tleLine1, tleLine2;
getline(tleFile, tleLine1);
std::memcpy(line1, tleLine1.c_str(), 69);
getline(tleFile, tleLine2);
std::memcpy(line2, tleLine2.c_str(), 69);
} else {
triggerEvent(acs::TLE_FILE_READ_FAILED);
2023-12-01 12:56:31 +01:00
return READ_FILE_FAILED;
2023-11-29 16:59:03 +01:00
}
tleFile.close();
2023-12-01 12:56:31 +01:00
} else {
triggerEvent(acs::TLE_FILE_READ_FAILED);
return READ_FILE_FAILED;
2023-08-23 15:21:40 +02:00
}
return returnvalue::OK;
}
2023-01-23 16:34:52 +01:00
void AcsController::copyGyrData() {
{
PoolReadGuard pg(&sensorValues.gyr0AdisSet);
if (pg.getReadResult() == returnvalue::OK) {
gyrDataRaw.gyr0Adis.value[0] = sensorValues.gyr0AdisSet.angVelocX.value;
gyrDataRaw.gyr0Adis.value[1] = sensorValues.gyr0AdisSet.angVelocY.value;
gyrDataRaw.gyr0Adis.value[2] = sensorValues.gyr0AdisSet.angVelocZ.value;
gyrDataRaw.gyr0Adis.setValid(sensorValues.gyr0AdisSet.angVelocX.isValid() &&
sensorValues.gyr0AdisSet.angVelocY.isValid() &&
sensorValues.gyr0AdisSet.angVelocZ.isValid());
2022-09-28 15:27:51 +02:00
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.gyr1L3gSet);
if (pg.getReadResult() == returnvalue::OK) {
gyrDataRaw.gyr1L3.value[0] = sensorValues.gyr1L3gSet.angVelocX.value;
gyrDataRaw.gyr1L3.value[1] = sensorValues.gyr1L3gSet.angVelocY.value;
gyrDataRaw.gyr1L3.value[2] = sensorValues.gyr1L3gSet.angVelocZ.value;
gyrDataRaw.gyr1L3.setValid(sensorValues.gyr1L3gSet.angVelocX.isValid() &&
sensorValues.gyr1L3gSet.angVelocY.isValid() &&
sensorValues.gyr1L3gSet.angVelocZ.isValid());
2022-09-28 15:27:51 +02:00
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.gyr2AdisSet);
if (pg.getReadResult() == returnvalue::OK) {
gyrDataRaw.gyr2Adis.value[0] = sensorValues.gyr2AdisSet.angVelocX.value;
gyrDataRaw.gyr2Adis.value[1] = sensorValues.gyr2AdisSet.angVelocY.value;
gyrDataRaw.gyr2Adis.value[2] = sensorValues.gyr2AdisSet.angVelocZ.value;
gyrDataRaw.gyr2Adis.setValid(sensorValues.gyr2AdisSet.angVelocX.isValid() &&
sensorValues.gyr2AdisSet.angVelocY.isValid() &&
sensorValues.gyr2AdisSet.angVelocZ.isValid());
2022-09-28 15:27:51 +02:00
}
2023-01-23 16:34:52 +01:00
}
{
PoolReadGuard pg(&sensorValues.gyr3L3gSet);
if (pg.getReadResult() == returnvalue::OK) {
gyrDataRaw.gyr3L3.value[0] = sensorValues.gyr3L3gSet.angVelocX.value;
gyrDataRaw.gyr3L3.value[1] = sensorValues.gyr3L3gSet.angVelocY.value;
gyrDataRaw.gyr3L3.value[2] = sensorValues.gyr3L3gSet.angVelocZ.value;
gyrDataRaw.gyr3L3.setValid(sensorValues.gyr3L3gSet.angVelocX.isValid() &&
sensorValues.gyr3L3gSet.angVelocY.isValid() &&
sensorValues.gyr3L3gSet.angVelocZ.isValid());
2022-09-28 15:27:51 +02:00
}
}
2023-01-23 16:34:52 +01:00
}