diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1aac02c2..14a2d83c 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -122,26 +122,6 @@ void AcsController::performControlOperation() { } } - timeval timeAbsolute; - Clock::getClock_timeval(&timeAbsolute); - timeval timeRelative; - Clock::getClockMonotonic(&timeRelative); - - ReturnValue_t result = navigation.useSpg4(timeAbsolute, &gpsDataProcessed); - 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; - } - - sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, - &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, - &gyrDataProcessed, &fusedRotRateData); - result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData, &acsParameters); - switch (internalState) { case InternalState::STARTUP: { initialCountdown.resetTimer(); @@ -156,25 +136,7 @@ void AcsController::performControlOperation() { } case InternalState::READY: { if (mode != MODE_OFF) { - 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; - } + performAttitudeControl(); } break; } @@ -183,39 +145,93 @@ void AcsController::performControlOperation() { } } -void AcsController::performSafe() { - timeval now; - Clock::getClock_timeval(&now); +void AcsController::performAttitudeControl() { + timeval timeAbsolute; + Clock::getClock_timeval(&timeAbsolute); + timeval timeRelative; + Clock::getClockMonotonic(&timeRelative); - ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed); + ReturnValue_t result = navigation.useSpg4(timeAbsolute, &gpsDataProcessed); 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; } - sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, - &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + + sensorProcessing.process(timeAbsolute, timeRelative, &sensorValues, &mgmDataProcessed, + &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &fusedRotRateData); result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData, &acsParameters); - if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && - result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { - if (not mekfInvalidFlag) { - triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value); - mekfInvalidFlag = true; - } - if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { - triggerEvent(acs::MEKF_AUTOMATIC_RESET); - navigation.resetMekf(&mekfData); - mekfLost = true; - } - } else if (mekfInvalidFlag) { - triggerEvent(acs::MEKF_RECOVERY); - mekfInvalidFlag = false; - } + switch (mode) { + case acs::SAFE: + if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && + result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { + if (not mekfInvalidFlag) { + triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value); + mekfInvalidFlag = true; + } + if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { + triggerEvent(acs::MEKF_AUTOMATIC_RESET); + navigation.resetMekf(&mekfData); + mekfLost = true; + } + } else if (mekfInvalidFlag) { + triggerEvent(acs::MEKF_RECOVERY); + mekfInvalidFlag = false; + } + 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: + if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && + result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { + mekfInvalidCounter++; + if (not mekfInvalidFlag) { + triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value); + mekfInvalidFlag = true; + } + if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { + triggerEvent(acs::MEKF_AUTOMATIC_RESET); + navigation.resetMekf(&mekfData); + mekfLost = true; + } + if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) { + // Trigger this so STR FDIR can set the device faulty. + EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, + 0); + mekfInvalidCounter = 0; + } + commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, + cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], + acsParameters.rwHandlingParameters.rampTime); + return; + } else { + if (mekfInvalidFlag) { + triggerEvent(acs::MEKF_RECOVERY); + mekfInvalidFlag = false; + } + mekfInvalidCounter = 0; + } + performPointingCtrl(); + break; + } +} + +void AcsController::performSafe() { // get desired satellite rate, sun direction to align to and inertia double sunTargetDir[3] = {0, 0, 0}; guidance.getTargetParamsSafe(sunTargetDir); @@ -316,37 +332,6 @@ void AcsController::performSafe() { } void AcsController::performDetumble() { - timeval now; - Clock::getClock_timeval(&now); - - ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed); - if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) { - triggerEvent(acs::TLE_TOO_OLD); - tleTooOldFlag = true; - } else { - tleTooOldFlag = false; - } - sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, - &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, - &gyrDataProcessed, &fusedRotRateData); - result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData, &acsParameters); - if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && - result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { - if (not mekfInvalidFlag) { - triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value); - mekfInvalidFlag = true; - } - if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { - triggerEvent(acs::MEKF_AUTOMATIC_RESET); - navigation.resetMekf(&mekfData); - mekfLost = true; - } - } else if (mekfInvalidFlag) { - triggerEvent(acs::MEKF_RECOVERY); - mekfInvalidFlag = false; - } acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy( mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.isValid(), @@ -409,51 +394,9 @@ void AcsController::performDetumble() { } void AcsController::performPointingCtrl() { - timeval now; - Clock::getClock_timeval(&now); - - ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed); - if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) { - triggerEvent(acs::TLE_TOO_OLD); - tleTooOldFlag = true; - } else { - tleTooOldFlag = false; - } - sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, - &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData, &acsParameters); - if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && - result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { - mekfInvalidCounter++; - if (not mekfInvalidFlag) { - triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value); - mekfInvalidFlag = true; - } - if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { - triggerEvent(acs::MEKF_AUTOMATIC_RESET); - navigation.resetMekf(&mekfData); - mekfLost = true; - } - if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) { - // Trigger this so STR FDIR can set the device faulty. - EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, 0); - mekfInvalidCounter = 0; - } - commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], - cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], - acsParameters.rwHandlingParameters.rampTime); - return; - } else { - if (mekfInvalidFlag) { - triggerEvent(acs::MEKF_RECOVERY); - mekfInvalidFlag = false; - } - mekfInvalidCounter = 0; - } uint8_t enableAntiStiction = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); + ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); if (result == returnvalue::FAILED) { if (multipleRwUnavailableCounter >= acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) { diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index e03f2a55..89919ccc 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -37,6 +37,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes uint16_t startAtIndex) override; protected: + void performAttitudeControl(); void performSafe(); void performDetumble(); void performPointingCtrl();