i hope i get a medal from Robin for this
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
This commit is contained in:
parent
f61da1002f
commit
25354ee7b4
@ -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) {
|
switch (internalState) {
|
||||||
case InternalState::STARTUP: {
|
case InternalState::STARTUP: {
|
||||||
initialCountdown.resetTimer();
|
initialCountdown.resetTimer();
|
||||||
@ -156,25 +136,7 @@ void AcsController::performControlOperation() {
|
|||||||
}
|
}
|
||||||
case InternalState::READY: {
|
case InternalState::READY: {
|
||||||
if (mode != MODE_OFF) {
|
if (mode != MODE_OFF) {
|
||||||
switch (mode) {
|
performAttitudeControl();
|
||||||
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;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@ -183,39 +145,93 @@ void AcsController::performControlOperation() {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void AcsController::performSafe() {
|
void AcsController::performAttitudeControl() {
|
||||||
timeval now;
|
timeval timeAbsolute;
|
||||||
Clock::getClock_timeval(&now);
|
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) {
|
if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
|
||||||
triggerEvent(acs::TLE_TOO_OLD);
|
triggerEvent(acs::TLE_TOO_OLD);
|
||||||
tleTooOldFlag = true;
|
tleTooOldFlag = true;
|
||||||
} else if (result != Sgp4Propagator::TLE_TOO_OLD) {
|
} else if (result != Sgp4Propagator::TLE_TOO_OLD) {
|
||||||
tleTooOldFlag = false;
|
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,
|
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
|
||||||
&gyrDataProcessed, &fusedRotRateData);
|
&gyrDataProcessed, &fusedRotRateData);
|
||||||
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
||||||
&susDataProcessed, &mekfData, &acsParameters);
|
&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
|
// get desired satellite rate, sun direction to align to and inertia
|
||||||
double sunTargetDir[3] = {0, 0, 0};
|
double sunTargetDir[3] = {0, 0, 0};
|
||||||
guidance.getTargetParamsSafe(sunTargetDir);
|
guidance.getTargetParamsSafe(sunTargetDir);
|
||||||
@ -316,37 +332,6 @@ void AcsController::performSafe() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void AcsController::performDetumble() {
|
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(
|
acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
|
||||||
mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
|
mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
|
||||||
mgmDataProcessed.mgmVecTotDerivative.isValid(),
|
mgmDataProcessed.mgmVecTotDerivative.isValid(),
|
||||||
@ -409,51 +394,9 @@ void AcsController::performDetumble() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void AcsController::performPointingCtrl() {
|
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;
|
uint8_t enableAntiStiction = true;
|
||||||
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
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 (result == returnvalue::FAILED) {
|
||||||
if (multipleRwUnavailableCounter >=
|
if (multipleRwUnavailableCounter >=
|
||||||
acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
|
acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
|
||||||
|
|||||||
@ -37,6 +37,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
|||||||
uint16_t startAtIndex) override;
|
uint16_t startAtIndex) override;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
void performAttitudeControl();
|
||||||
void performSafe();
|
void performSafe();
|
||||||
void performDetumble();
|
void performDetumble();
|
||||||
void performPointingCtrl();
|
void performPointingCtrl();
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user