/* 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::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) { /* CalcBrakingCurves_Pkg::internalOperators::InitAccWithTargetArc */ static ParabolaArc_T_CalcBrakingCurves_types mk_struct; /* CalcBrakingCurves_Pkg::internalOperators::InitAccWithTargetArc */ static A_internal_real_Type_SDM_Types_Pkg tmp1; /* CalcBrakingCurves_Pkg::internalOperators::InitAccWithTargetArc */ static ParabolaCurve_T_CalcBrakingCurves_types tmp; /* CalcBrakingCurves_Pkg::internalOperators::InitAccWithTargetArc::_L8 */ static V_internal_real_Type_SDM_Types_Pkg _L8; /* CalcBrakingCurves_Pkg::internalOperators::InitAccWithTargetArc::_L9 */ static kcg_int _L9; /* CalcBrakingCurves_Pkg::internalOperators::InitAccWithTargetArc::_L10 */ static L_internal_real_Type_SDM_Types_Pkg _L10; /* CalcBrakingCurves_Pkg::internalOperators::InitAccWithTargetArc::_L11 */ static kcg_int _L11; (*Output1).distance = EBDfoot; (*Output1).speed = (*target).speed; /* 1 */ getDistanceStepFromDistance_CalcBrakingCurves_Pkg_Step_Function_operators( EBDfoot, &(*aSafe).distance_definition, &_L10, &_L11); (*Output1).distanceIndex = _L11; /* 1 */ getSpeedStepFromSpeed_CalcBrakingCurves_Pkg_Step_Function_operators( (*target).speed, &(*aSafe).speed_definition, &_L8, &_L9); (*Output1).speedIndex = _L9; tmp1 = /* 2 */ getAFromAsafeByIndex_CalcBrakingCurves_Pkg_Step_Function_operators( _L11, _L9, &(*aSafe).data); /* 4 */ makeEmptyParabolaCurve_CalcBrakingCurves_types(&tmp); mk_struct.distance = EBDfoot; mk_struct.speed = (*target).speed; mk_struct.acceleration = tmp1; /* 1 */ setArc_CalcBrakingCurves_Pkg_internalOperators( &mk_struct, &tmp, 0, &(*Output1).BC); }
/* CalcBrakingCurves_Pkg::internalOperators::OneCycle */ void OneCycle_CalcBrakingCurves_Pkg_internalOperators( /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::currentDistance */ kcg_real currentDistance, /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::currentSpeed */ kcg_real currentSpeed, /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::currentDistanceIndex */ kcg_int currentDistanceIndex, /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::currentSpeedIndex */ kcg_int currentSpeedIndex, /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::backwardCalculation */ kcg_bool backwardCalculation, /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::inAsafe */ ASafe_T_CalcBrakingCurves_types *inAsafe, /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::newDistance */ kcg_real *newDistance, /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::newSpeed */ kcg_real *newSpeed, /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::newDistanceIndex */ kcg_int *newDistanceIndex, /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::newSpeedIndex */ kcg_int *newSpeedIndex, /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::newArc */ ParabolaArc_T_CalcBrakingCurves_types *newArc) { /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::currAcceleration */ kcg_real currAcceleration; /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::newSpeedValid */ kcg_bool newSpeedValid; /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::_L1 */ kcg_real _L1; /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::_L30 */ kcg_real _L30; /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::_L34 */ kcg_real _L34; /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::_L87 */ kcg_int _L87; /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::_L88 */ kcg_int _L88; /* CalcBrakingCurves_Pkg::internalOperators::OneCycle::_L98 */ kcg_bool _L98; currAcceleration = /* 1 */ getAFromAsafeByIndex_CalcBrakingCurves_Pkg_Step_Function_operators( currentDistanceIndex, currentSpeedIndex, &(*inAsafe).data); _L87 = /* changeDistanceIndex */ changeDistanceIndex_CalcBrakingCurves_Pkg_internalOperators( backwardCalculation, currentDistanceIndex); if ((0 <= _L87) & (_L87 < 15)) { _L30 = (*inAsafe).distance_definition[_L87]; } else { _L30 = 0.0; } /* 1 */ FormularNewSpeed_CalcBrakingCurves_Pkg_internalOperators( currAcceleration, currentDistance, _L30, currentSpeed, &_L1, &_L98); _L88 = /* 1 */ changeSpeedIndex_CalcBrakingCurves_Pkg_internalOperators( backwardCalculation, currentSpeedIndex); if ((0 <= _L88) & (_L88 < 10)) { _L34 = (*inAsafe).speed_definition[_L88]; } else { _L34 = 0.0; } newSpeedValid = _L98 & (_L1 <= _L34); if (newSpeedValid) { *newDistanceIndex = _L87; *newSpeedIndex = currentSpeedIndex; *newDistance = _L30; *newSpeed = _L1; } else { *newDistanceIndex = currentDistanceIndex; *newSpeedIndex = _L88; *newDistance = /* 1 */ FormularNewPosition_CalcBrakingCurves_Pkg_internalOperators( currentDistance, _L34, currentSpeed, currAcceleration); *newSpeed = _L34; } (*newArc).distance = *newDistance; (*newArc).speed = *newSpeed; (*newArc).acceleration = /* 2 */ getAFromAsafeByIndex_CalcBrakingCurves_Pkg_Step_Function_operators( *newDistanceIndex, *newSpeedIndex, &(*inAsafe).data); }