eager/lab1
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

init

Browse Source
This commit is contained in:
eager
2025-09-12 15:20:28 +08:00
commit 3257a14c32
449 changed files with 388780 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

242
processors/ripr_agents/aiai/old.formations.old/formation.script Normal file
View File

@@ -0,0 +1,242 @@
# ****************************************************************************
# 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