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)