diff --git a/mission/system/acs/AcsBoardAssembly.cpp b/mission/system/acs/AcsBoardAssembly.cpp index 2441beda..08711f76 100644 --- a/mission/system/acs/AcsBoardAssembly.cpp +++ b/mission/system/acs/AcsBoardAssembly.cpp @@ -289,25 +289,29 @@ ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode) { using namespace returnvalue; ReturnValue_t status = returnvalue::OK; - auto overwriteHealthForOneDev = [&](object_id_t dev) { - HealthState health = healthHelper.healthTable->getHealth(dev); - if (health == FAULTY or health == PERMANENT_FAULTY) { - overwriteDeviceHealth(dev, health); - status = NEED_TO_CHANGE_HEALTH; - } else if (health == EXTERNAL_CONTROL) { + auto checkAcsBoardSensorGroup = [&](object_id_t o0, object_id_t o1, object_id_t o2, + object_id_t o3) { + HealthState h0 = healthHelper.healthTable->getHealth(o0); + HealthState h1 = healthHelper.healthTable->getHealth(o1); + HealthState h2 = healthHelper.healthTable->getHealth(o2); + HealthState h3 = healthHelper.healthTable->getHealth(o3); + if ((h0 == FAULTY or h0 == PERMANENT_FAULTY) and (h1 == FAULTY or h1 == PERMANENT_FAULTY) and + (h2 == FAULTY or h2 == PERMANENT_FAULTY) and (h3 == FAULTY or h3 == PERMANENT_FAULTY)) { + overwriteDeviceHealth(o0, h0); + overwriteDeviceHealth(o1, h1); + overwriteDeviceHealth(o2, h2); + overwriteDeviceHealth(o3, h3); + } + if (h0 == EXTERNAL_CONTROL or h1 == EXTERNAL_CONTROL or h2 == EXTERNAL_CONTROL or + h3 == EXTERNAL_CONTROL) { modeHelper.setForced(true); } }; if (deviceSubmode == duallane::DUAL_MODE) { - overwriteHealthForOneDev(helper.mgm0Lis3IdSideA); - overwriteHealthForOneDev(helper.mgm1Rm3100IdSideA); - overwriteHealthForOneDev(helper.mgm2Lis3IdSideB); - overwriteHealthForOneDev(helper.mgm3Rm3100IdSideB); - overwriteHealthForOneDev(helper.gyro0AdisIdSideA); - overwriteHealthForOneDev(helper.gyro1L3gIdSideA); - overwriteHealthForOneDev(helper.gyro2AdisIdSideB); - overwriteHealthForOneDev(helper.gyro3L3gIdSideB); - overwriteHealthForOneDev(helper.gpsId); + checkAcsBoardSensorGroup(helper.mgm0Lis3IdSideA, helper.mgm1Rm3100IdSideA, + helper.mgm2Lis3IdSideB, helper.mgm3Rm3100IdSideB); + checkAcsBoardSensorGroup(helper.gyro0AdisIdSideA, helper.gyro1L3gIdSideA, + helper.gyro2AdisIdSideB, helper.gyro3L3gIdSideB); } return status; } diff --git a/mission/system/acs/SusAssembly.cpp b/mission/system/acs/SusAssembly.cpp index 9008e7e0..45123ce4 100644 --- a/mission/system/acs/SusAssembly.cpp +++ b/mission/system/acs/SusAssembly.cpp @@ -158,18 +158,29 @@ void SusAssembly::refreshHelperModes() { ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode) { using namespace returnvalue; ReturnValue_t status = returnvalue::OK; - auto overwriteHealthForOneDev = [&](object_id_t dev) { + auto checkSusGroup = [&](object_id_t devNom, object_id_t devRed) { + HealthState healthNom = healthHelper.healthTable->getHealth(devNom); + HealthState healthRed = healthHelper.healthTable->getHealth(devRed); + if ((healthNom == FAULTY or healthNom == PERMANENT_FAULTY) and + (healthRed == FAULTY or healthRed == PERMANENT_FAULTY)) { + overwriteDeviceHealth(devNom, healthNom); + overwriteDeviceHealth(devRed, healthRed); + } + }; + auto checkHealthForOneDev = [&](object_id_t dev) { HealthState health = healthHelper.healthTable->getHealth(dev); - if (health == FAULTY or health == PERMANENT_FAULTY) { - overwriteDeviceHealth(dev, health); - status = NEED_TO_CHANGE_HEALTH; - } else if (health == EXTERNAL_CONTROL) { + if (health == EXTERNAL_CONTROL) { modeHelper.setForced(true); } }; if (deviceSubmode == duallane::DUAL_MODE) { - for (uint8_t idx = 0; idx < 12; idx++) { - overwriteHealthForOneDev(helper.susIds[idx]); + uint8_t idx = 0; + for (idx = 0; idx < 6; idx++) { + checkSusGroup(helper.susIds[idx], helper.susIds[idx + 6]); + checkHealthForOneDev(helper.susIds[idx]); + } + for (idx = 6; idx < 12; idx++) { + checkHealthForOneDev(helper.susIds[idx]); } } return status;