added returnvalue handling
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
This commit is contained in:
parent
5205c5b78a
commit
5d9ed222f5
@ -35,14 +35,13 @@ enum commonClassIds : uint8_t {
|
||||
SA_DEPL_HANDLER, // SADPL
|
||||
MPSOC_RETURN_VALUES_IF, // MPSOCRTVIF
|
||||
SUPV_RETURN_VALUES_IF, // SPVRTVIF
|
||||
ACS_KALMAN, // ACSKAL
|
||||
ACS_CTRL, // ACSCTRL
|
||||
ACS_MEKF, // ACSMEKF
|
||||
ACS_SAFE, // ACSSAF
|
||||
ACS_PTG, // ACSPTG
|
||||
ACS_DETUMBLE, // ACSDTB
|
||||
ACS_MEKF, // ACSMEK
|
||||
COMMON_CLASS_ID_END // [EXPORT] : [END]
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* COMMON_CONFIG_COMMONCLASSIDS_H_ */
|
||||
|
@ -49,7 +49,10 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
|
||||
const uint8_t *data, size_t size) {
|
||||
switch (actionId) {
|
||||
case SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL: {
|
||||
guidance.solarArrayDeploymentComplete();
|
||||
ReturnValue_t result = guidance.solarArrayDeploymentComplete();
|
||||
if (result == returnvalue::FAILED) {
|
||||
return FILE_DELETION_FAILED;
|
||||
}
|
||||
return HasActionsIF::EXECUTION_FINISHED;
|
||||
}
|
||||
case RESET_MEKF: {
|
||||
@ -140,8 +143,8 @@ void AcsController::performSafe() {
|
||||
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
||||
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
||||
&susDataProcessed, &mekfData);
|
||||
if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
|
||||
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
|
||||
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
|
||||
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
|
||||
if (not mekfInvalidFlag) {
|
||||
triggerEvent(acs::MEKF_INVALID_INFO);
|
||||
mekfInvalidFlag = true;
|
||||
@ -155,7 +158,7 @@ void AcsController::performSafe() {
|
||||
// if MEKF is working
|
||||
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
|
||||
bool magMomMtqValid = false;
|
||||
if (result == MultiplicativeKalmanFilter::KALMAN_RUNNING) {
|
||||
if (result == MultiplicativeKalmanFilter::MEKF_RUNNING) {
|
||||
safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
|
||||
mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
|
||||
susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
|
||||
@ -205,8 +208,8 @@ void AcsController::performDetumble() {
|
||||
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
||||
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
||||
&susDataProcessed, &mekfData);
|
||||
if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
|
||||
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
|
||||
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
|
||||
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
|
||||
if (not mekfInvalidFlag) {
|
||||
triggerEvent(acs::MEKF_INVALID_INFO);
|
||||
mekfInvalidFlag = true;
|
||||
@ -252,8 +255,8 @@ void AcsController::performPointingCtrl() {
|
||||
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
||||
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
||||
&susDataProcessed, &mekfData);
|
||||
if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
|
||||
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
|
||||
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
|
||||
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
|
||||
if (not mekfInvalidFlag) {
|
||||
triggerEvent(acs::MEKF_INVALID_INFO);
|
||||
mekfInvalidFlag = true;
|
||||
|
@ -70,6 +70,9 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||
static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0;
|
||||
static const DeviceCommandId_t RESET_MEKF = 0x1;
|
||||
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL;
|
||||
static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0);
|
||||
|
||||
ReturnValue_t initialize() override;
|
||||
ReturnValue_t handleCommandMessage(CommandMessage* message) override;
|
||||
void performControlOperation() override;
|
||||
|
@ -74,15 +74,15 @@ class MultiplicativeKalmanFilter {
|
||||
};
|
||||
|
||||
// resetting Mekf
|
||||
static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN;
|
||||
static constexpr ReturnValue_t KALMAN_UNINITIALIZED = returnvalue::makeCode(IF_KAL_ID, 2);
|
||||
static constexpr ReturnValue_t KALMAN_NO_GYR_DATA = returnvalue::makeCode(IF_KAL_ID, 3);
|
||||
static constexpr ReturnValue_t KALMAN_NO_MODEL_VECTORS = returnvalue::makeCode(IF_KAL_ID, 4);
|
||||
static constexpr ReturnValue_t KALMAN_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_KAL_ID, 5);
|
||||
static constexpr ReturnValue_t KALMAN_COVARIANCE_INVERSION_FAILED =
|
||||
returnvalue::makeCode(IF_KAL_ID, 6);
|
||||
static constexpr ReturnValue_t KALMAN_INITIALIZED = returnvalue::makeCode(IF_KAL_ID, 7);
|
||||
static constexpr ReturnValue_t KALMAN_RUNNING = returnvalue::makeCode(IF_KAL_ID, 8);
|
||||
static constexpr uint8_t IF_MEKF_ID = CLASS_ID::ACS_MEKF;
|
||||
static constexpr ReturnValue_t MEKF_UNINITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 2);
|
||||
static constexpr ReturnValue_t MEKF_NO_GYR_DATA = returnvalue::makeCode(IF_MEKF_ID, 3);
|
||||
static constexpr ReturnValue_t MEKF_NO_MODEL_VECTORS = returnvalue::makeCode(IF_MEKF_ID, 4);
|
||||
static constexpr ReturnValue_t MEKF_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_MEKF_ID, 5);
|
||||
static constexpr ReturnValue_t MEKF_COVARIANCE_INVERSION_FAILED =
|
||||
returnvalue::makeCode(IF_MEKF_ID, 6);
|
||||
static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7);
|
||||
static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8);
|
||||
|
||||
private:
|
||||
/*Parameters*/
|
||||
|
@ -25,7 +25,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
|
||||
sensorValues->strSet.caliQy.isValid() &&
|
||||
sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid();
|
||||
|
||||
if (mekfStatus == MultiplicativeKalmanFilter::KALMAN_UNINITIALIZED) {
|
||||
if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
|
||||
mekfStatus = multiplicativeKalmanFilter.init(
|
||||
mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
|
||||
susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
|
||||
|
Loading…
Reference in New Issue
Block a user