125 lines
4.0 KiB
Plaintext
125 lines
4.0 KiB
Plaintext
# ****************************************************************************
|
|
# CUI
|
|
#
|
|
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
|
|
#
|
|
# The use, dissemination or disclosure of data in this file is subject to
|
|
# limitation or restriction. See accompanying README and LICENSE for details.
|
|
# ****************************************************************************
|
|
|
|
//TODO use default values from higher level context (processor or parent behavior that holds default)
|
|
|
|
|
|
#assumes aDraw duration & layer is set
|
|
script void DrawRoute(WsfDraw aDraw, WsfRoute aRoute)
|
|
if (aRoute.IsValid())
|
|
{
|
|
aDraw.SetColor(0,1,1); //teal?
|
|
aDraw.SetLineSize(2);
|
|
aDraw.SetLineStyle("solid");
|
|
aDraw.BeginPolyline();
|
|
for (int i=0; i<aRoute.Size(); i=i+1)
|
|
{
|
|
aDraw.Vertex(aRoute.Waypoint(i).Location());
|
|
}
|
|
aDraw.End();
|
|
|
|
aDraw.SetColor(1.0,0.3,0.3); //pink?
|
|
aDraw.SetPointSize(4);
|
|
aDraw.BeginPoints();
|
|
for (int i=0; i<aRoute.Size(); i=i+1)
|
|
{
|
|
aDraw.Vertex(aRoute.Waypoint(i).Location());
|
|
}
|
|
aDraw.End();
|
|
}
|
|
end_script
|
|
|
|
|
|
behavior planned_route
|
|
|
|
script_debug_writes off
|
|
|
|
script_variables
|
|
bool mDrawRoute = false;
|
|
WsfDraw mDraw = WsfDraw();
|
|
double cDEFAULT_SPEED = 450.0 * MATH.MPS_PER_NMPH();
|
|
double cDEFAULT_ACCEL = 7.5 * Earth.ACCEL_OF_GRAVITY(); // 7.5 G (m/s^2)
|
|
end_script_variables
|
|
|
|
precondition
|
|
writeln_d("precondition planned_route");
|
|
return true;
|
|
end_precondition
|
|
|
|
on_new_execute
|
|
PLATFORM.Comment("route");
|
|
end_on_new_execute
|
|
|
|
execute
|
|
writeln_d(PLATFORM.Name(), " executing planned_route, T=", TIME_NOW);
|
|
#only command the platform to do something different if its not currently flying a route
|
|
WsfMover aMover = PLATFORM.Mover();
|
|
if (aMover.IsValid()) {
|
|
if (aMover.IsExtrapolating()) {
|
|
WsfGeoPoint pt = PLATFORM.Location();
|
|
WsfRoute ro = aMover.DefaultRoute().Copy(); #now we have a modifiable route
|
|
if (!ro.IsValid())
|
|
return;
|
|
|
|
writeln_d("flying route, name: ", ro.Name(), ", type: ", ro.Type());
|
|
WsfGeoPoint close = ro.LocationAtDistance(ro.DistanceAlongRoute(pt));
|
|
if (!close.IsValid()) {
|
|
return;
|
|
}
|
|
close.SetAltitudeAGL(pt.Altitude());
|
|
if (mDrawRoute)
|
|
{
|
|
mDraw.BeginLines();
|
|
mDraw.Vertex(pt);
|
|
mDraw.Vertex(close);
|
|
mDraw.End();
|
|
}
|
|
double d1 = ro.DistanceFromRoute(pt);
|
|
double d2 = pt.GroundRangeTo(close);
|
|
double d3 = -1;
|
|
Array<double> turnRad = aMover.PropertyDouble("turn_radius");
|
|
if (turnRad.Size() > 0) {
|
|
d3 = 2*turnRad[0];
|
|
}
|
|
int i = 0;
|
|
for (; i < ro.Size(); i = i+1)
|
|
{
|
|
WsfWaypoint wpt = ro.Waypoint(i);
|
|
WsfGeoPoint rpt = wpt.Location();
|
|
//check if we are close to an existing waypoint, if so... break & fly at that one
|
|
if (rpt.GroundRangeTo(close) < 926) {
|
|
break;
|
|
}
|
|
double dist = ro.DistanceAlongRoute(rpt);
|
|
if (dist > d1) {
|
|
if (d2 > d3) {
|
|
ro.Insert(i, WsfWaypoint.Create(close, wpt.Speed()));
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
if (i >= ro.Size()) {
|
|
i = ro.Size() - 1;
|
|
}
|
|
//go at default speed; this gets overwritten if route waypoint has defined a speed
|
|
PLATFORM.GoToSpeed(cDEFAULT_SPEED, cDEFAULT_ACCEL, true);
|
|
PLATFORM.FollowRoute(ro, i);
|
|
}
|
|
}
|
|
|
|
if (mDrawRoute)
|
|
{
|
|
mDraw.SetDuration(PROCESSOR.UpdateInterval());
|
|
mDraw.SetLayer("behavior_planned_route");
|
|
DrawRoute(mDraw, PLATFORM.Route());
|
|
}
|
|
end_execute
|
|
|
|
end_behavior
|