def completed_course(self):
# The course is considered completed if the aircraft has crossed the line
# perpendicular to the course and intersecting the terminal waypoint
# Translate the current position to a coordinate space with the terminal point at origin
angle,distance,rel_lng = util.TrueHeadingAndDistance(self.DesiredCourse)
lng,lat = self.CurrentPosition
tlng,tlat = self.DesiredCourse[1]
dlng = lng-tlng
dlat = lat-tlat
dlng *= rel_lng
x,y = util.rotate2d (angle * util.M_PI / 180, dlng, dlat)
#util.log_occasional_info("completed_course",
# "cur_pos = (%g,%g), to=(%g,%g), dlng=%g, dlat=%g, angle=%g, xy=%g,%g"%(
# self.CurrentPosition[0], self.CurrentPosition[1],
# self.DesiredCourse[1][0], self.DesiredCourse[1][1],
# dlng, dlat, angle, x, y))
# turn_circumference = airspeed(nm / minute) / turn_rate (degrees / minute) * 360 degrees
# turn_diameter = turn_circumference / (2 * PI)
# turn_radius = turn_diameter / 2
if self._course_rounding:
turn_radius = self.turn_radius()
else:
turn_radius = 0
if y >= -turn_radius / 60.0:
logger.debug("Flight Control completed course")
return True
else:
return False
def SetPitch(self, ms):
if self._pitch_mode == None:
# Figure out if we're using air speed or climb rate to select pitch:
if (self._throttle_control.GetCurrent() == self._throttle_range[1]
and
(self.CurrentAirSpeed <= self.MinClimbAirSpeed or
(self._using_airspeed_pitch
and self.CurrentAirSpeed <= self.MinClimbAirSpeed * 1.1))
and self.DesiredAltitude > self.CurrentAltitude):
# Change to Use airspeed to control pitch
asret = self.UpdateAirspeedPitch(ms)
crret = self.UpdateClimbratePitch(ms)
if asret < crret:
self._using_airspeed_pitch = True
logger.log(
3,
"Pitch chosen to preserve airspeed (%g) instead of climb rate (%g)",
asret, crret)
ret = asret
else:
self._using_airspeed_pitch = False
logger.log(
3,
"Pitch chosen to preserve climb rate (%g) instead of air speed (%g)",
crret, asret)
ret = crret
else:
self._airspeedPitchPID.SetMode(PID.MANUAL,
self.CurrentAirSpeed,
-self._desired_pitch)
ret = self.UpdateClimbratePitch(ms)
self._using_airspeed_pitch = False
else: # Pitch mode for controlled descent
if self._climbPitchPID.GetMode() != PID.AUTOMATIC:
self._climbPitchPID.SetMode(PID.AUTOMATIC,
self.CurrentClimbRate,
self._desired_pitch)
# Figure out how high or low are we
remaining_course = [self.CurrentPosition, self.DesiredCourse[1]]
_, remaining_distance, self._rel_lng = util.TrueHeadingAndDistance(
remaining_course, rel_lng=self._rel_lng)
fraction_remaining = remaining_distance / self._descent_distance
desired_altitude = (
self.DesiredAltitude * (1 - fraction_remaining) +
self._descent_starting_altitude * fraction_remaining)
altitude_error = self.CurrentAltitude - desired_altitude
self._desired_climb_rate = util.rate_curve(altitude_error,
self.DescentCurve)
self._desired_climb_rate += self._nominal_descent_rate
self._climbPitchPID.SetSetPoint(self._desired_climb_rate,
self.ClimbRateAchievementSeconds)
logger.log(5, "Descent Rate %g/%g--> %g", self.CurrentAltitude,
desired_altitude, self._desired_climb_rate)
ret = self._climbPitchPID.Compute(self.CurrentClimbRate, ms)
logger.log(5, "Descent Pitch %g/%g--> %g", self.CurrentClimbRate,
self._desired_climb_rate, ret)
return ret
def FlyCourse(self,
course,
desired_altitude=0,
desired_airspeed=0,
rounding=False):
assert (self.CurrentFlightMode == Globals.FLIGHT_MODE_AIRBORN)
self.DesiredCourse = course
heading, _a, _ = util.TrueHeadingAndDistance(self.DesiredCourse)
curpos = self._sensors.Position()
dist = [curpos, course[1]]
_a, distance, _ = util.TrueHeadingAndDistance(dist)
ret = "Flying course with destination waypoint %g nm away, bearing %g degrees, with altitude %g, airspeed %g" % (
distance, heading, desired_altitude, desired_airspeed)
logger.info(ret)
if desired_altitude:
self.DesiredAltitude = desired_altitude
if desired_airspeed:
self.DesiredAirSpeed = desired_airspeed
self._flight_control.FlyCourse(course, desired_altitude,
desired_airspeed, rounding)
return ret
def FlyTo(self, next_waypoint, desired_altitude=0, desired_airspeed=0):
assert (self.CurrentFlightMode == Globals.FLIGHT_MODE_AIRBORN)
last_waypoint = self._sensors.Position()
self.DesiredCourse = [last_waypoint, next_waypoint]
heading, distance, _ = util.TrueHeadingAndDistance(self.DesiredCourse)
ret = "Flying to waypoint %g nm away, bearing %g degrees, with altitude %g, airspeed %g" % (
distance, heading, desired_altitude, desired_airspeed)
logger.info(ret)
if desired_altitude:
self.DesiredAltitude = desired_altitude
if desired_airspeed:
self.DesiredAirSpeed = desired_airspeed
self._flight_control.FlyTo(next_waypoint, desired_altitude,
desired_airspeed)
return ret
def initiate_linear_descent(self):
if self.HnavMode == self.HNAV_MODE_FLIGHT_PLAN and \
self._previous_waypoint < len(self.Waypoints) and \
self._previous_waypoint >= 0:
self._descent_starting_altitude = self.WPAltitudes[self._previous_waypoint]
full_course = self.DesiredCourse
else:
self._descent_starting_altitude = self.CurrentAltitude
full_course = [self.CurrentPosition, self.DesiredCourse[1]]
_,self._descent_distance, self._rel_lng = util.TrueHeadingAndDistance (full_course)
hours_to_dest = self._descent_distance / self.DesiredAirSpeed
self._airspeed_achievement_minutes = hours_to_dest * 60.0
full_descent = self._DesiredAltitude - self._descent_starting_altitude
self._nominal_descent_rate = full_descent / (hours_to_dest * 60.0)
logger.debug("Starting Descent course: distance=%g, descent of %g feet, nominal rate = %g",
self._descent_distance, full_descent,
self._nominal_descent_rate)
def Takeoff(self, approach_endpoints, soft_field):
self._flight_mode = SUBMODE_ACCELERATE
self._abort = False
self._last_pitch = self._sensors.Pitch()
if soft_field:
self._desired_pitch = self.TakeoffPitch
else:
self._desired_pitch = self._last_pitch
self._attitude_control.StartFlight()
self._throttle_control.Set(1.0)
logger.debug("Throttle up")
self.DesiredCourse = approach_endpoints
self.DesiredTrueHeading, _, __ = util.TrueHeadingAndDistance(
self.DesiredCourse)
self.CurrentHeadingRate = self._sensors.HeadingRateChange()
self._RudderPID.SetMode(PID.AUTOMATIC, self.CurrentHeadingRate,
self.InitialRudder)
def DescentCourse(self,
course,
desired_altitude,
desired_airspeed=0,
rounding=True):
self.DesiredCourse = course
self.DesiredAltitude = desired_altitude
if desired_airspeed:
self.DesiredAirSpeed = desired_airspeed
self._flight_mode = SUBMODE_COURSE
self._pitch_mode = "controlled_descent"
self._course_rounding = rounding
self._descent_starting_altitude = self.CurrentAltitude
full_course = [self.CurrentPosition, course[1]]
_, self._descent_distance, self._rel_lng = util.TrueHeadingAndDistance(
full_course)
hours_to_dest = self._descent_distance / self.DesiredAirSpeed
self._airspeed_achievement_minutes = hours_to_dest * 60.0
full_descent = self.DesiredAltitude - self._descent_starting_altitude
self._nominal_descent_rate = full_descent / (hours_to_dest * 60.0)
logger.debug(
"Starting Descent course: distance=%g, descent of %g feet, nominal rate = %g",
self._descent_distance, full_descent, self._nominal_descent_rate)
def SetPitch(self, ms):
alt_err = self._DesiredAltitude - self.CurrentAltitude
if abs(alt_err) < self.ClimbPitchCurve[0][0]:
self._altitude_acheived = True
elif abs(alt_err) > self.ClimbPitchCurve[-1][0]:
self._altitude_acheived = False
if self._altitude_acheived:
# After altitude acheived, go into maintenance mode
# Figure out if we're using air speed or climb rate to select pitch:
if ( (self._throttle_control is None or
self._throttle_control.GetCurrent() == self._throttle_range[1]) and
(self.CurrentAirSpeed <= self.MinClimbAirSpeed or
(self._force_airspeed_pitch and
self.CurrentAirSpeed <= self.MinClimbAirSpeed * 1.1)) and
self._DesiredAltitude > self.CurrentAltitude):
# Change to Use airspeed to control pitch
asret = self.UpdateAirspeedPitch(ms)
crret = self.AltitudeMaintenancePitch(ms)
if asret < crret:
self._force_airspeed_pitch = True
logger.log (3, "Pitch chosen to preserve airspeed (%g) instead of climb rate (%g)",
asret, crret)
ret = asret
else:
self._force_airspeed_pitch = False
logger.log (3, "Pitch chosen to preserve climb rate (%g) instead of air speed (%g)",
crret, asret)
ret = crret
else:
self._airspeedPitchPID.SetMode (PID.MANUAL, self.CurrentAirSpeed, -self._desired_pitch)
ret = self.AltitudeMaintenancePitch(ms)
self._force_airspeed_pitch = False
elif self._VnavMode == self.VNAV_MODE_LINEAR:
if self._climbPitchPID.GetMode() != PID.AUTOMATIC:
self._climbPitchPID.SetMode (PID.AUTOMATIC,
self.CurrentClimbRate, self._sensors.Pitch())
self._set_pitch_pid_limits(self.PitchPIDLimits, self._climbPitchPID)
# Figure out how high or low are we
remaining_course = [self.CurrentPosition, self.DesiredCourse[1]]
_,remaining_distance,self._rel_lng = util.TrueHeadingAndDistance(remaining_course,
rel_lng=self._rel_lng)
fraction_remaining = remaining_distance / self._descent_distance
desired_altitude = (self._DesiredAltitude * (1 - fraction_remaining) +
self._descent_starting_altitude * fraction_remaining)
altitude_error = desired_altitude - self.CurrentAltitude
self._desired_climb_rate = util.rate_curve (altitude_error, self.DescentCurve)
self._desired_climb_rate += self._nominal_descent_rate
self._climbPitchPID.SetSetPoint (self._desired_climb_rate, self.ClimbRateAchievementSeconds)
logger.log (5, "Descent Rate nominal %g, %g/%g--> %g", self._nominal_descent_rate,
self.CurrentAltitude, desired_altitude, self._desired_climb_rate)
ret = self._climbPitchPID.Compute (self.CurrentClimbRate, self._sensors.Pitch(), ms)
logger.log (5, "Descent Pitch %g/%g--> %g", self.CurrentClimbRate, self._desired_climb_rate,
ret)
elif self._VnavMode == self.VNAV_MODE_AS:
ret = self.UpdateAirspeedPitch(ms, False)
elif self._VnavMode == self.VNAV_MODE_PITCH:
ret = self.SelectedPitch * self._pitch_sign
elif self._VnavMode == self.VNAV_MODE_VS:
if self._climbPitchPID.GetMode() != PID.AUTOMATIC:
self._climbPitchPID.SetMode (PID.AUTOMATIC,
self.CurrentClimbRate, self._sensors.Pitch())
self._set_pitch_pid_limits(self.PitchPIDLimits, self._climbPitchPID)
if self._in_pid_optimization != "climb_pitch":
self._climbPitchPID.SetSetPoint (self.DesiredClimbRate * self._pitch_sign,
self.ClimbRateAchievementSeconds)
ret = self._climbPitchPID.Compute (self.CurrentClimbRate, self._sensors.Pitch(), ms)
logger.log (5, "Climb Pitch PID: %g/%g-->%g",
self.CurrentClimbRate, self._desired_climb_rate, ret)
if self._in_pid_optimization == "climb_pitch":
self._pid_optimization_scoring.IncrementScore(self.CurrentClimbRate, ret, self._pid_optimization_goal, self._journal_file)
if self._journal_file and self.JournalPitch:
self._journal_file.write(",%g,%g,%g"%(self._desired_climb_rate, self.CurrentClimbRate, ret))
else:
raise RuntimeError("Unimplemented VNAV mode: %d"%self._VnavMode)
return ret
def EstimateNextAttitude(
self,
desired_position, # Where to stay or move to (2-tuple of lng,lat)
CorrectionCurve,
sensors): # Reference to the sensors object
_time = sensors.Time()
current_speed = sensors.GroundSpeed()
ground_track = sensors.GroundTrack()
current_velocity = Spatial.Vector()
# y is North component (positive latitude)
current_velocity.y = current_speed * math.cos(
ground_track * util.RAD_DEG)
# x is East Component (positive longitude)
current_velocity.x = current_speed * math.sin(
ground_track * util.RAD_DEG)
# velocity units: knots
current_position = sensors.Position()
if current_position != self.last_position:
self.last_position = current_position
self.position_offset.x = 0
self.position_offset.y = 0
else:
time_delta = _time - self.last_time
if time_delta > 0.0:
# Convert knots (nautical miles per hour) to globe degrees per second:
# 60 nautical miles per degree, 3600 seconds per hour
UpdatePosition(
self.position_offset, time_delta, ground_track,
current_speed / (NAUTICAL_MILES_PER_DEGREE * 3600))
current_position = (current_position[0] + self.position_offset.x,
current_position[1] + self.position_offset.y)
deltat = _time - self.last_time
logger.debug("current_velocity %g(%g) ==> %g,%g", current_speed,
ground_track, current_velocity.x, current_velocity.y)
desired_heading, distance, _ = util.TrueHeadingAndDistance(
[current_position, desired_position])
# Units nautical miles (nm) per hour (knots)
# Distance units given in globe degrees (which contain 60 nm)
desired_groundspeed = util.rate_curve(
distance * FEET_PER_NAUTICAL_MILE, CorrectionCurve)
# desired_groundspeed in knots
desired_velocity = Spatial.Vector()
desired_velocity.y = desired_groundspeed * math.cos(
desired_heading * util.RAD_DEG)
desired_velocity.x = desired_groundspeed * math.sin(
desired_heading * util.RAD_DEG)
logger.debug(
"current_position = %g,%g, desired_position = %g,%g, Desired Velocity: %g(%g) ==> %g,%g",
current_position[0], current_position[1], desired_position[0],
desired_position[1], desired_groundspeed, desired_heading,
desired_velocity.x, desired_velocity.y)
delta_vel = copy.copy(desired_velocity)
delta_vel.sub(current_velocity)
current_acceleration = copy.copy(current_velocity)
if self.last_time == 0 or deltat == 0.0:
current_acceleration.x = 0.0
current_acceleration.y = 0.0
else:
current_acceleration.sub(self._last_velocity)
current_acceleration.div(deltat)
self._last_velocity = current_velocity
current_acceleration.x = util.FIRFilter(current_acceleration.x,
self._last_acceleration_x,
self.AccelerationFIR)
current_acceleration.y = util.FIRFilter(current_acceleration.y,
self._last_acceleration_y,
self.AccelerationFIR)
desired_accel = Spatial.Vector()
ms = util.millis(_time)
self.xpid.SetSetPoint(delta_vel.x)
desired_accel.x = self.xpid.Compute(current_acceleration.x, ms)
self.ypid.SetSetPoint(delta_vel.y)
desired_accel.y = self.ypid.Compute(current_acceleration.y, ms)
dvnorm = desired_accel.norm()
if dvnorm > .001:
desired_thrust_direction = util.atan_globe(desired_accel.x,
desired_accel.y)
relative_vector = dvnorm
current_true_heading = sensors.TrueHeading()
desired_relative_thrust = desired_thrust_direction - current_true_heading * util.RAD_DEG
forward_vector = math.cos(
desired_relative_thrust) * relative_vector
side_vector = math.sin(desired_relative_thrust) * relative_vector
# tan(-pitch) = forward_vector
self.desired_pitch = -math.atan(forward_vector) * util.DEG_RAD
self.desired_roll = math.atan(side_vector) * util.DEG_RAD
else:
self.desired_pitch = 0.0
self.desired_roll = 0.0
if self._journal_file:
self._journal_file.write(
"%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n" %
(_time, distance * FEET_PER_NAUTICAL_MILE, current_velocity.x,
current_velocity.y, desired_velocity.x, desired_velocity.y,
current_acceleration.x, current_acceleration.y,
desired_accel.x * 10, desired_accel.y * 10,
self.desired_pitch, self.desired_roll, sensors.Pitch(),
sensors.Roll()))
self.last_time = _time
return self.desired_pitch, self.desired_roll
def Update(self):
ms = util.millis(self._sensors.Time())
self.CurrentPosition = self._sensors.Position()
self.CurrentAltitude = self._sensors.Altitude()
self._desired_climb_rate = self.get_climb_rate()
self.CurrentHeading = self._sensors.Heading()
throttle_overrides = None
self._desired_heading_rate_change = 0.0
use_control_surfaces = False
self._desired_pitch, self._desired_roll = (
self._attitude_vtol_estimation.EstimateNextAttitude(
self.DesiredPosition, self.CorrectionCurve, self._sensors))
if self._flight_mode == SUBMODE_DESCEND or self._flight_mode == SUBMODE_POSITION:
agl = self._sensors.AGL()
logger.debug("Checking touchdown mode: %g / %g", agl,
self._callback.GroundEffectHeight)
if agl < 0.5:
self._flight_mode = SUBMODE_TOUCHDOWN
self._desired_pitch = 0.0
self._desired_roll = 0.0
self._desired_climb_rate = -20.0
logger.debug("Entering touchdown mode")
elif self._flight_mode == SUBMODE_TOUCHDOWN:
self._desired_climb_rate = -20.0
self._desired_pitch = 0
self._desired_roll = 0
if self._attitude_control.ForceThrottleDown():
self.get_next_directive()
elif self._flight_mode == SUBMODE_TRANSITION_PRIME:
# Release tilt locks, apply movement slowing brakes, add power to rear engine, and
# apply downward elevator to compensate for rear engine upward thrust.
# Adding power to the rear engines will cause the front and back engines to tilt
# into forward flight mode. The movement brakes will force the transition to be
# gradual enough for a smooth transition.
if self._sensors.OuterEnginePosition() != "vertical":
use_control_surfaces = True
throttle_overrides = [.1, .1, 1.0, 1.0, .3, .3]
self._desired_pitch = 0.0
self._desired_roll = 0.0
else:
if self._forward_engine_release != None:
self._forward_engine_release.Set(0)
self._flight_mode = SUBMODE_TRANSITION_SECONDARY
elif self._flight_mode == SUBMODE_TRANSITION_SECONDARY:
# move the middle engines into vertical thrust, following self.TransitionSteps
self._desired_pitch = 0.0
self._desired_roll = 0.0
if (self._engine_tilt == None) or (self._engine_tilt.GetCurrent()
== 0):
self._sensors.FlightMode(Globals.FLIGHT_MODE_LANDING, True)
self._flight_mode = SUBMODE_POSITION
elif self._sensors.AirSpeed() <= self.TransitionSteps[
self._transition_step][1]:
self._transition_step += 1
if self._transition_step >= len(self.TransitionSteps):
self._engine_tilt.Set(0.0)
self._flight_mode = SUBMODE_POSITION
self._sensors.FlightMode(Globals.FLIGHT_MODE_LANDING, True)
else:
self._engine_tilt.Set(
self.TransitionSteps[self._transition_step][0])
if self.TransitionSteps[self._transition_step][0] < 10:
self._sensors.FlightMode(Globals.FLIGHT_MODE_LANDING, True)
else:
use_control_surfaces = True
if self._flight_mode == SUBMODE_POSITION:
self.CurrentPosition = self._sensors.Position()
heading, distance, _ = util.TrueHeadingAndDistance(
[self.CurrentPosition, self.DesiredPosition])
self.DesiredTrueHeading = heading
if distance < .1:
self.DesiredTrueHeading, _, _ = util.TrueHeadingAndDistance(
self.DesiredCourse)
self.DesiredAltitude = self.RunwayAltitude - 20
self._flight_mode = SUBMODE_DESCEND
if self._journal_file:
self._journal_file.write(str(ms))
if self._journal_file:
self._journal_file.write("\n")
self._journal_flush_count += 1
if self._journal_flush_count >= 10:
self._journal_file.flush()
self._journal_flush_count = 0
self.compute_heading_rate()
logger.log(
5, "Descend desired pitch = %g, roll=%g, climb = %g" %
(self._desired_pitch, self._desired_roll,
self._desired_climb_rate))
self._attitude_control.UpdateControls(
self._desired_pitch, self._desired_roll,
self._desired_heading_rate_change, self._desired_climb_rate,
use_control_surfaces, throttle_overrides)