243 lines
9.4 KiB
Plaintext
243 lines
9.4 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.
|
|
# ****************************************************************************
|
|
|
|
// functions to fly in formation
|
|
|
|
script_variables
|
|
// the proximity in which I think I've achieved the waypoint (nm)
|
|
double cWAYPOINT_PROXIMITY = 0.2 * MATH.M_PER_NM();
|
|
// the range that I think I'm far from my waypoint and need to fly a "max" speed to catch up
|
|
double cFAR_FROM_WAYPOINT = 5.0 * MATH.M_PER_NM();
|
|
// the slowest I will fly to get back into formation
|
|
double cMIN_MACH = 0.6;
|
|
// the fastest I will fly to get back into formation
|
|
double cMAX_MACH = 1.5;
|
|
// once I'm in formation, the range that I think I'm out again
|
|
double cOUT_OF_FORMATION = 2.0 * MATH.M_PER_NM();
|
|
|
|
bool mInFormation = false;
|
|
end_script_variables
|
|
|
|
script void formation_initialize(double aWaypointProximity,
|
|
double aFarFromWaypoint,
|
|
double aMinMach,
|
|
double aMaxMach,
|
|
double aOutOfFormation)
|
|
// set the constants base on the inputs
|
|
// allows users to parameterize their own formation
|
|
cWAYPOINT_PROXIMITY = aWaypointProximity;
|
|
cFAR_FROM_WAYPOINT = aFarFromWaypoint;
|
|
cMIN_MACH = aMinMach;
|
|
cMAX_MACH = aMaxMach;
|
|
cOUT_OF_FORMATION = aOutOfFormation;
|
|
end_script
|
|
|
|
script void waypoint_behind_logic(WsfWaypoint aWaypoint,
|
|
WsfPlatform aLeadPlatform,
|
|
WsfPlatform aExternalPlatform)
|
|
// what's the minimum I want to fly to let the waypoint "catch up"?
|
|
// a fraction of my lead's mach
|
|
double machToFly = aExternalPlatform.MachNumber();
|
|
double rangeToWaypoint = PLATFORM.SlantRangeTo(aWaypoint);
|
|
double relBearing = PLATFORM.RelativeBearingTo(aWaypoint.Latitude(), aWaypoint.Longitude(), aWaypoint.Altitude());
|
|
double absBearing = PLATFORM.TrueBearingTo(aWaypoint.Latitude(), aWaypoint.Longitude(), aWaypoint.Altitude());
|
|
|
|
writeln_d(" wbl, mach: ", machToFly, ", range: ", rangeToWaypoint, ", relBearing: ", relBearing);
|
|
|
|
// if I've reached my waypoint, just fly my lead's heading
|
|
if (rangeToWaypoint <= cWAYPOINT_PROXIMITY)
|
|
{
|
|
writeln_d(" in formation!");
|
|
mInFormation = true;
|
|
}
|
|
// if I'm far from my waypoint, fly a scale of my lead's speed
|
|
else if (rangeToWaypoint > cFAR_FROM_WAYPOINT)
|
|
{
|
|
writeln_d(" far from formation!");
|
|
// fly a fraction of my lead's speed and let the waypoint catch up
|
|
// machToFly = cMIN_MACH;
|
|
|
|
machToFly = cMAX_MACH;
|
|
PLATFORM.GoToMachNumber(machToFly);
|
|
// PLATFORM.GoToLocation(aWaypoint);
|
|
PLATFORM.TurnToHeading(absBearing);
|
|
PLATFORM.GoToAltitude(aExternalPlatform.Altitude());
|
|
}
|
|
// if I haven't reached my waypoint yet
|
|
else if (rangeToWaypoint > cWAYPOINT_PROXIMITY)
|
|
{
|
|
writeln_d(" close, but not in formation!");
|
|
// some speed in the middle representative of the range
|
|
// "slow down as the waypoint approaches me"
|
|
double multiplier = MATH.Sqrt(
|
|
(rangeToWaypoint - cWAYPOINT_PROXIMITY) /
|
|
(cFAR_FROM_WAYPOINT - cWAYPOINT_PROXIMITY));
|
|
multiplier = MATH.Sqrt(multiplier);
|
|
machToFly = machToFly - ((machToFly - cMIN_MACH) * multiplier);
|
|
|
|
// fly in front of the waypoint so when it reaches me, i'm flying on the same line with it
|
|
double angle = 0.0;
|
|
if (relBearing <= 0.0)
|
|
{
|
|
angle = 90.0;
|
|
}
|
|
else
|
|
{
|
|
angle = -90.0;
|
|
}
|
|
writeln_d(" angle == ", angle);
|
|
double newAngle = angle * (relBearing + 180.0) / 360.0;
|
|
writeln_d(" newAngle == ", newAngle);
|
|
if (newAngle > 45.0)
|
|
{
|
|
newAngle = 45.0;
|
|
}
|
|
|
|
if (newAngle < -45.0)
|
|
{
|
|
newAngle = -45.0;
|
|
}
|
|
writeln_d(" newAngle == ", newAngle);
|
|
|
|
double heading = aExternalPlatform.Heading() + newAngle;
|
|
writeln_d(" new heading == ", heading);
|
|
PLATFORM.TurnToHeading(heading);
|
|
PLATFORM.GoToMachNumber(machToFly);
|
|
PLATFORM.GoToAltitude(aExternalPlatform.Altitude());
|
|
}
|
|
end_script
|
|
|
|
script void waypoint_in_front_logic(WsfWaypoint aWaypoint,
|
|
WsfPlatform aLeadPlatform,
|
|
WsfPlatform aExternalPlatform)
|
|
// what's the maximum I want to fly to catch up to the waypont?
|
|
// a scale of my lead's mach
|
|
double machToFly = aExternalPlatform.MachNumber();
|
|
double rangeToWaypoint = PLATFORM.SlantRangeTo(aWaypoint);
|
|
double absBearing = PLATFORM.TrueBearingTo(aWaypoint.Latitude(), aWaypoint.Longitude(), aWaypoint.Altitude());
|
|
|
|
writeln_d(" wifl, mach: ", machToFly, ", range: ", rangeToWaypoint);
|
|
|
|
// if I've reached my waypoint, just fly my lead's heading
|
|
if (rangeToWaypoint <= cWAYPOINT_PROXIMITY)
|
|
{
|
|
writeln_d(" in formation!");
|
|
mInFormation = true;
|
|
PLATFORM.TurnToHeading(aExternalPlatform.Heading());
|
|
PLATFORM.GoToMachNumber(machToFly);
|
|
PLATFORM.GoToAltitude(aExternalPlatform.Altitude());
|
|
}
|
|
// if I'm really far from my waypoint, fly a scale of my lead's speed
|
|
else if (rangeToWaypoint > cFAR_FROM_WAYPOINT)
|
|
{
|
|
writeln_d(" far from formation!");
|
|
// fly a multiple of my lead's speed and catch up to the waypoint
|
|
// using lead pursuit
|
|
machToFly = cMAX_MACH;
|
|
PLATFORM.GoToMachNumber(machToFly);
|
|
|
|
// double lat = aWaypoint.Latitude();
|
|
// double lon = aWaypoint.Longitude();
|
|
// XXX ???? mSoarAirAgent.PushWaypointToLead(aWaypoint, aExternalPlatform.TruthId());
|
|
|
|
// PLATFORM.GoToLocation(aWaypoint);
|
|
PLATFORM.TurnToHeading(absBearing);
|
|
PLATFORM.GoToAltitude(aExternalPlatform.Altitude());
|
|
}
|
|
// if i haven't reached my waypoint yet
|
|
else if (rangeToWaypoint > cWAYPOINT_PROXIMITY)
|
|
{
|
|
writeln_d(" close but not in formation!");
|
|
// some speed in the middle representative of the range
|
|
// "slow down as I approach the waypoint"
|
|
double multiplier = MATH.Sqrt(
|
|
(rangeToWaypoint - (cWAYPOINT_PROXIMITY)) /
|
|
(cFAR_FROM_WAYPOINT - (cWAYPOINT_PROXIMITY)));
|
|
multiplier = MATH.Sqrt(multiplier);
|
|
machToFly = machToFly + ((cMAX_MACH - machToFly) * multiplier);
|
|
|
|
PLATFORM.GoToMachNumber(machToFly);
|
|
|
|
// double lat = aWaypoint.Latitude();
|
|
// double lon = aWaypoint.Longitude();
|
|
// XXX ???? mSoarAirAgent.PushWaypointToLead(aWaypoint, aExternalPlatform.TruthId());
|
|
|
|
PLATFORM.TurnToHeading(absBearing);
|
|
// PLATFORM.GoToLocation(aWaypoint);
|
|
}
|
|
end_script
|
|
|
|
script void formation_logic(WsfWaypoint aWaypoint,
|
|
WsfPlatform aLeadPlatform,
|
|
WsfPlatform aExternalPlatform,
|
|
bool aTightFormation)
|
|
// is waypoint behind or in front of me?
|
|
double relBearing = PLATFORM.RelativeBearingTo(aWaypoint.Latitude(), aWaypoint.Longitude(), aWaypoint.Altitude());
|
|
double waypointRange = PLATFORM.SlantRangeTo(aWaypoint);
|
|
|
|
writeln_d(" relBearing: ", relBearing, ", waypointRange: ", waypointRange);
|
|
// if waypoint is behind me and I'm somewhat close to it, just let it catch up to me
|
|
if (MATH.Fabs(relBearing) > 90 && waypointRange < 20.0 * MATH.M_PER_NM())
|
|
{
|
|
writeln_d(" waypoint behind, relBearing:", relBearing);
|
|
waypoint_behind_logic(aWaypoint, aLeadPlatform, aExternalPlatform);
|
|
}
|
|
else // waypoint is in front of me
|
|
{
|
|
writeln_d(" waypoint in front, relBearing: ", relBearing);
|
|
// if I'm in formation, don't get out of it easily
|
|
if ((aTightFormation == false) && (mInFormation == true))
|
|
{
|
|
// check if I'm still in formation
|
|
if (waypointRange >= cOUT_OF_FORMATION)
|
|
{
|
|
mInFormation = false;
|
|
}
|
|
else
|
|
{
|
|
// fly my lead's heading
|
|
PLATFORM.TurnToHeading(aExternalPlatform.Heading());
|
|
PLATFORM.GoToMachNumber(aExternalPlatform.MachNumber());
|
|
PLATFORM.GoToAltitude(aExternalPlatform.Altitude());
|
|
return;
|
|
}
|
|
}
|
|
|
|
waypoint_in_front_logic(aWaypoint, aLeadPlatform, aExternalPlatform);
|
|
}
|
|
end_script
|
|
|
|
script void formation_update(WsfPlatform aLeadPlatform)
|
|
WsfWaypoint formationPoint = WsfWaypoint();
|
|
WsfWaypoint leadPoint = WsfWaypoint();
|
|
int followingIndex = GetFollowingFormationIndex();
|
|
WsfPlatform following = mFlightLeadAgent.GetPlatformInFormation(followingIndex);
|
|
int flightLeadIndex = mFlightLeadAgent.GetFormationIndex(mFlightLeadAgent.GetPlatformInFormation(-1));
|
|
|
|
if (followingIndex == flightLeadIndex)
|
|
{
|
|
// get the formation point for my position
|
|
if ((mFlightLeadAgent.GetFormationPosition(PLATFORM, formationPoint)) == false)
|
|
{
|
|
return;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
PROCESSOR.FollowPlatform(following, 110.0, 1.5 * MATH.M_PER_NM(), formationPoint);
|
|
}
|
|
writeln_d(" following: ", followingIndex, ", flightLeadIndex: ", flightLeadIndex);
|
|
writeln_d(" me: (", PLATFORM.Latitude(), ", ", PLATFORM.Longitude(), ", ", PLATFORM.Altitude(), ")");
|
|
writeln_d(" to: (", formationPoint.Latitude(), ", ", formationPoint.Longitude(), ", ", formationPoint.Altitude(), ")");
|
|
// fly formation logic
|
|
formation_logic(formationPoint, aLeadPlatform, following, false);
|
|
end_script
|
|
|
|
|