/* 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);
}
