def updated(self, channel): if self.a_z != 0: if isinstance(self.accelerometer_calibration, dict): self.a_x = util.rate_curve(self.a_x, self.accelerometer_calibration['x']) self.a_y = util.rate_curve(self.a_y, self.accelerometer_calibration['y']) self.a_z = util.rate_curve(self.a_z, self.accelerometer_calibration['z']) self.pitch_estimate = math.atan( float(self.a_y) / float(self.a_z)) * util.DEG_RAD self.timestamp = self.accelerometers_updated self.publish() print("PitchEstimate: %d,%d => %g" % (self.a_z, self.a_y, self.pitch_estimate))
def AltitudeMaintenancePitch(self, ms): alt_err = self._DesiredAltitude - self.CurrentAltitude if abs(alt_err) < self.ClimbPitchCurve[0][0]: 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._desired_climb_rate, self.ClimbRateAchievementSeconds) desired_pitch = self._climbPitchPID.Compute (self.CurrentClimbRate, self._sensors.Pitch(), ms) logger.log (5, "Climb Pitch PID: %g/%g-->%g", self.CurrentClimbRate, self._desired_climb_rate, desired_pitch) if self._in_pid_optimization == "climb_pitch": self._pid_optimization_scoring.IncrementScore(self.CurrentClimbRate, desired_pitch, 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, desired_pitch)) else: if self._climbPitchPID.GetMode() != PID.MANUAL: self._climbPitchPID.SetMode (PID.MANUAL, self.CurrentClimbRate, self._desired_pitch) desired_pitch = util.rate_curve(alt_err, self.ClimbPitchCurve) mn,mx = self._find_pitch_limits(self.PitchPIDLimits) if desired_pitch < mn: desired_pitch = mn elif desired_pitch > mx: desired_pitch = mx logger.log (5, "Climb Pitch Curve: %g-->%g", alt_err, desired_pitch) return desired_pitch
def _calibrated_heading(self, heading): # In each table, estimate the calibrated heading # estimate calibrated heading through a piece wise linear function if isinstance(self.heading_calibration, dict): heading = rate_curve( heading, self.heading_calibration['compass_correction'], False) return heading
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 updated(self, channel): if self.ascurve is not None: if self.pitot_pressure > 0: # Only output if we have 2 valid pressures to compare self.timestamp = self.pressuresensors_updated pdiff = self.static_pressure - self.pitot_pressure self.airspeed_computed = util.rate_curve(pdiff, self.ascurve) self.airspeed_computed = int(round(self.airspeed_computed)) self.publish() print("AirspeedComputed: %d" % self.airspeed_computed) else: print("AirspeedComputed: no pitot") else: print("AirspeedComputed: don't have curve")
def get_climb_rate(self): altitude_error = self._DesiredAltitude - self.CurrentAltitude if self.ClimbRateCurve != None: ret = util.rate_curve(altitude_error, self.ClimbRateCurve) logger.log (5, "Climb rate curve %g ==> %g", altitude_error, ret) else: climb_rate = altitude_error / self.AltitudeAchievementMinutes limits = self.ClimbRateLimits if climb_rate < limits[0]: climb_rate = limits[0] elif climb_rate > limits[1]: climb_rate = limits[1] ret = climb_rate return ret
def updated(self, channel): if channel == 'accelerometers': if isinstance(self.accelerometer_calibration, dict): self.a_x = rate_curve (self.a_x, self.accelerometer_calibration['x'], False) self.a_x += self.xoffset self.yaw = self.a_x * self.accel_factor self.timestamp = self.accelerometers_updated self.yaw_confidence = 10.0 self.publish () print ("Yaw: %g => %g"%(self.a_x, self.yaw)) elif channel == 'systemcommand': if self.command.startswith( b'0att'): self.xoffset = -self.a_x
def updated(self, channel): if channel == 'accelerometers': if self.a_z != 0: if isinstance(self.accelerometer_calibration, dict): self.a_y = util.rate_curve (self.a_y, self.accelerometer_calibration['y'], False) self.a_z = util.rate_curve (self.a_z, self.accelerometer_calibration['z'], False) self.a_y += self.yoffset roll = 0 if self.roll is not None and \ self.Roll_updated - self.accelerometers_updated < .4: roll = self.roll * util.RAD_DEG self.pitch_estimate = math.atan(self.a_y / (self.a_z*math.cos(roll))) self.pitch_estimate *= util.DEG_RAD self.timestamp = self.accelerometers_updated self.publish () print ("PitchEstimate: roll(%.1f) %.2f,%.2f,%.2f => %.2f"%(roll, self.a_x, self.a_y, self.a_z, self.pitch_estimate)) elif channel == 'systemcommand' and hasattr(self, 'a_y'): if self.command.startswith( b'0att'): self.yoffset = -self.a_y
def updated(self, channel): if channel == 'knownaltitude': if self.temperature is not None: self.standard_sea_level_temp = self.temperature + 1.98 * self.known_altitude / 1000.0 elif channel == 'systemcommand': print("Got systemcommand: %s" % str(self.command)) if self.command.startswith(b'baroinhg'): print("Got new baro setting: %.2f" % float(self.args.strip(b'\x00'))) self.sea_level_pressure = (float(self.args.strip(b'\x00')) * KPA_INHG) elif self.command.startswith(b'barompa'): self.sea_level_pressure = (float(self.args.strip(b'\x00')) * 1000) if self.static_pressure is not None: if isinstance(self.pressure_calibration, dict): self.static_pressure = util.rate_curve( self.static_pressure, self.pressure_calibration['pressure_calibration']) if self.known_altitude is not None and self.static_pressure is not None and \ self.temperature is not None: self.sea_level_pressure = (self.static_pressure / pow( 1.0 - (self.known_altitude * util.METERS_FOOT * PRESSURE_DIVISOR / (self.temperature + KELVIN_OFFSET)), PRESSURE_POWER)) self.known_altitude = None if self.sea_level_pressure is not None and self.static_pressure is not None and \ self.temperature is not None: if self.standard_sea_level_temp is None: altitude_computed = ( (pow(self.sea_level_pressure / self.static_pressure, 1 / PRESSURE_POWER) - 1) * (self.temperature + KELVIN_OFFSET)) / PRESSURE_DIVISOR altitude_computed *= util.FEET_METER self.standard_sea_level_temp = self.temperature + 1.98 * altitude_computed / 1000.0 self.cas2tas = math.sqrt( self.AirDensity(self.sea_level_pressure, self.standard_sea_level_temp) / self.AirDensity(self.static_pressure, self.temperature)) self.publish() print("PressureFactors: %g, %g" % (self.sea_level_pressure, self.cas2tas))
def ComputeHeadingRate(self): pos = self._sensors.Position() speed = self._sensors.GroundSpeed() if speed <= 0: speed = 10.0 turn_radius = ((360.0 * (speed / 60.0) / self.TurnRate) / (4 * util.M_PI)) turn_radius *= self.InterceptMultiplier intercept_heading = util.CourseHeading(pos, self.DesiredCourse, turn_radius) heading_error = intercept_heading - self._sensors.GroundTrack() if heading_error < -300: heading_error += 360 elif heading_error > 300: heading_error -= 360 ret = util.rate_curve(abs(heading_error), self.HeadingRateCurve) ret = ret if heading_error >= 0 else -ret #logger.debug("Heading rate: Ground Track = %g, error = %g, ret=%g", self._sensors.GroundTrack(), heading_error, ret) return ret
def updated(self, channel): if channel == 'pressuresensors': self.timestamp = self.pressuresensors_updated if self.sea_level_pressure is not None and self.temperature is not None: if isinstance(self.pressure_calibration, dict): self.static_pressure = util.rate_curve( self.static_pressure, self.pressure_calibration['pressure_calibration']) self.altitude_computed = ( (pow(self.sea_level_pressure / self.static_pressure, 1 / PRESSURE_POWER) - 1) * (self.temperature + KELVIN_OFFSET)) / PRESSURE_DIVISOR self.altitude_computed *= util.FEET_METER self.publish() print("AltitudeComputed: %d" % self.altitude_computed) else: if self.temperature is None: print("AltitudeComputed: No temp data") if self.sea_level_pressure is None: print("AltitudeComputed: No sea level pressure data")
def updated(self, channel): if channel == 'knownaltitude': if self.temperature is not None: self.standard_sea_level_temp = self.temperature + 1.98 * self.known_altitude / 1000.0 elif channel == 'givenbarometer': self.sea_level_pressure = (self.given_barometer * KPA_INHG) if self.static_pressure is not None: if isinstance(self.pressure_calibration, dict): self.static_pressure = util.rate_curve( self.static_pressure, self.pressure_calibration['pressure_calibration']) if self.known_altitude is not None and self.static_pressure is not None and \ self.temperature is not None: self.sea_level_pressure = (self.static_pressure / pow( 1.0 - (self.known_altitude * util.METERS_FOOT * PRESSURE_DIVISOR / (self.temperature + KELVIN_OFFSET)), PRESSURE_POWER)) self.known_altitude = None if self.sea_level_pressure is not None and self.static_pressure is not None and \ self.temperature is not None: if self.standard_sea_level_temp is None: altitude_computed = ( (pow(self.sea_level_pressure / self.static_pressure, 1 / PRESSURE_POWER) - 1) * (self.temperature + KELVIN_OFFSET)) / PRESSURE_DIVISOR altitude_computed *= util.FEET_METER self.standard_sea_level_temp = self.temperature + 1.98 * altitude_computed / 1000.0 self.cas2tas = math.sqrt( self.AirDensity(self.sea_level_pressure, self.standard_sea_level_temp) / self.AirDensity(self.static_pressure, self.temperature)) self.publish() print("PressureFactors: %g, %g" % (self.sea_level_pressure, self.cas2tas))
def compute_roll(self, intercept_heading): heading_error = intercept_heading - self._sensors.GroundTrack() # Handle wrap around if heading_error > 180: heading_error -= 360 elif heading_error < -180: heading_error += 360 if self._force_turn_direction < 0: if heading_error <= 0: self._force_turn_direction = 0 else: while heading_error >= 0: heading_error -= 360 elif self._force_turn_direction > 0: if heading_error >= 0: self._force_turn_direction = 0 else: while heading_error <= 0: heading_error += 360 # heading rate change in degrees per second ret = util.rate_curve (heading_error, self.RollCurve) #logger.debug ("compute_roll: %g/%g-->%g error -->%g roll", self._sensors.GroundTrack(), # intercept_heading, heading_error, ret) return ret, heading_error
def get_roll_rate(self): return util.rate_curve(self.DesiredRoll - self.CurrentRoll, self.RollRateCurve)
def _find_pitch_limits(self, absolute_limits): if isinstance(absolute_limits,tuple): return absolute_limits roll = abs(self._sensors.Roll()) max_pitch = util.rate_curve(roll, absolute_limits) return -absolute_limits[0][1],max_pitch
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 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 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()