mirror of
https://github.com/FlightControl-Master/MOOSE.git
synced 2025-10-29 16:58:06 +00:00
ARMYGROUP
This commit is contained in:
Frank
parent
7f98e703f1
commit
ce10390406
@ -140,6 +140,18 @@ function ARMYGROUP:GetClosestRoad()
|
||||
return self:GetCoordinate():GetClosestPointToRoad()
|
||||
end
|
||||
|
||||
--- Get 2D distance to the closest road.
|
||||
-- @param #ARMYGROUP self
|
||||
-- @return #number Distance in meters to the closest road.
|
||||
function ARMYGROUP:GetClosestRoadDist()
|
||||
local road=self:GetClosestRoad()
|
||||
if road then
|
||||
local dist=road:Get2DDistance(self:GetCoordinate())
|
||||
return dist
|
||||
end
|
||||
return math.huge
|
||||
end
|
||||
|
||||
|
||||
--- Add a *scheduled* task to fire at a given coordinate.
|
||||
-- @param #ARMYGROUP self
|
||||
@ -372,80 +384,75 @@ function ARMYGROUP:onafterUpdateRoute(From, Event, To, n, Speed, Formation)
|
||||
-- Waypoints.
|
||||
local waypoints={}
|
||||
|
||||
-- Total number of waypoints
|
||||
local N=#self.waypoints
|
||||
|
||||
-- Add remaining waypoints to route.
|
||||
for i=n, N do
|
||||
-- Next waypoint.
|
||||
local wp=UTILS.DeepCopy(self.waypoints[n]) --Ops.OpsGroup#OPSGROUP.Waypoint
|
||||
|
||||
-- Copy waypoint.
|
||||
local wp=UTILS.DeepCopy(self.waypoints[i]) --Ops.OpsGroup#OPSGROUP.Waypoint
|
||||
|
||||
if i==n then
|
||||
|
||||
---
|
||||
-- Next Waypoint
|
||||
---
|
||||
|
||||
if Speed then
|
||||
wp.speed=UTILS.KnotsToMps(Speed)
|
||||
else
|
||||
-- Take default waypoint speed.
|
||||
end
|
||||
|
||||
if self.formationPerma then
|
||||
--if self.formationPerma==ENUMS.Formation.Vehicle.OnRoad then
|
||||
wp.action=self.formationPerma
|
||||
--end
|
||||
elseif Formation then
|
||||
wp.action=Formation
|
||||
end
|
||||
|
||||
-- Current set formation.
|
||||
self.option.Formation=wp.action
|
||||
|
||||
-- Current set speed in m/s.
|
||||
self.speedWp=wp.speed
|
||||
|
||||
else
|
||||
-- Do we want to drive on road to the next wp?
|
||||
local onroad=wp.action==ENUMS.Formation.Vehicle.OnRoad
|
||||
|
||||
---
|
||||
-- Later Waypoint(s)
|
||||
---
|
||||
|
||||
if self.formationPerma then
|
||||
wp.action=self.formationPerma
|
||||
else
|
||||
-- Take default waypoint speed.
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
if wp.roaddist>100 and wp.action==ENUMS.Formation.Vehicle.OnRoad then
|
||||
|
||||
-- Waypoint is actually off road!
|
||||
wp.action=ENUMS.Formation.Vehicle.OffRoad
|
||||
-- Current point.
|
||||
local current=self:GetCoordinate():WaypointGround(UTILS.MpsToKmph(self.speedWp), ENUMS.Formation.Vehicle.OffRoad)
|
||||
table.insert(waypoints, current)
|
||||
|
||||
-- Insert a point on road.
|
||||
if onroad then
|
||||
local current=self:GetClosestRoad():WaypointGround(UTILS.MpsToKmph(self.speedWp), ENUMS.Formation.Vehicle.OnRoad)
|
||||
table.insert(waypoints, current)
|
||||
end
|
||||
|
||||
-- Add "On Road" waypoint in between.
|
||||
local wproad=wp.roadcoord:WaypointGround(wp.speed, ENUMS.Formation.Vehicle.OnRoad)
|
||||
table.insert(waypoints, wproad)
|
||||
end
|
||||
|
||||
-- Debug info.
|
||||
self:T(string.format("WP %d %s: Speed=%d m/s, alt=%d m, Action=%s", i, wp.type, wp.speed, wp.alt, wp.action))
|
||||
|
||||
-- Add waypoint.
|
||||
table.insert(waypoints, wp)
|
||||
-- Speed.
|
||||
if Speed then
|
||||
wp.speed=UTILS.KnotsToMps(Speed)
|
||||
else
|
||||
-- Take default waypoint speed.
|
||||
end
|
||||
|
||||
-- Formation.
|
||||
if self.formationPerma then
|
||||
wp.action=self.formationPerma
|
||||
elseif Formation then
|
||||
wp.action=Formation
|
||||
end
|
||||
|
||||
-- Current set formation.
|
||||
self.option.Formation=wp.action
|
||||
|
||||
-- Current set speed in m/s.
|
||||
self.speedWp=wp.speed
|
||||
|
||||
-- Add waypoint in between because this waypoint is "On Road" but lies "Off Road".
|
||||
if onroad then
|
||||
|
||||
-- The real waypoint is actually off road.
|
||||
wp.action=ENUMS.Formation.Vehicle.OffRoad
|
||||
|
||||
-- Add "On Road" waypoint in between.
|
||||
local wproad=wp.roadcoord:WaypointGround(wp.speed, ENUMS.Formation.Vehicle.OnRoad) --Ops.OpsGroup#OPSGROUP.Waypoint
|
||||
|
||||
-- Insert road waypoint.
|
||||
table.insert(waypoints, wproad)
|
||||
end
|
||||
|
||||
-- Add waypoint.
|
||||
table.insert(waypoints, wp)
|
||||
|
||||
-- Debug output.
|
||||
if false then
|
||||
for i,_wp in pairs(waypoints) do
|
||||
local wp=_wp
|
||||
local text=string.format("WP #%d UID=%d type=%s: Speed=%d m/s, alt=%d m, Action=%s", i, wp.uid and wp.uid or 0, wp.type, wp.speed, wp.alt, wp.action)
|
||||
self:I(text)
|
||||
if wp.coordinate then
|
||||
wp.coordinate:MarkToAll(text)
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
-- Current waypoint.
|
||||
local current=self:GetCoordinate():WaypointGround(UTILS.MpsToKmph(self.speedWp), self.option.Formation)
|
||||
table.insert(waypoints, 1, current)
|
||||
table.insert(waypoints, 1, current) -- Seems to be better to add this twice. Otherwise, the passing waypoint functions is triggered to early!
|
||||
|
||||
if #waypoints>2 then
|
||||
if #waypoints>=2 then
|
||||
|
||||
-- Debug info.
|
||||
self:T(self.lid..string.format("Updateing route: WP %d-->%d-->%d (#%d), Speed=%.1f knots, Formation=%s",
|
||||
self.currentwp, n, #self.waypoints, #waypoints-2, UTILS.MpsToKnots(self.speedWp), tostring(self.option.Formation)))
|
||||
|
||||
@ -646,7 +653,7 @@ end
|
||||
|
||||
--- Add an a waypoint to the route.
|
||||
-- @param #ARMYGROUP self
|
||||
-- @param Core.Point#COORDINATE Coordinate The coordinate of the waypoint. Use COORDINATE:SetAltitude(altitude) to define the altitude.
|
||||
-- @param Core.Point#COORDINATE Coordinate The coordinate of the waypoint.
|
||||
-- @param #number Speed Speed in knots. Default is default cruise speed or 70% of max speed.
|
||||
-- @param #number AfterWaypointWithID Insert waypoint after waypoint given ID. Default is to insert as last waypoint.
|
||||
-- @param #number Formation Formation the group will use.
|
||||
@ -683,7 +690,7 @@ function ARMYGROUP:AddWaypoint(Coordinate, Speed, AfterWaypointWithID, Formation
|
||||
end
|
||||
|
||||
-- Debug info.
|
||||
self:T(self.lid..string.format("Adding waypoint UID=%d (index=%d), Speed=%.1f knots, Dist2Road=%d m, Action=%s", waypoint.uid, wpnumber, Speed, waypoint.roaddist, waypoint.action))
|
||||
self:I(self.lid..string.format("Adding waypoint UID=%d (index=%d), Speed=%.1f knots, Dist2Road=%d m, Action=%s", waypoint.uid, wpnumber, Speed, waypoint.roaddist, waypoint.action))
|
||||
|
||||
-- Update route.
|
||||
if Updateroute==nil or Updateroute==true then
|
||||
|
||||
@ -1133,18 +1133,21 @@ end
|
||||
--- Get next waypoint index.
|
||||
-- @param #OPSGROUP self
|
||||
-- @param #boolean cyclic If true, return first waypoint if last waypoint was reached. Default is patrol ad infinitum value set.
|
||||
-- @param #number i Waypoint index from which the next index is returned. Default is the last waypoint passed.
|
||||
-- @return #number Next waypoint index.
|
||||
function OPSGROUP:GetWaypointIndexNext(cyclic)
|
||||
function OPSGROUP:GetWaypointIndexNext(cyclic, i)
|
||||
|
||||
if cyclic==nil then
|
||||
cyclic=self.adinfinitum
|
||||
end
|
||||
|
||||
local N=#self.waypoints
|
||||
|
||||
local n=math.min(self.currentwp+1, N)
|
||||
|
||||
if cyclic and self.currentwp==N then
|
||||
i=i or self.currentwp
|
||||
|
||||
local n=math.min(i+1, N)
|
||||
|
||||
if cyclic and i==N then
|
||||
n=1
|
||||
end
|
||||
|
||||
@ -2677,18 +2680,18 @@ function OPSGROUP:onafterPassingWaypoint(From, Event, To, Waypoint)
|
||||
-- Set switch to true.
|
||||
self.passedfinalwp=true
|
||||
|
||||
-- Check if all tasks/mission are done? If so, RTB or WAIT.
|
||||
-- Note, we delay it for a second to let the OnAfterPassingwaypoint function to be executed in case someone wants to add another waypoint there.
|
||||
if ntasks==0 then
|
||||
self:_CheckGroupDone(1)
|
||||
end
|
||||
end
|
||||
|
||||
-- Check if all tasks/mission are done? If so, RTB or WAIT.
|
||||
-- Note, we delay it for a second to let the OnAfterPassingwaypoint function to be executed in case someone wants to add another waypoint there.
|
||||
if ntasks==0 then
|
||||
self:_CheckGroupDone()
|
||||
end
|
||||
|
||||
-- Debug info.
|
||||
local text=string.format("Group passed waypoint %s/%d ID=%d: final=%s detour=%s astar=%s",
|
||||
tostring(wpindex), #self.waypoints, Waypoint.uid, tostring(self.passedfinalwp), tostring(Waypoint.detour), tostring(Waypoint.astar))
|
||||
self:T(self.lid..text)
|
||||
self:I(self.lid..text)
|
||||
|
||||
end
|
||||
|
||||
@ -3058,92 +3061,60 @@ function OPSGROUP:_CheckGroupDone(delay)
|
||||
self:ScheduleOnce(delay, self._CheckGroupDone, self)
|
||||
else
|
||||
|
||||
if self.passedfinalwp then
|
||||
|
||||
---
|
||||
-- Passed FINAL waypoint
|
||||
---
|
||||
|
||||
if #self.waypoints>1 then
|
||||
|
||||
if self.adinfinitum then
|
||||
|
||||
-- Get positive speed to first waypoint.
|
||||
local speed=self:GetSpeedToWaypoint(1)
|
||||
|
||||
-- Start route at first waypoint.
|
||||
self:__UpdateRoute(-1, 1, speed)
|
||||
|
||||
self:T(self.lid..string.format("Passed final WP, #WP>1, adinfinitum=TRUE ==> Goto WP 1 at speed>0"))
|
||||
|
||||
self.passedfinalwp=false
|
||||
|
||||
else
|
||||
-- No further waypoints. Command a full stop.
|
||||
self:__FullStop(-1)
|
||||
|
||||
self:T(self.lid..string.format("Passed final WP, #WP>1, adinfinitum=FALSE ==> Full Stop"))
|
||||
end
|
||||
|
||||
elseif #self.waypoints==1 then
|
||||
|
||||
--- Only one WP left
|
||||
|
||||
-- The last waypoint.
|
||||
local waypoint=self.waypoints[1] --Ops.OpsGroup#OPSGROUP.Waypoint
|
||||
|
||||
local dist=self:GetCoordinate():Get2DDistance(waypoint.coordinate)
|
||||
|
||||
|
||||
if self.adinfinitum and dist>1000 then -- Note that dist>100 caused the same wp to be passed a lot of times.
|
||||
|
||||
self:T(self.lid..string.format("Passed final WP, #WP=1, adinfinitum=TRUE dist>1000 ==> Goto WP 1 at speed>0"))
|
||||
|
||||
-- Get positive speed to first waypoint.
|
||||
local speed=self:GetSpeedToWaypoint(1)
|
||||
|
||||
-- Start route at first waypoint.
|
||||
self:__UpdateRoute(-1, 1, speed)
|
||||
|
||||
self.passedfinalwp=false
|
||||
|
||||
else
|
||||
|
||||
self:T(self.lid..string.format("Passed final WP, #WP=1, adinfinitum=FALSE or dist<1000 ==> Full Stop"))
|
||||
|
||||
self:__FullStop(-1)
|
||||
|
||||
end
|
||||
|
||||
else
|
||||
|
||||
--- No waypoints left
|
||||
|
||||
-- No further waypoints. Command a full stop.
|
||||
self:T(self.lid..string.format("No waypoints left ==> Full Stop"))
|
||||
|
||||
self:__FullStop(-1)
|
||||
|
||||
end
|
||||
|
||||
if self.adinfinitum then
|
||||
|
||||
---
|
||||
-- Parol Ad Infinitum
|
||||
---
|
||||
|
||||
-- Next waypoint index.
|
||||
local i=self:GetWaypointIndexNext(true)
|
||||
|
||||
-- Get positive speed to first waypoint.
|
||||
local speed=self:GetSpeedToWaypoint(i)
|
||||
|
||||
-- Start route at first waypoint.
|
||||
self:UpdateRoute(i, speed)
|
||||
|
||||
self:T(self.lid..string.format("Adinfinitum=TRUE ==> Goto WP index=%d at speed=%d knots", i, speed))
|
||||
|
||||
else
|
||||
|
||||
---
|
||||
-- Final waypoint NOT passed yet
|
||||
-- Finite Patrol
|
||||
---
|
||||
|
||||
if #self.waypoints>0 then
|
||||
self:T(self.lid..string.format("NOT Passed final WP, #WP>0 ==> Update Route"))
|
||||
self:__UpdateRoute(-1)
|
||||
else
|
||||
self:E(self.lid..string.format("WARNING: No waypoints left! Commanding a Full Stop"))
|
||||
|
||||
if self.passedfinalwp then
|
||||
|
||||
---
|
||||
-- Passed FINAL waypoint
|
||||
---
|
||||
|
||||
-- No further waypoints. Command a full stop.
|
||||
self:__FullStop(-1)
|
||||
|
||||
self:T(self.lid..string.format("Passed final WP, #WP>1, adinfinitum=FALSE ==> Full Stop"))
|
||||
|
||||
else
|
||||
|
||||
---
|
||||
-- Final waypoint NOT passed yet
|
||||
---
|
||||
|
||||
if #self.waypoints>0 then
|
||||
self:T(self.lid..string.format("NOT Passed final WP, #WP>0 ==> Update Route"))
|
||||
self:UpdateRoute()
|
||||
else
|
||||
self:E(self.lid..string.format("WARNING: No waypoints left! Commanding a Full Stop"))
|
||||
self:__FullStop(-1)
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
@ -3368,7 +3339,7 @@ function OPSGROUP:_UpdateWaypointTasks(n)
|
||||
if i>=n or nwaypoints==1 then
|
||||
|
||||
-- Debug info.
|
||||
self:T(self.lid..string.format("Updating waypoint task for waypoint %d/%d ID=%d. Last waypoint passed %d", i, nwaypoints, wp.uid, self.currentwp))
|
||||
self:T2(self.lid..string.format("Updating waypoint task for waypoint %d/%d ID=%d. Last waypoint passed %d", i, nwaypoints, wp.uid, self.currentwp))
|
||||
|
||||
-- Tasks of this waypoint
|
||||
local taskswp={}
|
||||
@ -3401,12 +3372,12 @@ function OPSGROUP._PassingWaypoint(group, opsgroup, uid)
|
||||
|
||||
if waypoint then
|
||||
|
||||
-- Current wp.
|
||||
local currentwp=opsgroup.currentwp
|
||||
|
||||
-- Get the current waypoint index.
|
||||
opsgroup.currentwp=opsgroup:GetWaypointIndex(uid)
|
||||
|
||||
-- Increase passing counter.
|
||||
waypoint.npassed=waypoint.npassed+1
|
||||
|
||||
|
||||
-- Set expected speed and formation from the next WP.
|
||||
local wpnext=opsgroup:GetWaypointNext()
|
||||
if wpnext then
|
||||
@ -3450,13 +3421,18 @@ function OPSGROUP._PassingWaypoint(group, opsgroup, uid)
|
||||
|
||||
-- Debug message.
|
||||
local text=string.format("Group passing waypoint uid=%d", uid)
|
||||
opsgroup:T2(opsgroup.lid..text)
|
||||
|
||||
opsgroup:T(opsgroup.lid..text)
|
||||
|
||||
-- Trigger PassingWaypoint event.
|
||||
if not (waypoint.astar or waypoint.detour) then
|
||||
|
||||
-- Increase passing counter.
|
||||
waypoint.npassed=waypoint.npassed+1
|
||||
|
||||
-- Call event function.
|
||||
opsgroup:PassingWaypoint(waypoint)
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user