def start_experiment(self):
# save current state
mission.task.state.save(modes=True, circle=False, targets=False)
# read config
self.top_altitude = self.config_node.getFloat("top_agl_ft")
if self.top_altitude < 200: self.top_altitude = 200
self.bot_altitude = self.config_node.getFloat("bottom_agl_ft")
if self.bot_altitude < 100: self.bot_altitude = 100
self.pitch = self.config_node.getFloat("pitch_start_deg")
if self.pitch > 10: self.pitch = 10
if self.pitch < -20: self.pitch = -20
self.pitch_end = self.config_node.getFloat("pitch_end_deg")
if self.pitch_end > 10: self.pitch_end = 10
if self.pitch_end < -20: self.pitch_end = -20
self.pitch_incr = self.config_node.getFloat("pitch_increment")
if abs(self.pitch_incr) <= 0.01: self.pitch_incr = 1
# correct sign of increment value if needed
if self.pitch > self.pitch_end and self.pitch_incr > 0:
self.pitch_incr = -self.pitch_incr
if self.pitch < self.pitch_end and self.pitch_incr < 0:
self.pitch_incr = -self.pitch_incr
self.glide_node.setFloat("pitch", self.pitch)
# configure initial climb to top altitude
self.targets_node.setFloat("altitude_agl_ft", self.top_altitude)
fcsmode.set("basic+tecs")
self.running = True
comms.events.log("glide", "task started")
def activate(self):
self.active = True
# save existing state
mission.task.state.save(modes=True, circle=True, targets=True)
# set modes
fcsmode.set("basic+tecs")
self.nav_node.setString("mode", "circle")
comms.events.log("mission", "parametric path")
def activate(self):
self.active = True
# save current state
mission.task.state.save(modes=True, circle=True, targets=False)
self.update_parameters()
# set modes
fcsmode.set("basic+tecs")
self.nav_node.setString("mode", "circle")
comms.events.log("mission", "circle")
def activate(self):
self.active = True
# start with roll control only, we fix elevator to neutral until
# flight speeds come up and steer the rudder directly
if self.target_pitch_deg is None:
fcsmode.set("roll")
else:
fcsmode.set("roll+pitch")
self.targets_node.setFloat("pitch_deg", self.target_pitch_deg)
self.targets_node.setFloat("roll_deg", 0.0)
self.targets_node.setFloat("altitude_agl_ft", self.mission_agl_ft)
self.targets_node.setFloat("airspeed_kt", self.target_speed_kt)
def activate(self):
self.active = True
# save existing state
mission.task.state.save(modes=True)
# if not in the air, set a simple flight control mode that
# does not touch the throttle, and set throttle to idle.
if not self.task_node.getBool("is_airborne"):
fcsmode.set("basic")
self.engine_node.setFloat("throttle", 0.0)
comms.events.log("mission", "idle")
def activate(self):
self.active = True
# save existing state
mission.task.state.save(modes=True, targets=True)
# set modes
fcsmode.set("basic+tecs")
self.nav_node.setString('mode', 'route')
if self.alt_agl_ft > 0.1:
self.targets_node.setFloat('altitude_agl_ft', self.alt_agl_ft)
self.route_node.setString('follow_mode', 'leader')
self.route_node.setString('start_mode', 'first_wpt')
self.route_node.setString('completion_mode', 'loop')
comms.events.log('mission', 'route')
def update(self, dt):
if not self.active:
return False
# test for start enable
enable = self.glide_node.getBool("enable")
if enable and not self.last_enable:
self.start_experiment()
# test if enable switched off while running experiment
if self.running and not enable and self.last_enable:
self.end_experiment(abort=True)
if self.running:
# test for experiment finished
if self.pitch_incr < 0 and self.pitch < self.pitch_end - 0.01:
self.end_experiment()
if self.pitch_incr > 0 and self.pitch > self.pitch_end + 0.01:
self.end_experiment()
# monitor for state transitions
alt = self.pos_node.getFloat("altitude_agl_ft")
if alt < self.bot_altitude + 10:
if fcsmode.get() != "basic+tecs":
fcsmode.set("basic+tecs")
self.pitch += self.pitch_incr
comms.events.log("glide", "start climb")
if alt > self.top_altitude - 10:
if fcsmode.get() != "basic":
fcsmode.set("basic")
self.glide_node.setFloat("pitch", self.pitch)
self.targets_node.setFloat("pitch_deg", self.pitch)
self.engine_node.setFloat("throttle", 0.0)
comms.events.log("glide",
"start decent pitch = " + str(self.pitch))
self.glide_node.setBool("running", self.running)
self.last_enable = enable
def activate(self):
if not self.active:
# build the approach with the current property tree values
self.build_approach()
# Save existing state
mission.task.state.save(modes=True, circle=True, targets=True)
fcsmode.set("basic+tecs")
self.nav_node.setString("mode", "circle")
self.targets_node.setFloat(
"airspeed_kt", self.land_node.getFloat("approach_speed_kt"))
self.flight_node.setFloat("flaps_setpoint", self.flaps)
# start at the beginning of the route (in case we inherit a
# partially flown approach from earlier in the flight)
# approach_mgr.restart() # FIXME
self.circle_capture = False
self.gs_capture = False
self.flare = False
self.active = True
comms.events.log("mission", "land")
def update(self, dt):
if not self.active:
return False
self.glideslope_rad = self.land_node.getFloat("glideslope_deg") * d2r
self.extend_final_leg_m = self.land_node.getFloat("extend_final_leg_m")
self.alt_bias_ft = self.land_node.getFloat("altitude_bias_ft")
# add ability for pilot to bias the glideslope altitude using
# stick/elevator (negative elevator is up.)
self.alt_bias_ft += -self.pilot_node.getFloat("elevator") * 25.0
# compute minimum 'safe' altitude
safe_dist_m = math.pi * self.turn_radius_m + self.final_leg_m
safe_alt_ft = safe_dist_m * math.tan(self.glideslope_rad) * m2ft \
+ self.alt_bias_ft
# position on circle descent
circle_pos = 0
mode = self.nav_node.getString('mode')
if mode == 'circle':
# circle descent portion of the approach
pos_lon = self.pos_node.getFloat("longitude_deg")
pos_lat = self.pos_node.getFloat("latitude_deg")
center_lon = self.circle_node.getFloat("longitude_deg")
center_lat = self.circle_node.getFloat("latitude_deg")
# compute course and distance to center of target circle
(course_deg, rev_deg, cur_dist_m) = \
wgs84.geo_inverse( center_lat, center_lon, pos_lat, pos_lon )
# test for circle capture
if not self.circle_capture:
fraction = abs(cur_dist_m / self.turn_radius_m)
#print 'heading to circle:', err, fraction
if fraction > 0.80 and fraction < 1.20:
# within 20% of target circle radius, call the
# circle capture
comms.events.log("land", "descent circle capture")
self.circle_capture = True
# compute portion of circle remaining to tangent point
current_crs = course_deg + self.side * 90
if current_crs > 360.0: current_crs -= 360.0
if current_crs < 0.0: current_crs += 360.0
circle_pos = (self.final_heading_deg - current_crs) * self.side
if circle_pos < -180.0: circle_pos += 360.0
if circle_pos > 180.0: circle_pos -= 360.0
# print 'circle_pos:', self.orient_node.getFloat('groundtrack_deg'), current_crs, self.final_heading_deg, circle_pos
angle_rem_rad = math.pi
if self.circle_capture and circle_pos > -10:
# circling, captured circle, and within 180 degrees
# towards tangent point (or just slightly passed)
angle_rem_rad = circle_pos * math.pi / 180.0
# distance to edge of circle + remaining circumference of
# circle + final approach leg
self.dist_rem_m = (cur_dist_m - self.turn_radius_m) \
+ angle_rem_rad * self.turn_radius_m \
+ self.final_leg_m
# print 'circle:', self.dist_rem_m, self.turn_radius_m, self.final_leg_m, cur_dist_m
if self.circle_capture and self.gs_capture:
# we are on the circle and on the glide slope, lets
# look for our lateral exit point
if abs(circle_pos) <= 10.0:
comms.events.log("land", "transition to final")
self.nav_node.setString("mode", "route")
else:
# on final approach
if control.route.dist_valid:
self.dist_rem_m = self.route_node.getFloat("dist_remaining_m")
# compute glideslope/target elevation
alt_m = self.dist_rem_m * math.tan(self.glideslope_rad)
# print ' ', mode, "dist = %.1f alt = %.1f" % (self.dist_rem_m, alt_m)
# Compute target altitude.
cur_alt = self.pos_node.getFloat("altitude_agl_ft")
cur_target_alt = self.targets_node.getFloat("altitude_agl_ft")
new_target_alt = alt_m * m2ft + self.alt_bias_ft
# prior to glide slope capture, never allow target altitude
# lower than safe altitude
if not self.gs_capture:
# print 'safe:', safe_alt_ft, 'new:', new_target_alt
if new_target_alt < safe_alt_ft:
new_target_alt = safe_alt_ft
# We want to avoid wasting energy needlessly gaining altitude.
# Once the approach has started, never raise the target
# altitude.
if new_target_alt > cur_target_alt:
new_target_alt = cur_target_alt
self.targets_node.setFloat("altitude_agl_ft", new_target_alt)
# compute error metrics relative to ideal glide slope
alt_error_ft = cur_alt - (alt_m * m2ft + self.alt_bias_ft)
gs_error = math.atan2(alt_error_ft * ft2m, self.dist_rem_m) * r2d
#print "alt_error_ft = %.1f" % alt_error_ft, "gs err = %.1f" % gs_error
if self.circle_capture and not self.gs_capture:
# on the circle, but haven't intercepted gs
#print 'waiting for gs intercept'
if gs_error <= 1.0 and circle_pos >= 0:
# 1 degree or less glide slope error and on the 2nd
# half of the circle, call the gs captured
comms.events.log("land", "glide slope capture")
self.gs_capture = True
# compute time to touchdown at current ground speed (assuming the
# navigation system has lined us up properly
ground_speed_ms = self.vel_node.getFloat("groundspeed_ms")
if ground_speed_ms > 0.01:
seconds_to_touchdown = self.dist_rem_m / ground_speed_ms
else:
seconds_to_touchdown = 1000.0 # lots
#print "dist_rem_m = %.1f gs = %.1f secs = %.1f" % \
# (self.dist_rem_m, ground_speed_ms, seconds_to_touchdown)
# approach_speed_kt = approach_speed_node.getFloat()
self.flare_pitch_deg = self.land_node.getFloat("flare_pitch_deg")
self.flare_seconds = self.land_node.getFloat("flare_seconds")
if seconds_to_touchdown <= self.flare_seconds and not self.flare:
# within x seconds of touchdown horizontally. Note these
# are padded numbers because we don't know the truth
# exactly ... we could easily be closer or lower or
# further or higher. Our flare strategy is to smoothly
# pull throttle to idle, while smoothly pitching to the
# target flare pitch (as configured in the task
# definition.)
comms.events.log("land", "start flare")
self.flare = True
self.flare_start_time = self.imu_node.getFloat("timestamp")
self.approach_throttle = self.engine_node.getFloat("throttle")
self.approach_pitch = self.targets_node.getFloat("pitch_deg")
self.flare_pitch_range = self.approach_pitch - self.flare_pitch_deg
fcsmode.set("basic")
if self.flare:
if self.flare_seconds > 0.01:
elapsed = self.imu_node.getFloat(
"timestamp") - self.flare_start_time
percent = elapsed / self.flare_seconds
if percent > 1.0:
percent = 1.0
self.targets_node.setFloat(
"pitch_deg",
self.approach_pitch - percent * self.flare_pitch_range)
self.engine_node.setFloat(
"throttle", self.approach_throttle * (1.0 - percent))
#printf("FLARE: elapsed=%.1f percent=%.2f speed=%.1f throttle=%.1f",
# elapsed, percent,
# approach_speed_kt - percent * self.flare_pitch_range,
# self.approach_throttle * (1.0 - percent))
else:
# printf("FLARE!!!!\n")
self.targets_node.setFloat("pitch_deg", self.flare_pitch_deg)
self.engine_node.setFloat("throttle", 0.0)
def update(self, dt):
if not self.active:
return False
throttle_time_sec = 2.0 # hard code for now (fixme: move to config)
feather_time = 5.0 # fixme: make this a configurable option
is_airborne = self.task_node.getBool("is_airborne")
# For wheeled take offs, track relative heading (initialized to
# zero) when autopilot mode is engaged and steer that error to
# zero with the rudder until flying/climbing
if self.ap_node.getBool("master_switch"):
if not self.last_ap_master:
# reset states when engaging AP mode
self.relhdg = 0.0
self.control_limit = 1.0
self.flight_node.setFloat("flaps_setpoint", self.flaps)
if not is_airborne:
# if engaging on the ground, start with zero throttle
self.engine_node.setFloat("throttle", 0.0)
if not is_airborne:
# run up throttle over the specified interval
throttle = self.engine_node.getFloat("throttle")
throttle += dt / throttle_time_sec
if throttle > 1.0:
throttle = 1.0
self.engine_node.setFloat("throttle", throttle)
# estimate short term heading
self.relhdg += self.imu_node.getFloat("r_rad_sec") * r2d * dt
# I am not clamping heading to +/- 180 here. The
# rational is that if we turn more than 180 beyond our
# starting heading we are probably majorly screwed up,
# but even so, let's unwind back the direction from
# where we came rather than doing a hard-over reversal
# of the rudder back to the other direction even if it
# would be a shorter turning direction. Less chance
# of upsetting the apple cart with a massive control
# input change.
# if airborne, then slowly feather our max steer_limit
# to zero over the period of 2 seconds. This avoids a
# hard rudder snap to zero if we are off heading, but
# the assumption is once we are airborne we'll prefer
# to keep our wings level and fly the current heading
# rather than fight with our rudder and add drag to
# get back on the heading.
if is_airborne and self.control_limit > 0:
delta = (1.0 / feather_time) * dt
self.control_limit -= delta
if self.control_limit < 0.0:
self.control_limit = 0.0
if self.rudder_enable:
# simple proportional controller to steer against
# (estimated) heading error
rudder = self.relhdg * self.rudder_gain
steer_limit = self.rudder_max * self.control_limit
if rudder < -steer_limit:
rudder = -steer_limit
if rudder > steer_limit:
rudder = steer_limit
self.flight_node.setFloat("rudder", rudder)
roll = -self.relhdg * self.roll_gain
if roll < -self.roll_limit:
roll = -self.roll_limit
if roll > self.roll_limit:
roll = self.roll_limit
self.targets_node.setFloat("roll_deg", roll)
if is_airborne or \
(self.vel_node.getFloat("airspeed_kt") > self.target_speed_kt):
# we are flying or we've reached our flying/climbout
# airspeed: switch to normal flight mode
if fcsmode.get() != "basic+tecs":
fcsmode.set("basic+tecs")
self.last_ap_master = self.ap_node.getBool("master_switch")
