diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 9b210369..dee2fb73 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -275,44 +275,32 @@ void AcsController::performControlOperation() { case SUBMODE_IDLE: guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, targetQuat, refSatRate); - for ( uint8_t i = 0; i < 4; i++ ) { - quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i]; - } + std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; case SUBMODE_PTG_TARGET: guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, refSatRate); - for ( uint8_t i = 0; i < 4; i++ ) { - quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i]; - } + std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; case SUBMODE_PTG_TARGET_GS: guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, targetQuat, refSatRate); - - for ( uint8_t i = 0; i < 4; i++ ) { - quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i]; - } + std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; case SUBMODE_PTG_NADIR: guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, refSatRate); - for ( uint8_t i = 0; i < 4; i++ ) { - quatRef[i] = acsParameters.nadirModeControllerParameters.quatRef[i]; - } + std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; break; case SUBMODE_PTG_INERTIAL: guidance.inertialQuatPtg(targetQuat, refSatRate); - - for ( uint8_t i = 0; i < 4; i++ ) { - quatRef[i] = acsParameters.inertialModeControllerParameters.quatRef[i]; - } + std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; }