/* CalculateTrainPosition_Pkg::BG_utilities_Pkg::indexOfPassedBG_by_id */ void indexOfPassedBG_by_id_CalculateTrainPosition_Pkg_BG_utilities_Pkg( /* CalculateTrainPosition_Pkg::BG_utilities_Pkg::indexOfPassedBG_by_id::BG */passedBG_T_BG_Types_Pkg *BG, /* CalculateTrainPosition_Pkg::BG_utilities_Pkg::indexOfPassedBG_by_id::BGs */positionedBGs_T_TrainPosition_Types_Pck *BGs, /* CalculateTrainPosition_Pkg::BG_utilities_Pkg::indexOfPassedBG_by_id::enable */kcg_bool enable, /* CalculateTrainPosition_Pkg::BG_utilities_Pkg::indexOfPassedBG_by_id::indexOfBG */kcg_int *indexOfBG, /* CalculateTrainPosition_Pkg::BG_utilities_Pkg::indexOfPassedBG_by_id::BG_found */kcg_bool *BG_found, /* CalculateTrainPosition_Pkg::BG_utilities_Pkg::indexOfPassedBG_by_id::indexValid */kcg_bool *indexValid) { static struct__129113 tmp; tmp.valid = (*BG).valid & (*BG).BG_Header.valid; tmp.nid_c = (*BG).BG_Header.nid_c; tmp.nid_bg = (*BG).BG_Header.nid_bg; tmp.q_link = (*BG).BG_Header.q_link; kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg( &tmp.location, (LocWithInAcc_T_Obu_BasicTypes_Pkg *) &cLocWithInAcc_0_Obu_BasicTypes_Pkg); tmp.seqNoOnTrack = 0; kcg_copy_infoFromLinking_T_TrainPosition_Types_Pck( &tmp.infoFromLinking, (infoFromLinking_T_TrainPosition_Types_Pck *) &cNoInfoFromLinking_CalculateTrainPosition_Pkg); kcg_copy_passedBG_T_BG_Types_Pkg(&tmp.infoFromPassing, BG); tmp.missed = kcg_false; /* 1 */ indexOfBG_by_id_CalculateTrainPosition_Pkg_BG_utilities_Pkg( &tmp, BGs, enable, indexOfBG, BG_found, indexValid); }
/* TA_Lib_internal::Get_Position_of_LRBG */ L_internal_Type_Obu_BasicTypes_Pkg Get_Position_of_LRBG_TA_Lib_internal( /* TA_Lib_internal::Get_Position_of_LRBG::MessageIn */ ReceivedMessage_T_Common_Types_Pkg *MessageIn, /* TA_Lib_internal::Get_Position_of_LRBG::balisegroups */ positionedBGs_T_TrainPosition_Types_Pck *balisegroups) { /* TA_Lib_internal::Get_Position_of_LRBG */ positionedBG_T_TrainPosition_Types_Pck mk_struct; /* TA_Lib_internal::Get_Position_of_LRBG */ positionedBG_T_TrainPosition_Types_Pck tmp; /* TA_Lib_internal::Get_Position_of_LRBG::_L3 */ kcg_bool _L3; /* TA_Lib_internal::Get_Position_of_LRBG::_L2 */ kcg_bool _L2; /* TA_Lib_internal::Get_Position_of_LRBG::_L1 */ kcg_int _L1; /* TA_Lib_internal::Get_Position_of_LRBG::_L5 */ kcg_int _L5; /* TA_Lib_internal::Get_Position_of_LRBG::_L6 */ kcg_int _L6; /* TA_Lib_internal::Get_Position_of_LRBG::_L9 */ kcg_bool _L9; /* TA_Lib_internal::Get_Position_of_LRBG::Position */ L_internal_Type_Obu_BasicTypes_Pkg Position; /* 1 */ Eval_LRBG_TA_Lib_internal(MessageIn, &_L9, &_L6, &_L5); mk_struct.valid = _L9; mk_struct.nid_c = _L6; mk_struct.nid_bg = _L5; mk_struct.q_link = DEFAULT_q_link_TA_Lib_internal; kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg( &mk_struct.location, (LocWithInAcc_T_Obu_BasicTypes_Pkg *) &DEFAULT_location_TA_Lib_internal); mk_struct.seqNoOnTrack = 0; kcg_copy_infoFromLinking_T_TrainPosition_Types_Pck( &mk_struct.infoFromLinking, (infoFromLinking_T_TrainPosition_Types_Pck *) &DEFAULT_InfoFromLinking_TA_Lib_internal); kcg_copy_passedBG_T_BG_Types_Pkg( &mk_struct.infoFromPassing, (passedBG_T_BG_Types_Pkg *) &DEFAULT_InfoFromPassing_TA_Lib_internal); mk_struct.missed = kcg_false; /* 1 */ indexOfBG_by_id_CalculateTrainPosition_Pkg_BG_utilities_Pkg( &mk_struct, balisegroups, kcg_true, &_L1, &_L2, &_L3); if ((0 <= _L1) & (_L1 < 41)) { kcg_copy_positionedBG_T_TrainPosition_Types_Pck( &tmp, &(*balisegroups)[_L1]); } else { kcg_copy_positionedBG_T_TrainPosition_Types_Pck( &tmp, (positionedBG_T_TrainPosition_Types_Pck *) &DEFAULT_positioned_BG_TA_Lib_internal); } Position = tmp.location.nominal; return Position; }
/* CalculateTrainPosition_Pkg::calculateTrainPosition */ void calculateTrainPosition_CalculateTrainPosition_Pkg( /* CalculateTrainPosition_Pkg::calculateTrainPosition::currentOdometry */ odometry_T_Obu_BasicTypes_Pkg *currentOdometry, /* CalculateTrainPosition_Pkg::calculateTrainPosition::passedBG */ passedBG_T_BG_Types_Pkg *passedBG, /* CalculateTrainPosition_Pkg::calculateTrainPosition::msgFromTrack */ ReceivedMessage_T_Common_Types_Pkg *msgFromTrack, /* CalculateTrainPosition_Pkg::calculateTrainPosition::trainProperties */ trainProperties_T_TrainPosition_Types_Pck *trainProperties, /* CalculateTrainPosition_Pkg::calculateTrainPosition::reset */ kcg_bool reset, outC_calculateTrainPosition_CalculateTrainPosition_Pkg *outC) { positionedBGs_T_TrainPosition_Types_Pck tmp; /* CalculateTrainPosition_Pkg::calculateTrainPosition::if_BGpassed_or_onlyAnnounced::then::_L10 */ positionedBGs_T_TrainPosition_Types_Pck _L10_if_BGpassed_or_onlyAnnounced; /* CalculateTrainPosition_Pkg::calculateTrainPosition::if_BGpassed_or_onlyAnnounced::else::then::_L13 */ kcg_bool _L13_if_BGpassed_or_onlyAnnounced; /* CalculateTrainPosition_Pkg::calculateTrainPosition::BGs_loc */ positionedBGs_T_TrainPosition_Types_Pck last_BGs_loc; /* CalculateTrainPosition_Pkg::calculateTrainPosition::if_BGpassed_or_onlyAnnounced */ kcg_bool if_BGpassed_or_onlyAnnounced_clock; /* CalculateTrainPosition_Pkg::calculateTrainPosition::BG_passed */ kcg_bool BG_passed; /* CalculateTrainPosition_Pkg::calculateTrainPosition::onlyBGsAnnouncedViaRadio */ kcg_bool onlyBGsAnnouncedViaRadio; /* CalculateTrainPosition_Pkg::calculateTrainPosition::errors_loc */ positionErrors_T_TrainPosition_Types_Pck errors_loc; /* CalculateTrainPosition_Pkg::calculateTrainPosition::passedBG_asPositionedBG */ positionedBG_T_TrainPosition_Types_Pck passedBG_asPositionedBG; /* CalculateTrainPosition_Pkg::calculateTrainPosition::trainPositionInfo */ trainPositionInfo_T_TrainPosition_Types_Pck trainPositionInfo; /* CalculateTrainPosition_Pkg::calculateTrainPosition::passedBG_loc */ passedBG_T_BG_Types_Pkg passedBG_loc; /* CalculateTrainPosition_Pkg::calculateTrainPosition::_L306 */ passedBG_T_BG_Types_Pkg _L306; outC->errors.doubleRepositioningError = kcg_false; if (outC->init) { outC->init = kcg_false; kcg_copy_positionedBGs_T_TrainPosition_Types_Pck( &last_BGs_loc, (positionedBGs_T_TrainPosition_Types_Pck *) &cNoPositionedBGs_CalculateTrainPosition_Pkg); } else { kcg_copy_positionedBGs_T_TrainPosition_Types_Pck(&last_BGs_loc, &outC->BGs); } /* 1 */ msg_2_passedBG_CalculateTrainPosition_Pkg_msgAdapter_Pkg( msgFromTrack, &_L306, &if_BGpassed_or_onlyAnnounced_clock, &_L13_if_BGpassed_or_onlyAnnounced); if ((*passedBG).valid) { BG_passed = (*passedBG).BG_Header.valid & (*passedBG).valid; onlyBGsAnnouncedViaRadio = !(*passedBG).BG_Header.valid & (*passedBG).valid; kcg_copy_passedBG_T_BG_Types_Pkg(&passedBG_loc, passedBG); } else { BG_passed = if_BGpassed_or_onlyAnnounced_clock; onlyBGsAnnouncedViaRadio = _L13_if_BGpassed_or_onlyAnnounced; kcg_copy_passedBG_T_BG_Types_Pkg(&passedBG_loc, &_L306); } if_BGpassed_or_onlyAnnounced_clock = BG_passed | reset; if (if_BGpassed_or_onlyAnnounced_clock) { if (reset) { kcg_copy_positionedBGs_T_TrainPosition_Types_Pck( &tmp, (positionedBGs_T_TrainPosition_Types_Pck *) &cNoPositionedBGs_CalculateTrainPosition_Pkg); } else { kcg_copy_positionedBGs_T_TrainPosition_Types_Pck(&tmp, &last_BGs_loc); } /* 6 */ calculateBGLocations_CalculateTrainPosition_Pkg( &passedBG_loc, &tmp, reset, trainProperties, &outC->Context_6); kcg_copy_positionedBGs_T_TrainPosition_Types_Pck( &_L10_if_BGpassed_or_onlyAnnounced, &outC->Context_6.BGs); kcg_copy_positionErrors_T_TrainPosition_Types_Pck( &errors_loc, &outC->Context_6.errors); /* 2 */ delDispensableBGs_CalculateTrainPosition_Pkg( &_L10_if_BGpassed_or_onlyAnnounced, kcg_true, &outC->BGs); kcg_copy_positionedBG_T_TrainPosition_Types_Pck( &passedBG_asPositionedBG, &outC->Context_6.passedPositionedBG); } else { if (onlyBGsAnnouncedViaRadio) { /* 1 */ addAnnouncedBGs_CalculateTrainPosition_Pkg( &passedBG_loc, &last_BGs_loc, trainProperties, &outC->BGs, &_L13_if_BGpassed_or_onlyAnnounced); kcg_copy_positionErrors_T_TrainPosition_Types_Pck( &errors_loc, (positionErrors_T_TrainPosition_Types_Pck *) &cNoPositionErrors_CalculateTrainPosition_Pkg); errors_loc.outOfMemSpace = _L13_if_BGpassed_or_onlyAnnounced; } else { kcg_copy_positionErrors_T_TrainPosition_Types_Pck( &errors_loc, (positionErrors_T_TrainPosition_Types_Pck *) &cNoPositionErrors_CalculateTrainPosition_Pkg); kcg_copy_positionedBGs_T_TrainPosition_Types_Pck( &outC->BGs, &last_BGs_loc); } kcg_copy_positionedBG_T_TrainPosition_Types_Pck( &passedBG_asPositionedBG, (positionedBG_T_TrainPosition_Types_Pck *) &cNoPositionedBG_CalculateTrainPosition_Pkg); } outC->errors.outOfMemSpace = errors_loc.outOfMemSpace; outC->errors.passedBG_foundNotWhereExpected = errors_loc.passedBG_foundNotWhereExpected; outC->errors.BGpassedInUnexpectedDirection = errors_loc.BGpassedInUnexpectedDirection; /* 1 */ calculateTrainPositionInfo_CalculateTrainPosition_Pkg( currentOdometry, &outC->BGs, (kcg_bool) (onlyBGsAnnouncedViaRadio | BG_passed | reset), &passedBG_asPositionedBG, reset, &outC->Context_1); outC->errors.positionCalculation_inconsistent = outC->Context_1.positionCalculationNotConsistent; kcg_copy_trainPositionInfo_T_TrainPosition_Types_Pck( &trainPositionInfo, &outC->Context_1.trainPositionInfo); /* 2 */ linkedBG_missed_CalculateTrainPosition_Pkg_Linking_Pkg( &trainPositionInfo.trainPosition, &outC->BGs, kcg_true, &outC->Context_2); outC->errors.linkedBGMissed = outC->Context_2.missed; outC->errors.BG_LinkingConsistencyError = outC->Context_2.missed | errors_loc.BG_LinkingConsistencyError; /* 1 */ twoConsecutiveLinkedBGs_missed_CalculateTrainPosition_Pkg_Linking_Pkg( outC->Context_2.missed, &outC->Context_2.BG, &passedBG_asPositionedBG, reset, &outC->_1_Context_1); outC->errors.twoConsecutiveLinkedBGs_missed = outC->_1_Context_1.secondConsecutiveBG_missed; if (outC->Context_2.missed) { kcg_copy_positionedBG_T_TrainPosition_Types_Pck( &outC->errors.bg, &outC->Context_2.BG); } else { kcg_copy_positionedBG_T_TrainPosition_Types_Pck( &outC->errors.bg, &errors_loc.bg); } /* 1 */ calculateTrainpositionAttributes_CalculateTrainPosition_Pkg( &trainPositionInfo, currentOdometry, trainProperties, &outC->trainPosition); }