def compute_roll_from_course(self):
pos = self._sensors.Position()
pos = util.AddPosition(
pos,
self._sensors.GroundSpeed() * self.CourseProjectionSeconds / 3600,
self._sensors.GroundTrack())
turn_radius = (360.0 * (self.CurrentAirSpeed / 60.0) /
self.TurnRate) / (4 * util.M_PI)
turn_radius *= self.InterceptMultiplier
intercept_heading = util.CourseHeading(pos, self.DesiredCourse,
turn_radius)
roll, _ = self.compute_roll(intercept_heading)
return roll
def ComputeRunwayEndpoints(self):
if self.RunwayAltitude == None:
self.RunwayAltitude = self._sensors.Altitude()
if len(self.ApproachEndpoints) == 0:
if self.RunwayTouchdownPoint != None:
position = self.RunwayTouchdownPoint
else:
position = self._sensors.Position()
if self.RunwayLength > 0:
end = util.AddPosition(position, self.RunwayLength,
self.RunwayHeading)
else:
end = position
self.ApproachEndpoints = [position, end]
def projected_pos(self):
pos = self._sensors.Position()
pos = util.AddPosition(pos,
self._sensors.GroundSpeed() * self.CourseProjectionSeconds / 3600,
self._sensors.GroundTrack())
return pos
def Land(self,
runway_points,
runway_alt,
outer_marker=None,
outer_altitude=0,
middle_marker=None,
right_pattern=False,
pattern_alt=0,
threshold_agl=-9999):
self._flight_control.Start(self._last_desired_pitch)
self._flight_mode = SUBMODE_APPROACH
# If given an outer and middle marker, fly to the outer marker and approach from there.
# otherwise use the pattern as appropriate
self._outer_marker = outer_marker
self._middle_marker = middle_marker
self._right_pattern = right_pattern
self.RunwayEndpoints = runway_points
self.RunwayAltitude = runway_alt
if threshold_agl != -9999:
self.ThresholdAgl = threshold_agl
flare_altitude = self.RunwayAltitude + self.ThresholdAgl
self._flight_control.SetCallback(self)
if pattern_alt > 0:
self.PatternAltitude = pattern_alt
else:
self.PatternAltitude = self.RunwayAltitude + 1000.0
if outer_altitude > 0:
self._outer_altitude = outer_altitude
else:
self._outer_altitude = self.PatternAltitude + 500.0
rel_heading, dist, heading, self.rel_lng = util.VORDME(
self._sensors.Position(), self.RunwayEndpoints)
turn_radius = (
(360.0 *
(self.FinalAirSpeed / 60.0) / self._flight_control.TurnRate) /
(4 * util.M_PI))
short_final_point = util.AddPosition(self.RunwayEndpoints[0],
turn_radius, heading - 180)
short_final_altitude = turn_radius * math.sin(
self.DescentAngle * util.RAD_DEG) * util.FEET_NM
short_final_altitude += flare_altitude
if outer_marker != None and middle_marker != None:
logger.debug(
"Making an instrument approach landing. Flying to outer marker"
)
approach_course = [outer_marker, middle_marker]
turn_radius = ((360.0 * (self.ApproachAirSpeed / 60.0) /
self._flight_control.TurnRate) / (4 * util.M_PI))
if self._sensors.Altitude() > self._outer_altitude:
altitude = self._outer_altitude
else:
altitude = 0 # Maintain current altitude
if abs(rel_heading) < 45:
logger.debug(
"Inserting to approach course from opposing quadrant")
insertion_point = util.AddPosition(outer_marker, heading + 90,
turn_radius * 2)
self._approach_directives.append(
(self._flight_control.FlyTo, (insertion_point, altitude)))
self._approach_directives.append(
(self._flight_control.TurnTo, (heading, -1)))
elif rel_heading > 0 and rel_heading < 135:
logger.debug(
"Inserting to approach course from upper quadrant")
insertion_point = util.AddPosition(outer_marker, heading + 90,
turn_radius)
self._approach_directives.append(
(self._flight_control.FlyTo, (insertion_point, altitude)))
self._approach_directives.append(
(self._flight_control.TurnTo, (heading, -1)))
elif rel_heading < 0 and rel_heading > -135:
logger.debug(
"Inserting to approach course from lower quadrant")
insertion_point = util.AddPosition(outer_marker, heading - 90,
turn_radius)
self._approach_directives.append(
(self._flight_control.FlyTo, (insertion_point, altitude)))
self._approach_directives.append(
(self._flight_control.TurnTo, (heading, 1)))
else:
logger.debug("Inserting to approach course straight in")
insertion_point = outer_marker
self._approach_directives.append(
(self._flight_control.FlyTo, (outer_marker, altitude)))
self._approach_directives.append(
(self._flight_control.FlyCourse,
(approach_course, self._outer_altitude, self.PatternAirSpeed,
False)))
self._approach_directives.append((self.DropGearAndFlaps, tuple()))
final_course = [middle_marker, short_final_point]
else:
logger.debug("Making an pattern approach landing.")
turn_radius = ((360.0 * (self.PatternAirSpeed / 60.0) /
self._flight_control.TurnRate) / (4 * util.M_PI))
if right_pattern:
pattern_offset_heading = heading + 90
else:
pattern_offset_heading = heading - 90
downwind_point = util.AddPosition(self.RunwayEndpoints[1],
turn_radius * 2.2,
pattern_offset_heading)
descent_distance = turn_radius * 2.2
base_distance = turn_radius * 2.2
total_approach_distance = descent_distance * 2 + base_distance
descent_point = util.AddPosition(self.RunwayEndpoints[0],
base_distance,
pattern_offset_heading)
base_point = util.AddPosition(descent_point, base_distance,
heading - 180)
outer_base_point = util.AddPosition(base_point, turn_radius * 4.2,
pattern_offset_heading)
final_point = util.AddPosition(self.RunwayEndpoints[0],
turn_radius * 2.2, heading - 180)
pattern_alt_nm = total_approach_distance * math.sin(
self.DescentAngle * util.RAD_DEG)
self.PatternAltitude = pattern_alt_nm * util.FEET_NM + flare_altitude
final_turn_altitude = descent_distance * math.sin(
self.DescentAngle * util.RAD_DEG)
final_turn_altitude *= util.FEET_NM
base_turn_altitude = self.PatternAltitude - final_turn_altitude
final_turn_altitude += flare_altitude
logger.debug("Pattern altitude %g feet", self.PatternAltitude)
outer_final_point = util.AddPosition(final_point,
turn_radius * 4.2,
heading - 180)
if self._sensors.Altitude() > self._outer_altitude:
altitude = self._outer_altitude
else:
altitude = self.PatternAltitude
include_downwind = False
include_base = False
if abs(rel_heading) > 135:
logger.debug("Making straight in final")
insertion_point = outer_marker
self._approach_directives.append(
(self._flight_control.FlyTo, (outer_final_point,
self.PatternAltitude)))
self._approach_directives.append(
(self.DropGearAndFlaps, tuple()))
elif (((not right_pattern) and rel_heading >= 10)
or (right_pattern and rel_heading <= -10)):
logger.debug("Inserting into pattern via crosswind")
insertion_point = self.RunwayEndpoints[1]
crosswind_course = [self.RunwayEndpoints[1], downwind_point]
self._approach_directives.append(
(self._flight_control.FlyCourse,
(crosswind_course, altitude, self.ApproachAirSpeed,
True)))
include_downwind = True
include_base = True
elif (((not right_pattern) and rel_heading < 10
and rel_heading > -60) or
(right_pattern) and rel_heading > -10 and rel_heading < 60):
logger.debug("Inserting into downwind pattern")
include_downwind = True
include_base = True
else:
if right_pattern:
logger.debug("Inserting into pattern via right base")
assert (rel_heading >= 0)
else:
logger.debug("Inserting into pattern via left base")
assert (rel_heading <= 0)
include_base = True
extended_base = [outer_base_point, base_point]
self._approach_directives.append(
(self._flight_control.FlyTo, (outer_base_point, altitude,
self.ApproachAirSpeed)))
self._approach_directives.append(
(self.DropGearAndFlaps, tuple()))
descent_airspeed = self.PatternAirSpeed * .7 + self.FinalAirSpeed * .3
self._approach_directives.append(
(self._flight_control.DescentCourse,
(extended_base, base_turn_altitude, descent_airspeed,
False)))
include_base = True
if include_downwind:
downwind_course = [downwind_point, descent_point]
self._approach_directives.append(
(self._flight_control.FlyCourse,
(downwind_course, self.PatternAltitude,
self.PatternAirSpeed, False)))
self._approach_directives.append(
(self.DropGearAndFlaps, tuple()))
descent_downwind = [downwind_point, base_point]
descent_airspeed = self.PatternAirSpeed * .7 + self.FinalAirSpeed * .3
self._approach_directives.append(
(self._flight_control.DescentCourse,
(descent_downwind, base_turn_altitude, descent_airspeed,
True)))
if include_base:
base_course = [base_point, final_point]
descent_airspeed = self.PatternAirSpeed * .5 + self.FinalAirSpeed * .5
self._approach_directives.append(
(self._flight_control.DescentCourse,
(base_course, final_turn_altitude, descent_airspeed,
True)))
final_course = [final_point, short_final_point]
self._approach_directives.append(
(self._flight_control.DescentCourse,
(final_course, short_final_altitude, self.FinalAirSpeed, False)))
self._approach_directives.append((self.FullFlaps, tuple()))
final_course = [short_final_point, self.RunwayEndpoints[0]]
self._approach_directives.append(
(self._flight_control.DescentCourse,
(final_course, flare_altitude, self.ShortFinalAirSpeed, False)))
func, args = self._approach_directives[self._approach_index]
func(*args)