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()
