/* CalcBrakingCurves_Pkg::internalOperators::setArc */ void setArc_CalcBrakingCurves_Pkg_internalOperators( /* CalcBrakingCurves_Pkg::internalOperators::setArc::Arc */ ParabolaArc_T_CalcBrakingCurves_types *Arc, /* CalcBrakingCurves_Pkg::internalOperators::setArc::oldBC */ ParabolaCurve_T_CalcBrakingCurves_types *oldBC, /* CalcBrakingCurves_Pkg::internalOperators::setArc::index */ kcg_int index, /* CalcBrakingCurves_Pkg::internalOperators::setArc::newBC */ ParabolaCurve_T_CalcBrakingCurves_types *newBC) { kcg_copy_ParabolaCurveDistances_T_CalcBrakingCurves_types( &(*newBC).distances, &(*oldBC).distances); if ((0 <= index) & (index < 114)) { (*newBC).distances[index] = (*Arc).distance; } kcg_copy_ParabolaCurveSpeeds_T_CalcBrakingCurves_types( &(*newBC).speeds, &(*oldBC).speeds); if ((0 <= index) & (index < 114)) { (*newBC).speeds[index] = (*Arc).speed; } kcg_copy_ParabolaCurveAccelerations_T_CalcBrakingCurves_types( &(*newBC).accelerations, &(*oldBC).accelerations); if ((0 <= index) & (index < 114)) { (*newBC).accelerations[index] = (*Arc).acceleration; } kcg_copy_ParabolaCurveValid_T_CalcBrakingCurves_types( &(*newBC).valid, &(*oldBC).valid); if ((0 <= index) & (index < 114)) { (*newBC).valid[index] = kcg_true; } }
/* CalcBrakingCurves_Pkg::internalOperators::removeLeadingElement */ void removeLeadingElement_CalcBrakingCurves_Pkg_internalOperators( /* CalcBrakingCurves_Pkg::internalOperators::removeLeadingElement::oldBC */ ParabolaCurve_T_CalcBrakingCurves_types *oldBC, /* CalcBrakingCurves_Pkg::internalOperators::removeLeadingElement::newBC */ ParabolaCurve_T_CalcBrakingCurves_types *newBC) { array_real_25 tmp1; ParabolaCurveValid_T_CalcBrakingCurves_types tmp; /* CalcBrakingCurves_Pkg::internalOperators::removeLeadingElement::_L12 */ array_real_1 _L12; _L12[0] = - 1.0; kcg_copy_array_real_24(&tmp1[0], (array_real_24 *) &(*oldBC).distances[1]); kcg_copy_array_real_1(&tmp1[24], &_L12); kcg_copy_ParabolaCurveDistances_T_CalcBrakingCurves_types( &(*newBC).distances, &tmp1); kcg_copy_array_real_24(&tmp1[0], (array_real_24 *) &(*oldBC).speeds[1]); kcg_copy_array_real_1(&tmp1[24], &_L12); kcg_copy_ParabolaCurveSpeeds_T_CalcBrakingCurves_types( &(*newBC).speeds, &tmp1); kcg_copy_array_real_24( &tmp1[0], (array_real_24 *) &(*oldBC).accelerations[1]); kcg_copy_array_real_1(&tmp1[24], &_L12); kcg_copy_ParabolaCurveAccelerations_T_CalcBrakingCurves_types( &(*newBC).accelerations, &tmp1); kcg_copy_array_bool_24(&tmp[0], (array_bool_24 *) &(*oldBC).valid[1]); (&tmp[24])[0] = kcg_false; kcg_copy_ParabolaCurveValid_T_CalcBrakingCurves_types(&(*newBC).valid, &tmp); }
/* CalcBrakingCurves_Pkg::internalOperators::InitAccWithTargetArc */ void InitAccWithTargetArc_CalcBrakingCurves_Pkg_internalOperators( /* CalcBrakingCurves_Pkg::internalOperators::InitAccWithTargetArc::EBDfoot */ L_internal_real_Type_SDM_Types_Pkg EBDfoot, /* CalcBrakingCurves_Pkg::internalOperators::InitAccWithTargetArc::target */ Target_real_T_TargetManagement_types *target, /* CalcBrakingCurves_Pkg::internalOperators::InitAccWithTargetArc::aSafe */ ASafe_T_CalcBrakingCurves_types *aSafe, /* CalcBrakingCurves_Pkg::internalOperators::InitAccWithTargetArc::Output1 */ BCAccumulator_type_CalcBrakingCurves_Pkg *Output1) { kcg_int i; array_real_25 tmp3; array_real_25 tmp2; array_real_25 tmp1; ParabolaCurveValid_T_CalcBrakingCurves_types tmp; /* CalcBrakingCurves_Pkg::internalOperators::InitAccWithTargetArc::_L8 */ kcg_real _L8; /* CalcBrakingCurves_Pkg::internalOperators::InitAccWithTargetArc::_L9 */ kcg_int _L9; (*Output1).distance = EBDfoot; (*Output1).speed = (*target).speed; /* 1 */ getDistanceStepFromDistance_CalcBrakingCurves_Pkg_Step_Function_operators( EBDfoot, &(*aSafe).distance_definition, &_L8, &i); (*Output1).distanceIndex = i; /* 1 */ getSpeedStepFromSpeed_CalcBrakingCurves_Pkg_Step_Function_operators( (*target).speed, &(*aSafe).speed_definition, &_L8, &_L9); (*Output1).speedIndex = _L9; (&tmp1[0])[0] = /* 2 */ getAFromAsafeByIndex_CalcBrakingCurves_Pkg_Step_Function_operators( i, _L9, &(*aSafe).data); _L8 = - 1.0; for (i = 0; i < 24; i++) { (&tmp1[1])[i] = _L8; } (&tmp3[0])[0] = EBDfoot; kcg_copy_array_real_24(&tmp3[1], (array_real_24 *) &tmp1[1]); kcg_copy_ParabolaCurveDistances_T_CalcBrakingCurves_types( &(*Output1).BC.distances, &tmp3); (&tmp2[0])[0] = (*target).speed; kcg_copy_array_real_24(&tmp2[1], (array_real_24 *) &tmp1[1]); kcg_copy_ParabolaCurveSpeeds_T_CalcBrakingCurves_types( &(*Output1).BC.speeds, &tmp2); kcg_copy_ParabolaCurveAccelerations_T_CalcBrakingCurves_types( &(*Output1).BC.accelerations, &tmp1); (&tmp[0])[0] = kcg_true; for (i = 0; i < 24; i++) { (&tmp[1])[i] = kcg_false; } kcg_copy_ParabolaCurveValid_T_CalcBrakingCurves_types( &(*Output1).BC.valid, &tmp); }
/* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc */ void InitilizeAccForEBDFootCalc_CalcBrakingCurves_Pkg_internalOperators( /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::target */ Target_real_T_TargetManagement_types *target, /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::aSafe */ ASafe_T_CalcBrakingCurves_types *aSafe, /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::BCACCForEBDFoot */ BCAccumulator_type_CalcBrakingCurves_Pkg *BCACCForEBDFoot) { kcg_int i; /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::_L4 */ ParabolaCurveValid_T_CalcBrakingCurves_types _L4; /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::_L5 */ array_real_25 _L5; /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::_L11 */ kcg_real _L11; /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::_L15 */ kcg_real _L15; (*BCACCForEBDFoot).distance = (*target).distance; _L15 = /* 2 */ dV_ebi_for_V_SDM_Types_Pkg((*target).speed) + (*target).speed; (*BCACCForEBDFoot).speed = _L15; /* 3 */ getDistanceStepFromDistance_CalcBrakingCurves_Pkg_Step_Function_operators( (*target).distance, &(*aSafe).distance_definition, &_L11, &(*BCACCForEBDFoot).distanceIndex); /* 3 */ getSpeedStepFromSpeed_CalcBrakingCurves_Pkg_Step_Function_operators( _L15, &(*aSafe).speed_definition, &_L11, &(*BCACCForEBDFoot).speedIndex); _L11 = - 1.0; for (i = 0; i < 25; i++) { _L5[i] = _L11; _L4[i] = kcg_false; } kcg_copy_ParabolaCurveSpeeds_T_CalcBrakingCurves_types( &(*BCACCForEBDFoot).BC.speeds, &_L5); kcg_copy_ParabolaCurveAccelerations_T_CalcBrakingCurves_types( &(*BCACCForEBDFoot).BC.accelerations, &(*BCACCForEBDFoot).BC.speeds); kcg_copy_ParabolaCurveDistances_T_CalcBrakingCurves_types( &(*BCACCForEBDFoot).BC.distances, &_L5); kcg_copy_ParabolaCurveValid_T_CalcBrakingCurves_types( &(*BCACCForEBDFoot).BC.valid, &_L4); }
/* CalcBrakingCurves_Pkg::internalOperators::SortByDistances */ void SortByDistances_CalcBrakingCurves_Pkg_internalOperators( /* CalcBrakingCurves_Pkg::internalOperators::SortByDistances::reverseBC */ ParabolaCurve_T_CalcBrakingCurves_types *reverseBC, /* CalcBrakingCurves_Pkg::internalOperators::SortByDistances::sortedBC */ ParabolaCurve_T_CalcBrakingCurves_types *sortedBC) { /* CalcBrakingCurves_Pkg::internalOperators::SortByDistances */ static ParabolaCurveSpeeds_T_CalcBrakingCurves_types tmp2; /* CalcBrakingCurves_Pkg::internalOperators::SortByDistances */ static ParabolaCurveAccelerations_T_CalcBrakingCurves_types tmp1; /* CalcBrakingCurves_Pkg::internalOperators::SortByDistances */ static ParabolaCurveValid_T_CalcBrakingCurves_types tmp; /* CalcBrakingCurves_Pkg::internalOperators::SortByDistances */ static ParabolaCurve_T_CalcBrakingCurves_types acc; /* CalcBrakingCurves_Pkg::internalOperators::SortByDistances */ static kcg_bool cond_iterw; static kcg_int i; for (i = 0; i < 114; i++) { (*sortedBC).distances[i] = (*reverseBC).distances[113 - i]; tmp2[i] = (*reverseBC).speeds[113 - i]; tmp1[i] = (*reverseBC).accelerations[113 - i]; tmp[i] = (*reverseBC).valid[113 - i]; } kcg_copy_ParabolaCurveSpeeds_T_CalcBrakingCurves_types( &(*sortedBC).speeds, &tmp2); kcg_copy_ParabolaCurveAccelerations_T_CalcBrakingCurves_types( &(*sortedBC).accelerations, &tmp1); kcg_copy_ParabolaCurveValid_T_CalcBrakingCurves_types( &(*sortedBC).valid, &tmp); for (i = 0; i < 114; i++) { kcg_copy_ParabolaCurve_T_CalcBrakingCurves_types(&acc, sortedBC); /* 1 */ deleteLeadingUndefinedElements_CalcBrakingCurves_Pkg_internalOperators( &acc, &cond_iterw, sortedBC); if (!cond_iterw) { break; } } }