/* 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_types::makeEmptyParabolaCurve */
void makeEmptyParabolaCurve_CalcBrakingCurves_types(
/* CalcBrakingCurves_types::makeEmptyParabolaCurve::Curve */ ParabolaCurve_T_CalcBrakingCurves_types *Curve)
{
/* CalcBrakingCurves_types::makeEmptyParabolaCurve::_L14 */
static array_real_114 _L14;
static kcg_int i;
for (i = 0; i < 114; i++) {
_L14[i] = 0.0;
}
kcg_copy_ParabolaCurveSpeeds_T_CalcBrakingCurves_types(
&(*Curve).speeds,
&_L14);
kcg_copy_ParabolaCurveAccelerations_T_CalcBrakingCurves_types(
&(*Curve).accelerations,
&(*Curve).speeds);
kcg_copy_ParabolaCurveDistances_T_CalcBrakingCurves_types(
&(*Curve).distances,
&_L14);
for (i = 0; i < 114; i++) {
(*Curve).valid[i] = kcg_false;
}
}
