/* BasicLocationFunctions_Pkg::odoLoc_2_refLocations */
void odoLoc_2_refLocations_BasicLocationFunctions_Pkg(
/* BasicLocationFunctions_Pkg::odoLoc_2_refLocations::refLoc_2 */ LocWithInAcc_T_Obu_BasicTypes_Pkg *refLoc_2,
/* BasicLocationFunctions_Pkg::odoLoc_2_refLocations::refLoc_1 */ LocWithInAcc_T_Obu_BasicTypes_Pkg *refLoc_1,
/* BasicLocationFunctions_Pkg::odoLoc_2_refLocations::refOdo_2 */ OdometryLocations_T_Obu_BasicTypes_Pkg *refOdo_2,
/* BasicLocationFunctions_Pkg::odoLoc_2_refLocations::refOdo_1 */ OdometryLocations_T_Obu_BasicTypes_Pkg *refOdo_1,
/* BasicLocationFunctions_Pkg::odoLoc_2_refLocations::odo */ OdometryLocations_T_Obu_BasicTypes_Pkg *odo,
/* BasicLocationFunctions_Pkg::odoLoc_2_refLocations::location */ LocWithInAcc_T_Obu_BasicTypes_Pkg *location)
{
/* BasicLocationFunctions_Pkg::odoLoc_2_refLocations */ LocWithInAcc_T_Obu_BasicTypes_Pkg tmp;
/* BasicLocationFunctions_Pkg::odoLoc_2_refLocations::_L1 */ LocWithInAcc_T_Obu_BasicTypes_Pkg _L1;
/* BasicLocationFunctions_Pkg::odoLoc_2_refLocations::_L11 */ kcg_bool _L11;
/* BasicLocationFunctions_Pkg::odoLoc_2_refLocations::_L10 */ LocWithInAcc_T_Obu_BasicTypes_Pkg _L10;
/* 1 */
add_odo_2_Location_BasicLocationFunctions_Pkg(refLoc_1, refOdo_1, odo, &_L1);
/* 2 */
add_odo_2_Location_BasicLocationFunctions_Pkg(refLoc_2, refOdo_2, odo, &tmp);
/* 1 */
overlapOf_2_Locations_BasicLocationFunctions_Pkg(&tmp, &_L1, &_L10, &_L11);
/* 1 */ if (_L11) {
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(location, &_L10);
}
else {
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(location, &_L1);
}
}
/* EVC_MEM_Support_Pkg::maintainTrainProperties */
void maintainTrainProperties_EVC_MEM_Support_Pkg(
/* EVC_MEM_Support_Pkg::maintainTrainProperties::trainProperties */ trainProperties_T_TrainPosition_Types_Pck *trainProperties,
/* EVC_MEM_Support_Pkg::maintainTrainProperties::trainRunningNumber */ DMI_Train_Running_Number_T_DMI_Messages_Bothways_Pkg *trainRunningNumber,
/* EVC_MEM_Support_Pkg::maintainTrainProperties::trainData */ trainData_T_TIU_Types_Pkg *trainData,
/* EVC_MEM_Support_Pkg::maintainTrainProperties::nid_engine */ NID_ENGINE nid_engine,
/* EVC_MEM_Support_Pkg::maintainTrainProperties::UpdatedtrainProperties */ trainProperties_T_TrainPosition_Types_Pck *UpdatedtrainProperties)
{
(*UpdatedtrainProperties).nid_engine = nid_engine;
(*UpdatedtrainProperties).l_train = (*trainData).trainLength / 100;
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&(*UpdatedtrainProperties).d_baliseAntenna_2_frontend,
(LocWithInAcc_T_Obu_BasicTypes_Pkg *) &cD_Antenna2FE);
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&(*UpdatedtrainProperties).d_frontend_2_rearend,
(LocWithInAcc_T_Obu_BasicTypes_Pkg *) &cD_FE2RE);
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&(*UpdatedtrainProperties).locationAccuracy_DefaultValue,
(LocWithInAcc_T_Obu_BasicTypes_Pkg *) &cLocationAccuracyDefault);
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&(*UpdatedtrainProperties).centerDetectionAcc_DefaultValue,
(LocWithInAcc_T_Obu_BasicTypes_Pkg *) &cCenterDetectionAccuracyDefault);
/* 1 */ if ((*trainRunningNumber).valid) {
(*UpdatedtrainProperties).nid_operational =
(*trainRunningNumber).trainRunningNumber;
}
else {
(*UpdatedtrainProperties).nid_operational =
(*trainProperties).nid_operational;
}
}
/* CalculateTrainPosition_Pkg::BG_utilities_Pkg::positionLinkedBGs */
void positionLinkedBGs_CalculateTrainPosition_Pkg_BG_utilities_Pkg(
/* CalculateTrainPosition_Pkg::BG_utilities_Pkg::positionLinkedBGs::passedPositionedBG */ positionedBG_T_TrainPosition_Types_Pck *passedPositionedBG,
/* CalculateTrainPosition_Pkg::BG_utilities_Pkg::positionLinkedBGs::linkedBGs */ LinkedBGs_T_BG_Types_Pkg *linkedBGs,
/* CalculateTrainPosition_Pkg::BG_utilities_Pkg::positionLinkedBGs::trainProperties */ trainProperties_T_TrainPosition_Types_Pck *trainProperties,
/* CalculateTrainPosition_Pkg::BG_utilities_Pkg::positionLinkedBGs::linkedPositionedBGs */ linkedBGs_asPositionedBGs_T_TrainPosition_Types_Pck *linkedPositionedBGs)
{
/* CalculateTrainPosition_Pkg::BG_utilities_Pkg::positionLinkedBGs */ LocWithInAcc_T_Obu_BasicTypes_Pkg acc;
kcg_int i;
/* CalculateTrainPosition_Pkg::BG_utilities_Pkg::positionLinkedBGs::_L6 */ LocWithInAcc_T_Obu_BasicTypes_Pkg _L6;
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&_L6,
(LocWithInAcc_T_Obu_BasicTypes_Pkg *) &cLocWithInAcc_0_Obu_BasicTypes_Pkg);
for (i = 0; i < 33; i++) {
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(&acc, &_L6);
/* 1 */
positionLinkedBGs_itr_CalculateTrainPosition_Pkg_BG_utilities_Pkg(
&acc,
passedPositionedBG,
&(*linkedBGs)[i],
trainProperties,
&_L6,
&(*linkedPositionedBGs)[i]);
}
}
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::improveUnlinkedBGLocation */
void improveUnlinkedBGLocation_CalculateTrainPosition_Pkg_BG_relocation_Pkg(
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::improveUnlinkedBGLocation::passedLinkedBG_2 */ positionedBG_T_TrainPosition_Types_Pck *passedLinkedBG_2,
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::improveUnlinkedBGLocation::passedLinkedBG_1 */ positionedBG_T_TrainPosition_Types_Pck *passedLinkedBG_1,
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::improveUnlinkedBGLocation::unlinkedBG_in */ positionedBG_T_TrainPosition_Types_Pck *unlinkedBG_in,
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::improveUnlinkedBGLocation::unlinkedBG_out */ positionedBG_T_TrainPosition_Types_Pck *unlinkedBG_out)
{
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::improveUnlinkedBGLocation */
static LocWithInAcc_T_Obu_BasicTypes_Pkg tmp;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::improveUnlinkedBGLocation::_L31 */
static kcg_bool _L31;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::improveUnlinkedBGLocation::_L30 */
static LocWithInAcc_T_Obu_BasicTypes_Pkg _L30;
/* 1 */
odoLoc_2_refLocations_BasicLocationFunctions_Pkg(
&(*passedLinkedBG_2).location,
&(*passedLinkedBG_1).location,
&(*passedLinkedBG_2).infoFromPassing.BG_Header.bgPosition.odo,
&(*passedLinkedBG_1).infoFromPassing.BG_Header.bgPosition.odo,
&(*unlinkedBG_in).infoFromPassing.BG_Header.bgPosition.odo,
&tmp);
/* 1 */
overlapOf_2_Locations_BasicLocationFunctions_Pkg(
&tmp,
&(*unlinkedBG_in).location,
&_L30,
&_L31);
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
unlinkedBG_out,
unlinkedBG_in);
/* 1 */ if ((*passedLinkedBG_2).valid & ((*passedLinkedBG_2).q_link ==
Q_LINK_Linked) & (*passedLinkedBG_2).infoFromPassing.valid &
!kcg_comp_positionedBG_T_TrainPosition_Types_Pck(
passedLinkedBG_2,
passedLinkedBG_1) & ((*passedLinkedBG_1).valid &
((*passedLinkedBG_1).q_link == Q_LINK_Linked) &
(*passedLinkedBG_1).infoFromPassing.valid) & ((*unlinkedBG_in).valid &
((*unlinkedBG_in).q_link == Q_LINK_Unlinked)) & _L31) {
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&(*unlinkedBG_out).location,
&_L30);
}
else {
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&(*unlinkedBG_out).location,
&(*unlinkedBG_in).location);
}
}
/* MA_Request::op_3_8_2_3_a */
void op_3_8_2_3_a_MA_Request(
/* MA_Request::op_3_8_2_3_a::t_mar */T_MAR t_mar,
/* MA_Request::op_3_8_2_3_a::trainPosition */trainPosition_T_TrainPosition_Types_Pck *trainPosition,
/* MA_Request::op_3_8_2_3_a::preindicationLoc */L_internal_Type_Obu_BasicTypes_Pkg preindicationLoc,
/* MA_Request::op_3_8_2_3_a::odometry */odometry_T_Obu_BasicTypes_Pkg *odometry,
outC_op_3_8_2_3_a_MA_Request *outC)
{
outC->_L2 = t_mar;
outC->_L17 = (kcg_real) outC->_L2;
outC->_L3 = preindicationLoc;
kcg_copy_trainPosition_T_TrainPosition_Types_Pck(&outC->_L1, trainPosition);
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&outC->_L5,
&outC->_L1.trainPosition);
outC->_L7 = outC->_L5.d_max;
outC->_L8 = outC->_L3 - outC->_L7;
outC->_L10 = (kcg_real) outC->_L8;
outC->_L13 = 100.0;
/* 1 */ Safe_DIV_real(outC->_L10, outC->_L13, &outC->Context_1);
outC->_L12 = outC->Context_1.Result;
kcg_copy_odometry_T_Obu_BasicTypes_Pkg(&outC->_L4, odometry);
kcg_copy_OdometrySpeeds_T_Obu_BasicTypes_Pkg(&outC->_L9, &outC->_L4.speed);
outC->_L19 = outC->_L9.v_upper;
outC->_L11 = (kcg_real) outC->_L19;
outC->_L14 = 0.28;
outC->_L15 = outC->_L11 * outC->_L14;
/* 2 */ Safe_DIV_real(outC->_L12, outC->_L15, &outC->Context_2);
outC->_L16 = outC->Context_2.Result;
outC->_L18 = outC->_L17 <= outC->_L16;
outC->triggerMA = outC->_L18;
}
/* 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;
}
/* ProvidePositionReport_Pkg::AddBGToFIFO */
void AddBGToFIFO_ProvidePositionReport_Pkg(
/* ProvidePositionReport_Pkg::AddBGToFIFO::reportedBGs */ ReportedBGList_T_ProvidePositionReport_Pkg *reportedBGs,
/* ProvidePositionReport_Pkg::AddBGToFIFO::reportIsSent */ kcg_bool reportIsSent,
/* ProvidePositionReport_Pkg::AddBGToFIFO::trainPos */ trainPosition_T_TrainPosition_Types_Pck *trainPos,
/* ProvidePositionReport_Pkg::AddBGToFIFO::out_reportedBGs */ ReportedBGList_T_ProvidePositionReport_Pkg *out_reportedBGs)
{
/* 1 */ if (reportIsSent) {
(*out_reportedBGs)[0].valid = reportIsSent;
(*out_reportedBGs)[0].nidBG = (*trainPos).LRBG.nid_bg;
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&(*out_reportedBGs)[0].location,
&(*trainPos).LRBG.location);
kcg_copy_ReportedBG_T_ProvidePositionReport_Pkg(
&(*out_reportedBGs)[1],
&(*reportedBGs)[0]);
kcg_copy_ReportedBG_T_ProvidePositionReport_Pkg(
&(*out_reportedBGs)[2],
&(*reportedBGs)[1]);
kcg_copy_ReportedBG_T_ProvidePositionReport_Pkg(
&(*out_reportedBGs)[3],
&(*reportedBGs)[2]);
kcg_copy_ReportedBG_T_ProvidePositionReport_Pkg(
&(*out_reportedBGs)[4],
&(*reportedBGs)[3]);
kcg_copy_ReportedBG_T_ProvidePositionReport_Pkg(
&(*out_reportedBGs)[5],
&(*reportedBGs)[4]);
kcg_copy_ReportedBG_T_ProvidePositionReport_Pkg(
&(*out_reportedBGs)[6],
&(*reportedBGs)[5]);
kcg_copy_ReportedBG_T_ProvidePositionReport_Pkg(
&(*out_reportedBGs)[7],
&(*reportedBGs)[6]);
}
else {
kcg_copy_ReportedBGList_T_ProvidePositionReport_Pkg(
out_reportedBGs,
reportedBGs);
}
}
/* CalculateTrainPosition_Pkg::Linking_Pkg::linkedBG_missed */
void linkedBG_missed_CalculateTrainPosition_Pkg_Linking_Pkg(
/* CalculateTrainPosition_Pkg::Linking_Pkg::linkedBG_missed::position */ LocWithInAcc_T_Obu_BasicTypes_Pkg *position,
/* CalculateTrainPosition_Pkg::Linking_Pkg::linkedBG_missed::BGs_in */ positionedBGs_T_TrainPosition_Types_Pck *BGs_in,
/* CalculateTrainPosition_Pkg::Linking_Pkg::linkedBG_missed::enable */ kcg_bool enable,
outC_linkedBG_missed_CalculateTrainPosition_Pkg_Linking_Pkg *outC)
{
/* CalculateTrainPosition_Pkg::Linking_Pkg::linkedBG_missed */
static LocWithInAcc_T_Obu_BasicTypes_Pkg tmp1;
/* CalculateTrainPosition_Pkg::Linking_Pkg::linkedBG_missed */
static LocWithInAcc_T_Obu_BasicTypes_Pkg tmp;
/* CalculateTrainPosition_Pkg::Linking_Pkg::linkedBG_missed::_L1 */
static kcg_bool _L1;
/* CalculateTrainPosition_Pkg::Linking_Pkg::linkedBG_missed::_L52 */
static LocWithInAcc_T_Obu_BasicTypes_Pkg _L52;
/* CalculateTrainPosition_Pkg::Linking_Pkg::linkedBG_missed::_L69 */
static kcg_bool _L69;
/* CalculateTrainPosition_Pkg::Linking_Pkg::linkedBG_missed::_L67 */
static kcg_int _L67;
/* last_init_ck_position */ if (outC->init) {
outC->init = kcg_false;
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&outC->BG,
(positionedBG_T_TrainPosition_Types_Pck *)
&cNoPositionedBG_CalculateTrainPosition_Pkg);
outC->indexOfBG = cNoValidIndex_CalculateTrainPosition_Pkg;
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&_L52,
(LocWithInAcc_T_Obu_BasicTypes_Pkg *)
&cLocWithInAcc_0_Obu_BasicTypes_Pkg);
}
else {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(&outC->BG, &outC->_L3);
outC->indexOfBG = outC->_L2;
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(&_L52, &outC->rem_position);
}
/* 2 */ sub_2_distances_BasicLocationFunctions_Pkg(position, &_L52, &tmp1);
_L1 = tmp1.nominal >= 0;
/* ck_movedAhead */ if (_L1) {
tmp.nominal = (*position).nominal;
tmp.d_min = _L52.nominal + _L52.d_min - (*position).nominal;
tmp.d_max = (*position).d_max;
}
else {
tmp.nominal = (*position).nominal;
tmp.d_min = (*position).d_min;
tmp.d_max = _L52.nominal + _L52.d_max - (*position).nominal;
}
/* 1 */
posInRangeOfBG_CalculateTrainPosition_Pkg_BG_utilities_Pkg(
&tmp,
BGs_in,
enable,
&_L1,
&outC->_L2,
&outC->_L3);
/* 1 */ FallingEdge_digital(_L1, &outC->Context_1);
outC->missed = ((/* 1 */
positionedBGs_ids_notEqual_CalculateTrainPosition_Pkg_BG_utilities_Pkg(
&outC->_L3,
&outC->BG) & _L1) | outC->Context_1.FE_Output) & (outC->BG.q_link ==
Q_LINK_Linked) & !outC->BG.infoFromPassing.valid;
outC->BG.missed = outC->missed;
/* 1 */
indexOfBG_by_id_CalculateTrainPosition_Pkg_BG_utilities_Pkg(
&outC->BG,
BGs_in,
outC->missed,
&_L67,
&_L1,
&_L69);
/* 1 */ if (_L1 & outC->missed) {
kcg_copy_positionedBGs_T_TrainPosition_Types_Pck(&outC->BGs_out, BGs_in);
if ((0 <= _L67) & (_L67 < 41)) {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&outC->BGs_out[_L67],
&outC->BG);
}
}
else {
kcg_copy_positionedBGs_T_TrainPosition_Types_Pck(&outC->BGs_out, BGs_in);
}
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(&outC->rem_position, position);
}
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_ahead_itr */
void recalculate_BG_locations_ahead_itr_CalculateTrainPosition_Pkg_BG_relocation_Pkg(
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_ahead_itr::refBGs_in */ refBGs_T_CalculateTrainPosition_Pkg_BG_relocation_Pkg *refBGs_in,
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_ahead_itr::BG_in */ positionedBG_T_TrainPosition_Types_Pck *BG_in,
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_ahead_itr::trainProperties */ trainProperties_T_TrainPosition_Types_Pck *trainProperties,
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_ahead_itr::refBGs_out */ refBGs_T_CalculateTrainPosition_Pkg_BG_relocation_Pkg *refBGs_out,
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_ahead_itr::BG_out */ positionedBG_T_TrainPosition_Types_Pck *BG_out)
{
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_ahead_itr */ LocWithInAcc_T_Obu_BasicTypes_Pkg tmp1;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_ahead_itr */ positionedBG_T_TrainPosition_Types_Pck tmp;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_ahead_itr::BGin_is_refBG */ kcg_bool BGin_is_refBG;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_ahead_itr::refLocation */ LocWithInAcc_T_Obu_BasicTypes_Pkg refLocation;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_ahead_itr::refBG */ positionedBG_T_TrainPosition_Types_Pck refBG;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_ahead_itr::relocatedBG */ positionedBG_T_TrainPosition_Types_Pck relocatedBG;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_ahead_itr::sumOfBestDistances */ LocWithInAcc_T_Obu_BasicTypes_Pkg sumOfBestDistances;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_ahead_itr::_L96 */ kcg_bool _L96;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_ahead_itr::_L106 */ kcg_bool _L106;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_ahead_itr::_L137 */ kcg_bool _L137;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_ahead_itr::_L302 */ LocWithInAcc_T_Obu_BasicTypes_Pkg _L302;
BGin_is_refBG = /* 3 */
positionedBGs_ids_equal_CalculateTrainPosition_Pkg_BG_utilities_Pkg(
&(*refBGs_in).refBG,
BG_in);
(*refBGs_out).recalculate = BGin_is_refBG | ((*refBGs_in).recalculate &
(*BG_in).valid);
/* calculateLocalBGInaccuracies */
calculateLocalBGInaccuracies_CalculateTrainPosition_Pkg_BG_relocation_Pkg(
BG_in,
trainProperties,
&refLocation);
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&_L302,
(LocWithInAcc_T_Obu_BasicTypes_Pkg *) &cLocWithInAcc_0_Obu_BasicTypes_Pkg);
_L302.nominal = (*BG_in).location.nominal;
/* 6 */
add_2_Distances_BasicLocationFunctions_Pkg(&refLocation, &_L302, &tmp1);
/* 1 */
overlapOf_2_Locations_BasicLocationFunctions_Pkg(
&tmp1,
&(*BG_in).location,
&_L302,
&_L106);
/* 14 */ if (BGin_is_refBG) {
/* 31 */ if (_L106) {
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(&refLocation, &_L302);
}
else {
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&refLocation,
&(*BG_in).location);
}
}
else {
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&refLocation,
&(*BG_in).location);
}
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&relocatedBG,
&(*refBGs_in).refBG);
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&relocatedBG.location,
&refLocation);
/* 24 */ if (BGin_is_refBG) {
/* 25 */ if ((*refBGs_in).refBG.valid &
(*refBGs_in).refBG.infoFromLinking.valid) {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(&refBG, &relocatedBG);
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&refBG.infoFromLinking.expectedLocation,
&refLocation);
}
else {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(&refBG, &relocatedBG);
}
}
else {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&refBG,
&(*refBGs_in).refBG);
}
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(&(*refBGs_out).refBG, &refBG);
_L106 = Q_LINK_Linked == refBG.q_link;
_L96 = BGin_is_refBG & !_L106 & refBG.infoFromPassing.valid &
(*refBGs_in).prevLinkedBG.valid &
(*refBGs_in).prevLinkedBG.infoFromPassing.valid;
/* 2 */
sub_2_odoDistances_BasicLocationFunctions_Pkg(
&refBG.infoFromPassing.BG_Header.bgPosition.odo,
&(*refBGs_in).prevLinkedBG.infoFromPassing.BG_Header.bgPosition.odo,
&tmp1);
/* 9 */
add_2_Distances_BasicLocationFunctions_Pkg(
&tmp1,
&(*refBGs_in).prevLinkedBG.infoFromPassing.BG_Header.BG_centerDetectionInaccuraccuracies,
&_L302);
/* 27 */ if (BGin_is_refBG & (*BG_in).valid) {
/* 30 */ if (_L96) {
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(&tmp1, &_L302);
}
else {
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&tmp1,
(LocWithInAcc_T_Obu_BasicTypes_Pkg *)
&cLocWithInAcc_0_Obu_BasicTypes_Pkg);
}
/* 4 */
sub_2_distances_BasicLocationFunctions_Pkg(
(LocWithInAcc_T_Obu_BasicTypes_Pkg *) &cLocWithInAcc_0_Obu_BasicTypes_Pkg,
&tmp1,
&sumOfBestDistances);
}
else /* 28 */ if ((*BG_in).valid & (*BG_in).infoFromLinking.valid) {
/* 7 */
add_2_Distances_BasicLocationFunctions_Pkg(
&(*BG_in).infoFromLinking.d_link,
&(*refBGs_in).sumOfBestDistances,
&sumOfBestDistances);
}
else /* 29 */ if ((*BG_in).valid & ((*BG_in).q_link == Q_LINK_Linked) &
(*BG_in).infoFromPassing.valid & (*refBGs_in).prevLinkedBG.valid &
(*refBGs_in).prevLinkedBG.infoFromPassing.valid) {
/* 4 */
sub_2_odoDistances_BasicLocationFunctions_Pkg(
&(*BG_in).infoFromPassing.BG_Header.bgPosition.odo,
&(*refBGs_in).prevLinkedBG.infoFromPassing.BG_Header.bgPosition.odo,
&refLocation);
/* 8 */
add_2_Distances_BasicLocationFunctions_Pkg(
&refLocation,
&(*refBGs_in).sumOfBestDistances,
&sumOfBestDistances);
}
else {
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&sumOfBestDistances,
&(*refBGs_in).sumOfBestDistances);
}
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&(*refBGs_out).sumOfBestDistances,
&sumOfBestDistances);
/* 23 */ if ((*refBGs_in).recalculate) {
/* 1 */
recalculate_BG_location_ahead_CalculateTrainPosition_Pkg_BG_relocation_Pkg(
BG_in,
&(*refBGs_in).prevLinkedBG,
&(*refBGs_in).refBG,
&sumOfBestDistances,
trainProperties,
&relocatedBG);
}
else {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(&relocatedBG, BG_in);
}
_L137 = Q_LINK_Unlinked == refBG.q_link;
/* 17 */ if (BGin_is_refBG & _L106) {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(&tmp, &refBG);
}
else /* 18 */ if (_L96) {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&tmp,
&(*refBGs_in).prevLinkedBG);
/* 2 */
sub_2_distances_BasicLocationFunctions_Pkg(
&refBG.location,
&_L302,
&tmp.location);
}
else /* 19 */ if (Q_LINK_Linked == relocatedBG.q_link) {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(&tmp, &relocatedBG);
}
else {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&tmp,
&(*refBGs_in).prevLinkedBG);
}
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&(*refBGs_out).prevLinkedBG,
&tmp);
/* 20 */ if (BGin_is_refBG & _L137) {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(&tmp, &refBG);
}
else /* 21 */ if (BGin_is_refBG & !_L137 & refBG.infoFromPassing.valid &
(*refBGs_in).prevUnlinkedBG.valid &
(*refBGs_in).prevUnlinkedBG.infoFromPassing.valid) {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&tmp,
&(*refBGs_in).prevUnlinkedBG);
/* 3 */
sub_2_odoDistances_BasicLocationFunctions_Pkg(
&refBG.infoFromPassing.BG_Header.bgPosition.odo,
&(*refBGs_in).prevUnlinkedBG.infoFromPassing.BG_Header.bgPosition.odo,
&refLocation);
/* 12 */
add_2_Distances_BasicLocationFunctions_Pkg(
&refLocation,
&(*refBGs_in).prevUnlinkedBG.infoFromPassing.BG_Header.BG_centerDetectionInaccuraccuracies,
&tmp1);
/* 3 */
sub_2_distances_BasicLocationFunctions_Pkg(
&refBG.location,
&tmp1,
&tmp.location);
}
else /* 22 */ if (Q_LINK_Unlinked == relocatedBG.q_link) {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(&tmp, &relocatedBG);
}
else {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&tmp,
&(*refBGs_in).prevUnlinkedBG);
}
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&(*refBGs_out).prevUnlinkedBG,
&tmp);
/* 26 */ if (BGin_is_refBG) {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(BG_out, &refBG);
}
else {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(BG_out, &relocatedBG);
}
}
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_astern_itr */
void recalculate_BG_locations_astern_itr_CalculateTrainPosition_Pkg_BG_relocation_Pkg(
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_astern_itr::refBGs_in */ refBGs_T_CalculateTrainPosition_Pkg_BG_relocation_Pkg *refBGs_in,
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_astern_itr::BG_in */ positionedBG_T_TrainPosition_Types_Pck *BG_in,
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_astern_itr::trainProperties */ trainProperties_T_TrainPosition_Types_Pck *trainProperties,
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_astern_itr::refBGs_out */ refBGs_T_CalculateTrainPosition_Pkg_BG_relocation_Pkg *refBGs_out,
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_astern_itr::BG_out */ positionedBG_T_TrainPosition_Types_Pck *BG_out)
{
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_astern_itr */
static LocWithInAcc_T_Obu_BasicTypes_Pkg tmp1;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_astern_itr */
static LocWithInAcc_T_Obu_BasicTypes_Pkg tmp;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_astern_itr::BGin_is_refBG */
static kcg_bool BGin_is_refBG;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_astern_itr::refBG */
static positionedBG_T_TrainPosition_Types_Pck refBG;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_astern_itr::relocatedBG */
static positionedBG_T_TrainPosition_Types_Pck relocatedBG;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_astern_itr::sumOfBestDistances */
static LocWithInAcc_T_Obu_BasicTypes_Pkg sumOfBestDistances;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_astern_itr::_L106 */
static kcg_bool _L106;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_locations_astern_itr::_L137 */
static kcg_bool _L137;
BGin_is_refBG = /* 1 */
positionedBGs_ids_equal_CalculateTrainPosition_Pkg_BG_utilities_Pkg(
&(*refBGs_in).refBG,
BG_in);
(*refBGs_out).recalculate = BGin_is_refBG | ((*refBGs_in).recalculate &
(*BG_in).valid);
/* 13 */ if (BGin_is_refBG) {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(&refBG, BG_in);
}
else {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&refBG,
&(*refBGs_in).refBG);
}
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(&(*refBGs_out).refBG, &refBG);
_L106 = Q_LINK_Linked == refBG.q_link;
_L137 = (*BG_in).valid & ((*BG_in).q_link == Q_LINK_Linked);
/* 16 */ if (BGin_is_refBG) {
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&sumOfBestDistances,
(LocWithInAcc_T_Obu_BasicTypes_Pkg *)
&cLocWithInAcc_0_Obu_BasicTypes_Pkg);
}
else /* 17 */ if (!(*refBGs_in).prevLinkedBG.valid &
(*refBGs_in).refBG.valid & ((*refBGs_in).refBG.q_link == Q_LINK_Unlinked) &
(*refBGs_in).refBG.infoFromPassing.valid & (*BG_in).infoFromPassing.valid &
_L137) {
/* 2 */
sub_2_odoDistances_BasicLocationFunctions_Pkg(
&(*refBGs_in).refBG.infoFromPassing.BG_Header.bgPosition.odo,
&(*BG_in).infoFromPassing.BG_Header.bgPosition.odo,
&tmp1);
/* 3 */
sub_2_distances_BasicLocationFunctions_Pkg(
&(*refBGs_in).sumOfBestDistances,
&tmp1,
&sumOfBestDistances);
}
else /* 14 */ if (_L137 & (*refBGs_in).prevLinkedBG.valid &
(*refBGs_in).prevLinkedBG.infoFromLinking.valid) {
/* 1 */
sub_2_distances_BasicLocationFunctions_Pkg(
&(*refBGs_in).sumOfBestDistances,
&(*refBGs_in).prevLinkedBG.infoFromLinking.d_link,
&sumOfBestDistances);
}
else /* 15 */ if (_L137 & (*refBGs_in).prevLinkedBG.valid &
!(*refBGs_in).prevLinkedBG.infoFromLinking.valid &
(*refBGs_in).prevLinkedBG.infoFromPassing.valid &
(*BG_in).infoFromPassing.valid) {
/* 1 */
sub_2_odoDistances_BasicLocationFunctions_Pkg(
&(*refBGs_in).prevLinkedBG.infoFromPassing.BG_Header.bgPosition.odo,
&(*BG_in).infoFromPassing.BG_Header.bgPosition.odo,
&tmp);
/* 2 */
sub_2_distances_BasicLocationFunctions_Pkg(
&(*refBGs_in).sumOfBestDistances,
&tmp,
&sumOfBestDistances);
}
else {
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&sumOfBestDistances,
&(*refBGs_in).sumOfBestDistances);
}
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&(*refBGs_out).sumOfBestDistances,
&sumOfBestDistances);
/* 9 */ if ((*refBGs_in).recalculate) {
/* 1 */
recalculate_BG_location_astern_CalculateTrainPosition_Pkg_BG_relocation_Pkg(
BG_in,
&(*refBGs_in).prevLinkedBG,
&(*refBGs_in).refBG,
&sumOfBestDistances,
trainProperties,
&relocatedBG);
}
else {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(&relocatedBG, BG_in);
}
_L137 = Q_LINK_Unlinked == refBG.q_link;
/* 3 */ if (BGin_is_refBG & _L106) {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&(*refBGs_out).prevLinkedBG,
&refBG);
}
else /* 4 */ if (BGin_is_refBG & !_L106) {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&(*refBGs_out).prevLinkedBG,
(positionedBG_T_TrainPosition_Types_Pck *)
&cNoPositionedBG_CalculateTrainPosition_Pkg);
}
else /* 5 */ if (Q_LINK_Linked == relocatedBG.q_link) {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&(*refBGs_out).prevLinkedBG,
&relocatedBG);
}
else {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&(*refBGs_out).prevLinkedBG,
&(*refBGs_in).prevLinkedBG);
}
/* 8 */ if (BGin_is_refBG & _L137) {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&(*refBGs_out).prevUnlinkedBG,
&refBG);
}
else /* 7 */ if (BGin_is_refBG & !_L137) {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&(*refBGs_out).prevUnlinkedBG,
(positionedBG_T_TrainPosition_Types_Pck *)
&cNoPositionedBG_CalculateTrainPosition_Pkg);
}
else /* 6 */ if (Q_LINK_Unlinked == relocatedBG.q_link) {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&(*refBGs_out).prevUnlinkedBG,
&relocatedBG);
}
else {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&(*refBGs_out).prevUnlinkedBG,
&(*refBGs_in).prevUnlinkedBG);
}
/* 12 */ if (BGin_is_refBG) {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(BG_out, &refBG);
}
else {
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(BG_out, &relocatedBG);
}
}
/* CalculateTrainPosition_Pkg::calculateTrainpositionAttributes */
void calculateTrainpositionAttributes_CalculateTrainPosition_Pkg(
/* CalculateTrainPosition_Pkg::calculateTrainpositionAttributes::trainPositionInfo */ trainPositionInfo_T_TrainPosition_Types_Pck *trainPositionInfo,
/* CalculateTrainPosition_Pkg::calculateTrainpositionAttributes::currentOdometry */ odometry_T_Obu_BasicTypes_Pkg *currentOdometry,
/* CalculateTrainPosition_Pkg::calculateTrainpositionAttributes::trainProperties */ trainProperties_T_TrainPosition_Types_Pck *trainProperties,
/* CalculateTrainPosition_Pkg::calculateTrainpositionAttributes::trainPosition */ trainPosition_T_TrainPosition_Types_Pck *trainPosition)
{
/* CalculateTrainPosition_Pkg::calculateTrainpositionAttributes::_L243 */ LocWithInAcc_T_Obu_BasicTypes_Pkg _L243;
/* CalculateTrainPosition_Pkg::calculateTrainpositionAttributes::_L266 */ kcg_bool _L266;
/* CalculateTrainPosition_Pkg::calculateTrainpositionAttributes::_L366 */ LocWithInAcc_T_Obu_BasicTypes_Pkg _L366;
(*trainPosition).valid = (*trainPositionInfo).valid;
(*trainPosition).timestamp = (*trainPositionInfo).timestamp;
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&(*trainPosition).trainPosition,
&(*trainPositionInfo).trainPosition);
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&(*trainPosition).LRBG,
&(*trainPositionInfo).lastPassedLinkedBG);
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(
&(*trainPosition).prvLRBG,
&(*trainPositionInfo).prevPassedLinkedBG);
(*trainPosition).linkingIsUsedOnboard =
(*trainPositionInfo).linkingIsUsedOnboard;
_L266 = !(*trainPositionInfo).lastPassedLinkedBG.valid;
(*trainPosition).noCoordinateSystemHasBeenAssigned = _L266 |
(*trainPositionInfo).lastPassedLinkedBG.infoFromPassing.BG_Header.noCoordinateSystemHasBeenAssigned;
/* 1 */
add_2_Distances_BasicLocationFunctions_Pkg(
&(*trainPositionInfo).trainPosition,
&(*trainProperties).d_baliseAntenna_2_frontend,
&_L243);
(*trainPosition).estimatedFrontEndPosition = _L243.nominal;
(*trainPosition).minSafeFrontEndPosition = _L243.nominal + _L243.d_min;
(*trainPosition).maxSafeFrontEndPostion = _L243.nominal + _L243.d_max;
/* 1 */
sub_2_distances_BasicLocationFunctions_Pkg(
&_L243,
&(*trainProperties).d_frontend_2_rearend,
&_L366);
(*trainPosition).estimatedRearEndPosition = _L366.nominal;
(*trainPosition).minSafeRearEndPosition = _L366.nominal + _L366.d_min;
(*trainPosition).maxSafeRearEndPosition = _L366.nominal + _L366.d_max;
(*trainPosition).trainPositionIsUnknown = _L266 | (/* nidC_nidBG_2_NIDLRBG */
nidC_nidBG_2_NIDLRBG_CalculateTrainPosition_Pkg_BG_utilities_Pkg(
(*trainPositionInfo).lastPassedLinkedBG.valid,
(*trainPositionInfo).lastPassedLinkedBG.nid_c,
(*trainPositionInfo).lastPassedLinkedBG.nid_bg) ==
cNID_LRBG_unknown_BG_Types_Pkg);
(*trainPosition).nominalOrReverseToLRBG = /* 1 */
frontendToLRBG_CalculateTrainPosition_Pkg_Pos_Pkg(
&(*trainPositionInfo).lastPassedLinkedBG,
trainPositionInfo,
trainProperties);
/* 1 */ if (_L266) {
(*trainPosition).trainOrientationToLRBG = Q_DIRLRBG_Unknown;
}
else {
(*trainPosition).trainOrientationToLRBG =
(*trainPositionInfo).lastPassedLinkedBG.infoFromPassing.BG_Header.trainOrientationToBG;
}
(*trainPosition).trainRunningDirectionToLRBG = /* 1 */
trainMoveDir_vs_refBG_CalculateTrainPosition_Pkg_Pos_Pkg(
currentOdometry,
&(*trainPositionInfo).lastPassedLinkedBG);
}
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_location_astern */
void recalculate_BG_location_astern_CalculateTrainPosition_Pkg_BG_relocation_Pkg(
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_location_astern::BG_in */ positionedBG_T_TrainPosition_Types_Pck *BG_in,
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_location_astern::prevLinkedBG */ positionedBG_T_TrainPosition_Types_Pck *prevLinkedBG,
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_location_astern::refBG */ positionedBG_T_TrainPosition_Types_Pck *refBG,
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_location_astern::sumOfBestDistances */ LocWithInAcc_T_Obu_BasicTypes_Pkg *sumOfBestDistances,
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_location_astern::trainProperties */ trainProperties_T_TrainPosition_Types_Pck *trainProperties,
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_location_astern::BG_out */ positionedBG_T_TrainPosition_Types_Pck *BG_out)
{
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_location_astern */ LocWithInAcc_T_Obu_BasicTypes_Pkg tmp1;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_location_astern */ LocWithInAcc_T_Obu_BasicTypes_Pkg tmp;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_location_astern::_L181 */ LocWithInAcc_T_Obu_BasicTypes_Pkg _L181;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_location_astern::_L195 */ kcg_bool _L195;
/* CalculateTrainPosition_Pkg::BG_relocation_Pkg::recalculate_BG_location_astern::_L212 */ kcg_bool _L212;
/* 1 */
add_2_Distances_BasicLocationFunctions_Pkg(
&(*refBG).location,
sumOfBestDistances,
&tmp1);
/* 1 */
calculateLocalBGInaccuracies_CalculateTrainPosition_Pkg_BG_relocation_Pkg(
BG_in,
trainProperties,
&tmp);
/* 1 */ sub_2_distances_BasicLocationFunctions_Pkg(&tmp1, &tmp, &_L181);
/* 1 */
sub_2_odoDistances_BasicLocationFunctions_Pkg(
&(*prevLinkedBG).infoFromPassing.BG_Header.bgPosition.odo,
&(*BG_in).infoFromPassing.BG_Header.bgPosition.odo,
&tmp);
/* 3 */
add_2_Distances_BasicLocationFunctions_Pkg(
&(*BG_in).infoFromPassing.BG_Header.BG_centerDetectionInaccuraccuracies,
&tmp,
&tmp1);
/* 3 */
sub_2_distances_BasicLocationFunctions_Pkg(
&(*prevLinkedBG).location,
&tmp1,
&tmp);
/* 1 */
overlapOf_2_Locations_BasicLocationFunctions_Pkg(&_L181, &tmp, &tmp1, &_L212);
_L195 = (*BG_in).valid & (*BG_in).infoFromPassing.valid &
(*prevLinkedBG).valid & (*prevLinkedBG).infoFromPassing.valid;
kcg_copy_positionedBG_T_TrainPosition_Types_Pck(BG_out, BG_in);
/* 1 */ if ((*BG_in).valid & (*refBG).valid & ((*BG_in).q_link ==
Q_LINK_Linked)) {
/* 3 */ if (_L212 & _L195) {
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(&(*BG_out).location, &tmp1);
}
else {
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(&(*BG_out).location, &_L181);
}
}
else /* 2 */ if (_L195) {
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(&(*BG_out).location, &tmp);
}
else {
kcg_copy_LocWithInAcc_T_Obu_BasicTypes_Pkg(
&(*BG_out).location,
&(*BG_in).location);
}
}
