def initialize(self, filelines):
self.InitializeFromFileLines(filelines)
ms = util.millis(self._sensors.Time())
self._last_update_time = ms
kp,ki,kd = self.ClimbPitchPIDTuningParams
self._climbPitchPID = PID.OutputAdaptivePID(0, kp, ki, kd, PID.DIRECT, ms)
if isinstance(self.PitchPIDLimits,tuple):
mn,mx = self.PitchPIDLimits
else:
_,mx = self.PitchPIDLimits[0]
mn = -mx
self._climbPitchPID.SetOutputLimits (mn, mx)
kp,ki,kd = self.AirspeedPitchPIDTuningParams
self._airspeedPitchPID = PID.OutputAdaptivePID(0, kp, ki, kd, PID.DIRECT, ms)
self._airspeedPitchPID.SetOutputLimits (mn, mx)
kp,ki,kd = self.ThrottlePIDTuningParams
self._throttlePID = PID.PID(0, kp, ki, kd, PID.DIRECT, ms)
self._climbPitchPID.SetSampleTime (self.PitchPIDSampleTime)
self._climbPitchPID.SetAdaptation(*self.ClimbPitchPIDAdaptation)
self._airspeedPitchPID.SetSampleTime (self.PitchPIDSampleTime)
self._airspeedPitchPID.SetAdaptation(*self.AirspeedPIDAdaptation)
self._throttlePID.SetSampleTime (self.ThrottlePIDSampleTime)
self._airspeed_achievement_minutes = self.AirSpeedAchievementMinutes
def initialize(self, filelines):
self.InitializeFromFileLines(filelines)
ms = util.millis(self._sensors.Time())
self._last_update_time = ms
kp, ki, kd = self.RudderPIDTuningParams
self._RudderPID = PID.PID(0, kp, ki, kd, PID.DIRECT, ms)
self._RudderPID.SetSampleTime(self.RudderPIDSampleTime)
def initialize(self, filelines):
self.InitalizeFromFileLines(filelines)
kp, ki, kd = self.GetTunings(self.RudderPIDTuningParams)
ms = util.millis(self._sensors.Time())
self._yawPID = PID.PID(kp, ki, kd, PID.DIRECT, ms)
mn, mx = self.RudderPIDLimits
self._yawPID.SetOutputLimits(mn, mx)
self.aileron_control = eval(self.aileron_control)
self.elevator_control = eval(self.elevator_control)
self.rudder_control = eval(self.rudder_control)
def _begin_flare(self):
# TODO: Make go-around decision
logger.info("Beginning touchdown flare")
self._flarePitchPID.SetMode(PID.AUTOMATIC, self._sensors.ClimbRate(),
self._last_desired_pitch)
self._flight_control.SetCallback(self._callback)
self._flight_control.Stop()
self._flight_mode = SUBMODE_FLARE
self._attitude_control.StartFlight(yaw_mode='side_slip')
self._SlipPID.SetMode(PID.AUTOMATIC, 0, 0)
self._SlipPID.SetSetPoint(0.0, 5.0)
self._last_pitch = self._sensors.Pitch()
self._last_update_time = util.millis(self._sensors.Time())
def initialize(self, filelines):
self.InitializeFromFileLines(filelines)
self._current_airspeed_index = self.get_airspeed_index()
kp, ki, kd = self.GetTunings(self.PitchPIDTuningParams)
ms = util.millis(self._sensors.Time())
self._pitchPID = PID.PID(0, kp, ki, kd, PID.DIRECT, ms)
kp, ki, kd = self.GetTunings(self.YawPIDTuningParams)
self._yawPID = PID.PID(0, kp, ki, kd, PID.DIRECT, ms)
kp, ki, kd = self.GetTunings(self.RollRatePIDTuningParams)
self._rollRatePID = PID.PID(0, kp, ki, kd, PID.DIRECT, ms)
self._pitchPID.SetSampleTime(self.PitchPIDSampleTime)
self._yawPID.SetSampleTime(self.YawPIDSampleTime)
self._rollRatePID.SetSampleTime(self.RollRatePIDSampleTime)
def initialize(self, filelines):
self.InitializeFromFileLines(filelines)
kp,ki,kd = self.PitchRatePIDTuningParams
ms = util.millis(self._sensors.Time())
self._pitchRatePID = PID.PID(0, kp, ki, kd, PID.DIRECT, ms)
kp,ki,kd = self.RollRatePIDTuningParams
self._rollRatePID = PID.PID(0, kp, ki, kd, PID.DIRECT, ms)
kp,ki,kd = self.YawPIDTuningParams
self._yawPID = PID.PID(0, kp, ki, kd, PID.DIRECT, ms)
kp,ki,kd = self.ClimbRatePIDTuningParams
self._climbRatePID = PID.PID(0, kp, ki, kd, PID.DIRECT, ms)
self._pitchRatePID.SetSampleTime (self.PitchPIDSampleTime)
self._yawPID.SetSampleTime (self.YawPIDSampleTime)
self._rollRatePID.SetSampleTime (self.RollRatePIDSampleTime)
self._climbRatePID.SetSampleTime (self.ClimbRatePIDSampleTime)
def initialize(self, filelines):
self.InitializeFromFileLines(filelines)
if self.ApproachAirSpeed == 0:
self.ApproachAirSpeed = self._callback.StallSpeed * 1.7
if self.PatternAirSpeed == 0:
self.PatternAirSpeed = self._callback.StallSpeed * 1.5
if self.FinalAirSpeed == 0:
self.FinalAirSpeed = self._callback.StallSpeed * 1.3
if self.ShortFinalAirSpeed == 0:
self.ShortFinalAirSpeed = self._callback.StallSpeed * 1.2
ms = util.millis(self._sensors.Time())
kp, ki, kd = self.SlipPIDTuningParams
self._SlipPID = PID.PID(0, kp, ki, kd, PID.DIRECT, ms)
mn, mx = self.SideSlipLimits
self._SlipPID.SetOutputLimits(mn, mx)
kp, ki, kd = self.PitchPIDTuningParams
self._flarePitchPID = PID.PID(0, kp, ki, kd, PID.DIRECT, ms)
mn, mx = self.PitchPIDLimits
self._flarePitchPID.SetOutputLimits(mn, mx)
self._flarePitchPID.SetSampleTime(self.PitchPIDSampleTime)
def Update(self):
ms = util.millis(self._sensors.Time())
self.CurrentAirSpeed = self._sensors.AirSpeed()
self.CurrentTrueHeading = self._sensors.TrueHeading()
self.CurrentAltitude = self._sensors.Altitude()
self.CurrentPosition = self._sensors.Position()
heading_nominal = False
if self._flight_mode == SUBMODE_TURN:
self._desired_roll,_ = self.compute_roll(self.DesiredTrueHeading)
if self.completed_turn():
self.get_next_directive()
return None
else:
if self._flight_mode == SUBMODE_SWOOP_DOWN:
roll = 0
if self.CurrentAltitude < self._DesiredAltitude + self.SwoopAltitudeReversal:
self._flight_mode = SUBMODE_SWOOP_UP
self._desired_pitch = self.SwoopPitch
self.DesiredAltitude = self._swoop_high_alt
self._throttle_control.Set (.75)
self._throttlePID.SetMode (PID.MANUAL, self.CurrentAirSpeed,
self._throttle_control.GetCurrent())
elif self.CurrentAirSpeed > self.SwoopMaxAirSpeed:
# Going too fast. Better throttle down
self._throttlePID.SetMode (PID.AUTOMATIC, self.CurrentAirSpeed,
self._throttle_control.GetCurrent())
self.DesiredAirSpeed = self.SwoopMaxAirSpeed
elif self._flight_mode == SUBMODE_SWOOP_UP:
roll = 0
if self.CurrentAltitude > self._DesiredAltitude - self.SwoopAltitudeReversal:
self._throttlePID.SetMode (PID.AUTOMATIC, self.CurrentAirSpeed,
self._throttle_control.GetCurrent())
self._flight_mode = self._mode_before_swoop
self.DesiredAirSpeed = self._airspeed_before_swoop
self.CurrentClimbRate = self._sensors.ClimbRate()
self._climbPitchPID.SetMode (PID.AUTOMATIC,
self.CurrentClimbRate, self._sensors.Pitch())
elif self.CurrentAirSpeed < self._swoop_min_as:
self._throttle_control.Set(1.0)
self._desired_pitch = self.SwoopPitch / 2.0
elif self._flight_mode != SUBMODE_COURSE or \
(self._starting and self.StartStrategy == self.START_STR_HEADING) or \
self.HnavMode == self.HNAV_MODE_HEADING:
if self._starting and self.HnavMode == self.HNAV_MODE_FLIGHT_PLAN:
if self.close_to_course():
self._starting = False
roll,heading_error = self.compute_roll(self.DesiredTrueHeading)
if abs(heading_error) < 5:
heading_nominal = True
else:
if self._flight_mode == SUBMODE_COURSE and self.completed_course():
self.get_next_directive()
return None
roll = self.compute_roll_from_course()
if abs(roll) > self.MaxRoll:
roll = self.MaxRoll if roll > 0 else -self.MaxRoll
self._desired_roll = roll
airspeed_nominal = False
if self._journal_file:
self._journal_file.write(str(ms))
if self._throttle_control is not None and self._throttlePID.GetMode() == PID.AUTOMATIC:
if self._in_pid_optimization != "throttle":
self._throttlePID.SetSetPoint (self.DesiredAirSpeed,
self._airspeed_achievement_minutes * 60.0)
th = self._throttlePID.Compute (self.CurrentAirSpeed, ms)
if self._in_pid_optimization == "throttle":
self._pid_optimization_scoring.IncrementScore(self.CurrentAirSpeed, th, self._pid_optimization_goal, self._journal_file)
if self._journal_file and self.JournalThrottle:
self._journal_file.write(",%g,%g,%g"%(self.DesiredAirSpeed, self.CurrentAirSpeed, th))
self._throttle_control.Set(th)
if abs(self.CurrentAirSpeed - self.DesiredAirSpeed) / self.DesiredAirSpeed < .05:
airspeed_nominal = True
self._desired_climb_rate = self.get_climb_rate()
logger.log(2, "Desired Climb Rate %g - %g ==> %g", self._DesiredAltitude, self.CurrentAltitude, self._desired_climb_rate)
altitude_nominal = False
if self._flight_mode != SUBMODE_SWOOP_DOWN and self._flight_mode != SUBMODE_SWOOP_UP:
self.CurrentClimbRate = self._sensors.ClimbRate()
self._desired_pitch = self.SetPitch(ms)
if abs(self.CurrentAltitude - self._DesiredAltitude) < 50:
altitude_nominal = True
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
pitch_diff = self._desired_pitch - self._last_pitch
time_diff = ms - self._last_update_time
if abs(pitch_diff) > self.MaxPitchChangePerSample * time_diff / self.PitchPIDSampleTime:
pitch_diff = self.MaxPitchChangePerSample * time_diff / self.PitchPIDSampleTime
if self._desired_pitch < self._last_pitch:
pitch_diff *= -1
self._last_pitch += pitch_diff
else:
self._last_pitch = self._desired_pitch
if self._attitude_control is not None:
self._attitude_control.UpdateControls (self._last_pitch, self._desired_roll, 0)
self._last_update_time = ms
if self._notify_when_nominal and altitude_nominal and airspeed_nominal and heading_nominal:
self._notify_when_nominal = False
self.get_next_directive()
return self._last_pitch,self._desired_roll
def initialize(self, filelines):
self.InitializeFromFileLines(filelines)
self._attitude_vtol_estimation.initialize(self.PitchPIDParameters, self.PitchPIDSamplePeriod,
self.PitchPIDLimits, util.millis(self._sensors.Time()))
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
# If not following a course, keep the starting GPS position
self._desired_pitch, self._desired_roll = (
self._attitude_vtol_estimation.EstimateNextAttitude(self.DesiredPosition,
self.CorrectionCurve, self._sensors)
)
if self._flight_mode == SUBMODE_ASCEND:
if self.CurrentAltitude >= self.DesiredAltitude:
self._desired_roll = 0.0
self._desired_pitch = 0.0
if isinstance(self.DesiredCourse, tuple):
self.DesiredTrueHeading = util.TrueHeading (self.DesiredCourse)
else:
self.DesiredTrueHeading = self.DesiredCourse
self._flight_mode = SUBMODE_TRANSITION_PRIME
elif self._flight_mode == SUBMODE_TRANSITION_PRIME:
aborted = False
if self.LandAfterReachingState.startswith (SUBMODE_TRANSITION_PRIME):
state_length = 0
if self.LandAfterReachingState.find ('.') > 0:
try:
state_length = int(self.LandAfterReachingState.rsplit('.', 1)[1])
except Exception as e:
logger.error ("%s: Unable to read land command substate penetration", str(e))
if self._transition_step >= state_length:
aborted = True
self.get_next_directive()
if not aborted:
self._desired_pitch = 0.0
self._desired_roll = 0.0
if self.TransitionSteps[self._transition_step][0] > 10:
# Engines are now facing foward enough that a differential would cause unwanted yaw.
# Just thrust forward
throttle_overrides = [None, None, 0.7, 0.7, None, None]
use_control_surfaces = True
self._sensors.FlightMode(Globals.FLIGHT_MODE_AIRBORN, True)
if self._sensors.AirSpeed() >= self.TransitionSteps[self._transition_step][1]:
self._transition_step += 1
if self._transition_step >= len(self.TransitionSteps):
self._flight_mode = SUBMODE_TRANSITION_SECONDARY
self._vtol_engine_release.Set(1)
throttle_overrides = [.1, .1, 1.0, 1.0, .3, .3]
self._transition_step = self._sensors.Time()
else:
self._engine_tilt.Set(self.TransitionSteps[self._transition_step][0])
elif self._flight_mode == SUBMODE_TRANSITION_SECONDARY:
# 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.LandAfterReachingState.startswith (SUBMODE_TRANSITION_SECONDARY):
state_length = 0
if self.LandAfterReachingState.find ('.') > 0:
try:
state_length = float(self.LandAfterReachingState.rsplit('.', 1)[1])
except Exception as e:
logger.error ("%s: Unable to read land command substate penetration", str(e))
if self._transition_step + state_length < self._sensors.Time():
self.get_next_directive()
use_control_surfaces = True
throttle_overrides = [.1, .1, 1.0, 1.0, .3, .3]
self._desired_pitch = 0.0
self._desired_roll = 0.0
if self._sensors.OuterEnginePosition() == "forward":
if self.LandAfterReachingState.startswith (SUBMODE_TRANSITION_SECONDARY):
logger.info ("Reached end of secondary transition")
self._vtol_engine_release.Set(0)
self.get_next_directive()
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 (3, "Ascend desired pitch = %g, roll=%g, heading_rate=%g",
self._desired_pitch, self._desired_roll, self._desired_heading_rate_change)
self._attitude_control.UpdateControls (self._desired_pitch, self._desired_roll,
self._desired_heading_rate_change, self._desired_climb_rate,
use_control_surfaces, throttle_overrides)
def Update(self):
ms = util.millis(self._sensors.Time())
self.CurrentAirSpeed = self._sensors.AirSpeed()
self.CurrentClimbRate = self._sensors.ClimbRate()
self.CurrentHeadingRate = self._sensors.HeadingRateChange()
if self._flight_mode == SUBMODE_ACCELERATE:
self.DesiredHeadingRate = self.ComputeHeadingRate()
self._RudderPID.SetSetPoint(self.DesiredHeadingRate)
rudder = self._RudderPID.Compute(self.CurrentHeadingRate, ms)
#logger.debug ("Takeoff Set Rudder %g/%g ==> %g", self.CurrentHeadingRate, self.DesiredHeadingRate, rudder)
self._rudder_control.Set(rudder)
if self._sensors.EnginesOut() > 0:
self._abort = True
self._throttle_control.Set(0)
self._brake_control.Set(.75)
if (not self._abort
) and self.CurrentAirSpeed >= self._callback.StallSpeed * 1.2:
self._flight_mode = SUBMODE_ROTATE
self._desired_pitch = self.TakeoffPitch
logger.debug("Takeoff rotating to pitch %g",
self._desired_pitch)
elif self._flight_mode == SUBMODE_ROTATE:
if abs(self._sensors.Pitch() - self._desired_pitch) < 2.0:
self._RudderPID.SetMode(PID.MANUAL, self.CurrentHeadingRate,
self._rudder_control.GetCurrent())
self._flight_mode = SUBMODE_POSITIVE_CLIMB
logger.debug("Takeoff looking for positive climb")
if self._sensors.AGL() > 50.0:
self._throttle_control.Set(self.PositiveLiftPowerSetting)
elif self._flight_mode == SUBMODE_POSITIVE_CLIMB:
if self.CurrentClimbRate >= 100.0 and self._sensors.AGL(
) > self.CompletionAGL:
if self._gear_control != None:
self._gear_control.Set(0)
self._flight_mode = SUBMODE_GEAR_UP
else:
if self._flap_control != None:
self._flap_control.Set(0)
self._flight_mode = SUBMODE_FLAPS_UP
else:
self.get_next_directive()
elif self._sensors.AGL() > 50.0:
self._throttle_control.Set(self.PositiveLiftPowerSetting)
elif self._flight_mode == SUBMODE_GEAR_UP:
if self._sensors.GearUpLocked():
self._flight_mode = SUBMODE_FLAPS_UP
self._flap_control.Set(0)
if self._flap_control != None:
self._flap_control.Set(0)
self._flight_mode = SUBMODE_FLAPS_UP
else:
self.get_next_directive()
elif self._flight_mode == SUBMODE_FLAPS_UP:
if self._sensors.FlapPosition() == 0:
self.get_next_directive()
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
if self._desired_pitch != None:
if self._last_pitch == None:
self._last_pitch = self._sensors.Pitch()
pitch_diff = self._desired_pitch - self._last_pitch
time_diff = ms - self._last_update_time
if abs(
pitch_diff
) > self.MaxPitchChangePerSample * time_diff / self.PitchPIDSampleTime:
pitch_diff = self.MaxPitchChangePerSample * time_diff / self.PitchPIDSampleTime
if self._desired_pitch < self._last_pitch:
pitch_diff *= -1
self._last_pitch += pitch_diff
else:
self._last_pitch = self._desired_pitch
desired_yaw = None if self._flight_mode == SUBMODE_ACCELERATE else 0
self._attitude_control.UpdateControls(self._last_pitch, 0.0,
desired_yaw)
self._last_update_time = ms
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 UpdateControls (self, desired_pitch, desired_roll, desired_heading_rate, desired_climb_rate,
use_control_surfaces, throttle_overrides=None):
self.DesiredPitch = desired_pitch
self.DesiredRoll = desired_roll
self.DesiredHeadingRate = desired_heading_rate
self.DesiredClimbRate = desired_climb_rate
if use_control_surfaces != self._last_surface_control:
self._last_surface_control = use_control_surfaces
if use_control_surfaces:
self._flight_attitude_control.StartFlight()
else:
self._flight_attitude_control.StopFlight()
if use_control_surfaces:
self._flight_attitude_control.UpdateControls (self.DesiredPitch, desired_roll, 0.0)
self.CurrentPitch = self._sensors.Pitch()
self.CurrentPitchRate = self._sensors.PitchRate()
self.CurrentRoll = self._sensors.Roll()
self.CurrentRollRate = self._sensors.RollRate()
ms = util.millis(self._sensors.Time())
if self._journal_file:
self._journal_file.write(str(ms))
if self._in_pid_optimization != "roll":
self.DesiredRollRate = util.get_rate(self.CurrentRoll, self.DesiredRoll, self.AttitudeAchievementSeconds, self.MaxAttitudeRate)
self._rollRatePID.SetSetPoint (self.DesiredRollRate)
self.CurrentRollRate = self._sensors.RollRate()
self._roll_throttle_offset = self._rollRatePID.Compute (self.CurrentRollRate, ms)
if self._in_pid_optimization == "roll":
self._pid_optimization_scoring.IncrementScore(self.CurrentRollRate, self._roll_throttle_offset, self._pid_optimization_goal, self._journal_file)
if self._journal_file and self.JournalRoll:
self._journal_file.write(",%g,%g,%g"%(self.DesiredRollRate, self.CurrentRollRate, self._roll_throttle_offset))
if self._in_pid_optimization != "pitch":
self.DesiredPitchRate = util.get_rate(self.CurrentPitch, self.DesiredPitch, self.AttitudeAchievementSeconds, self.MaxAttitudeRate)
self._pitchRatePID.SetSetPoint (self.DesiredPitchRate)
self.CurrentPitchRate = self._sensors.PitchRate()
self._pitch_throttle_offset = self._pitchRatePID.Compute(self.CurrentPitchRate, ms)
if self._in_pid_optimization == "pitch":
self._pid_optimization_scoring.IncrementScore(self.CurrentPitchRate, self._pitch_throttle_offset, self._pid_optimization_goal, self._journal_file)
if self._journal_file and self.JournalPitch:
self._journal_file.write(",%g,%g,%g"%(self.DesiredPitch, self.CurrentPitchRate, self._pitch_throttle_offset))
if self._in_pid_optimization != "climb":
self._climbRatePID.SetSetPoint (self.DesiredClimbRate)
self.CurrentClimbRate = self._sensors.ClimbRate()
self._throttle_baseline = self._climbRatePID.Compute (self.CurrentClimbRate, ms)
if self._in_pid_optimization == "climb":
self._pid_optimization_scoring.IncrementScore(self.CurrentClimbRate, self._throttle_baseline, self._pid_optimization_goal, self._journal_file)
if self._journal_file and self.JournalClimb:
self._journal_file.write(",%g,%g,%g"%(self.DesiredClimbRate, self.CurrentClimbRate, self._throttle_baseline))
if self._yaw_control:
self.CurrentHeadingRate = self._sensors.HeadingRateChange()
if self._in_pid_optimization != "yaw":
self._yawPID.SetSetPoint (self.DesiredHeadingRate)
tilt_delta = self._yawPID.Compute(self.CurrentHeadingRate, ms)
# TODO: Set vectored thrust for yaw
if self._in_pid_optimization == "yaw":
self._pid_optimization_scoring.IncrementScore(self.CurrentHeadingRate, tilt_delta, self._pid_optimization_goal, self._journal_file)
if self._journal_file and self.JournalYaw:
self._journal_file.write(",%g,%g,%g"%(self.DesiredHeadingRate, self.CurrentHeadingRate, tilt_delta))
self._yaw_control.Set(tilt_delta)
if self._journal_file and (not self._in_pid_optimization):
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
throttle_values = [self._throttle_baseline + self._baseline_offset for i in range(self.NumberEngines)]
# Add pitch throttle offsets to front and rear engines
for engine_number in self.FrontEngines:
throttle_values [engine_number] += self._pitch_throttle_offset
for engine_number in self.RearEngines:
throttle_values [engine_number] -= self._pitch_throttle_offset
# Add roll throttle offsets to right and left engines
for engine_number in self.RightEngines:
throttle_values [engine_number] -= self._roll_throttle_offset
for engine_number in self.LeftEngines:
throttle_values [engine_number] += self._roll_throttle_offset
throttle_values = [tv if tv >= 0.0 else 0.0 for tv in throttle_values]
throttle_values = [tv if tv <= 1.0 else 1.0 for tv in throttle_values]
if throttle_overrides != None:
throttle_values = list(map(lambda ct,ot: ot if ot!=None else ct, throttle_values, throttle_overrides))
# Set the throttles
self._throttle_controls.SetThrottles(throttle_values)
def UpdateControls(self, desired_pitch, desired_roll, desired_yaw):
self.DesiredPitch = desired_pitch
self.DesiredRoll = desired_roll
self.DesiredYaw = desired_yaw
asidx = self.get_new_airspeed_index()
if asidx != self._current_airspeed_index:
# Update the tuning parameters for the new airspeed category
self._current_airspeed_index = asidx
if self._in_pid_optimization != "yaw":
kp, ki, kd = self.GetTunings(self.YawPIDTuningParams)
self._yawPID.SetTunings(kp, ki, kd)
if self._in_pid_optimization != "roll":
kp, ki, kd = self.GetTunings(self.RollRatePIDTuningParams)
self._rollRatePID.SetTunings(kp, ki, kd)
if self._in_pid_optimization != "pitch":
kp, ki, kd = self.GetTunings(self.PitchPIDTuningParams)
self._pitchPID.SetTunings(kp, ki, kd)
self.CurrentYaw = self._sensors.Yaw()
self.CurrentRoll = self._sensors.Roll()
self.CurrentPitch = self._sensors.Pitch()
logger.log(5, "Attitude Control: pitch=%g/%g, roll=%g/%g, yaw = %g/%s",
self.CurrentPitch, desired_pitch, self.CurrentRoll,
desired_roll, self.CurrentYaw, str(desired_yaw))
ms = util.millis(self._sensors.Time())
self.CurrentRollRate = self._sensors.RollRate()
if self._journal_file:
self._journal_file.write(str(ms))
if self._in_pid_optimization != "roll":
desired_roll_rate = self.get_roll_rate()
self._rollRatePID.SetSetPoint(desired_roll_rate,
self.AttitudeAchievementTime / 5.0)
self.aileron_deflection = self._rollRatePID.Compute(
self.CurrentRollRate, ms)
self._aileron_control.Set(self.aileron_deflection)
if self._in_pid_optimization == "roll":
self._pid_optimization_scoring.IncrementScore(
self.CurrentRollRate, self.aileron_deflection,
self._pid_optimization_goal, self._journal_file)
if self._journal_file and self.JournalRoll:
self._journal_file.write(",%g,%g,%g" %
(desired_roll_rate, self.CurrentRollRate,
self.aileron_deflection))
if self._in_pid_optimization != "pitch":
desired_pitch = self.DesiredPitch + self.CurrentRoll * self.RollPitchRatio
self._pitchPID.SetSetPoint(desired_pitch,
self.AttitudeAchievementTime)
elevator_value = self._pitchPID.Compute(self.CurrentPitch, ms)
self._elevator_control.Set(elevator_value)
if self._in_pid_optimization == "pitch":
self._pid_optimization_scoring.IncrementScore(
self.CurrentPitch, elevator_value, self._pid_optimization_goal,
self._journal_file)
if self._journal_file and self.JournalPitch:
self._journal_file.write(
",%g,%g,%g" %
(desired_pitch, self.CurrentPitch, elevator_value))
if self.DesiredYaw != None:
if self._in_pid_optimization != "yaw":
self._yawPID.SetSetPoint(self.DesiredYaw,
self.AttitudeAchievementTime / 2.0)
if self._yaw_mode == None:
rudder_value = self._yawPID.Compute(self.CurrentYaw,
ms) + self.rudder_offset()
if self._in_pid_optimization == "yaw":
self._pid_optimization_scoring.IncrementScore(
self.CurrentYaw, rudder_value,
self._pid_optimization_goal, self._journal_file)
elif self._yaw_mode == 'side_slip':
input_val = self._sensors.TrueHeading()
if input_val - self.DesiredYaw < -180:
input_val += 360
elif input_val - self.DesiredYaw > 180:
input_val -= 360
rudder_value = self._yawPID.Compute(input_val, ms)
logger.log(5, "Side slip: %g/%g ==> %g",
self._sensors.TrueHeading(), self.DesiredYaw,
rudder_value)
self._rudder_control.Set(rudder_value)
if self._journal_file and self.JournalYaw:
self._journal_file.write(
",%g,%g,%g" %
(self.DesiredYaw, self.CurrentYaw, rudder_value))
if self._journal_file and (not self._in_pid_optimization):
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
def Update(self):
agl = self._sensors.AGL()
if agl < 100 and self._flight_mode != SUBMODE_FLARE:
self._begin_flare()
if self._flight_mode == SUBMODE_APPROACH:
self._flight_control.Update()
elif self._flight_mode == SUBMODE_FLARE:
ms = util.millis(self._sensors.Time())
# Compute Pitch
desired_descent = util.rate_curve(agl, self.FlareDescentCurve)
self.CurrentClimbRate = self._sensors.ClimbRate()
self._flarePitchPID.SetSetPoint(desired_descent,
self.ClimbRateAchievementSeconds)
desired_pitch = self._flarePitchPID.Compute(
self.CurrentClimbRate, ms)
logger.log(10, "Climb Pitch PID: %g/%g-->%g",
self.CurrentClimbRate, desired_descent, desired_pitch)
pitch_diff = desired_pitch - self._last_pitch
time_diff = ms - self._last_update_time
if abs(
pitch_diff
) > self.MaxPitchChangePerSample * time_diff / self.PitchPIDSampleTime:
pitch_diff = self.MaxPitchChangePerSample * time_diff / self.PitchPIDSampleTime
if desired_pitch < self._last_pitch:
pitch_diff *= -1
self._last_pitch += pitch_diff
else:
self._last_pitch = desired_pitch
self._last_update_time = ms
# Compute Roll
side, forward, heading, heading_to_dest = util.CourseDeviation(
self._sensors.Position(), self.RunwayEndpoints, self.rel_lng)
side *= util.FEET_NM
ground_track = self._sensors.GroundTrack()
ground_speed = self._sensors.GroundSpeed()
relative_ground_track = ground_track - heading
if relative_ground_track > 180:
relative_ground_track -= 360
elif relative_ground_track < -180:
relative_ground_track += 360
shift_rate = ground_speed * math.sin(
relative_ground_track * util.RAD_DEG)
shift_rate *= util.FEET_NM / (3600.0) # nm / hour --> feet / s
shift_rate = util.LowPassFilter(shift_rate, self._slip_history)
self._last_side_error = side
desired_shift_rate = util.rate_curve(side, self.SideSlipCurve)
self._SlipPID.SetSetPoint(desired_shift_rate, 2.0)
roll = self._SlipPID.Compute(shift_rate, ms)
self._throttle_control.Set(
util.rate_curve(agl, self.FlarePowerCurve))
logger.log(
10,
"Flaring with side error=%g, shift_rate=%g/%g, roll=%g, agl=%g, pitch=%g/%g",
side, shift_rate, desired_shift_rate, roll, agl,
self._last_pitch, desired_pitch)
self._attitude_control.UpdateControls(self._last_pitch, roll,
heading)
if self._sensors.AirSpeed() < self._callback.StallSpeed * .4:
self._throttle_control.Set(0.0)
# Landed
self.get_next_directive()
def UpdateControls(self, desired_yaw):
self.DesiredYaw = desired_yaw
ms = util.millis(self._sensors.Time())
self._yawPID.SetSetPoint(self.DesiredYaw)
self.rudder_control.Set(self._yawPID.Compute(self.CurrentYaw, ms))
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)