def dribble(reset=False):
global wp_counter
global dist_valid
if reset:
wp_counter = 0
dist_valid = False
# dribble active route into the active_node tree (one waypoint
# per interation to keep the load consistent and light.)
route_size = len(active_route)
active_route_node.setInt('route_size', route_size)
if route_size > 0:
if wp_counter >= route_size:
wp_counter = 0
dist_valid = True
wp = active_route[wp_counter]
wp_str = 'wpt[%d]' % wp_counter
wp_node = active_route_node.getChild(wp_str, True)
wp_node.setFloat("longitude_deg", wp.lon_deg)
wp_node.setFloat("latitude_deg", wp.lat_deg)
if wp_counter < route_size - 1:
# compute leg course and distance
next = active_route[wp_counter+1]
(leg_course, rev_course, leg_dist) = \
wgs84.geo_inverse( wp.lat_deg, wp.lon_deg,
next.lat_deg, next.lon_deg )
wp.leg_dist_m = leg_dist
wp_counter += 1
def geod2cart(ref, geod_points):
print('geod2cart()')
result = []
for p in geod_points:
# expects points as [lat, lon]
#heading, dist = gc.course_and_dist( [ref.y, ref.x], [p.y, p.x] )
(heading, reverse, dist) = \
wgs84.geo_inverse( ref.y, ref.x, p.y, p.x )
angle = (90 - heading) * d2r
x = math.cos(angle) * dist
y = math.sin(angle) * dist
print(p.pretty(), 'hdg:', heading, 'dist:', dist, 'cart:', x, y)
result.append(point.Point(x, y))
return result
def update(self, dt):
if not self.active:
return False
if not self.home_node.getBool("valid"):
if self.gps_node.getFloat("gps_age") < 1.0 and \
self.gps_node.getBool("settle"):
# Save current position as startup position
self.startup_node.setFloat(
"longitude_deg", self.gps_node.getFloat("longitude_deg"))
self.startup_node.setFloat(
"latitude_deg", self.gps_node.getFloat("latitude_deg"))
self.startup_node.setFloat(
"altitude_m", self.gps_node.getFloat("altitude_m"))
self.startup_node.setBool("valid", True)
# Set initial "home" position.
self.home_node.setFloat(
"longitude_deg", self.gps_node.getFloat("longitude_deg"))
self.home_node.setFloat("latitude_deg",
self.gps_node.getFloat("latitude_deg"))
self.home_node.setFloat("altitude_m",
self.gps_node.getFloat("altitude_m"))
self.home_node.setFloat("azimuth_deg", 0.0)
self.home_node.setBool("valid", True)
else:
current = (self.pos_node.getFloat("latitude_deg"),
self.pos_node.getFloat("longitude_deg"))
home = (self.home_node.getFloat("latitude_deg"),
self.home_node.getFloat("longitude_deg"))
(course_deg, rev_deg, dist_m) = \
wgs84.geo_inverse( self.pos_node.getFloat("latitude_deg"),
self.pos_node.getFloat("longitude_deg"),
self.home_node.getFloat("latitude_deg"),
self.home_node.getFloat("longitude_deg") )
self.home_node.setFloat("course_deg", course_deg)
self.home_node.setFloat("dist_m", dist_m)
# a mini cartesian (2d) system relative to home in meters
# FIXME: the parametric task is the only thing that uses this
# so why not put this code over into the parametric.py module?
theta = rev_deg * d2r
x = math.sin(theta) * dist_m
y = math.cos(theta) * dist_m
self.home_node.setFloat("x_m", x)
self.home_node.setFloat("y_m", y)
return True
print("dt:", time_str)
date_time_obj = datetime.datetime.strptime(main_str, '%Y-%m-%d %H:%M:%S')
unix_sec = float(date_time_obj.strftime('%s')) + fraction
print("from local:", unix_sec)
record = djicsv.query(unix_sec)
roll = record['roll']
pitch = record['pitch']
yaw = record['yaw']
if yaw < 0: yaw += 360.0
if abs(lat_deg) < 0.001 and abs(lon_deg) < 0.001:
continue
write_frame = False
# by distance camera has moved
(c1, c2, dist_m) = wgs84.geo_inverse(lat_deg, lon_deg, last_lat, last_lon)
print("dist:", dist_m)
#if time >= last_time + args.interval and dist_m >= args.distance:
if args.distance and dist_m >= args.distance:
write_frame = True
# by visual overlap
method = cv2.INTER_AREA
frame_scale = cv2.resize(frame, (0,0), fx=scale, fy=scale,
interpolation=method)
cv2.imshow('frame', frame_scale)
gray = cv2.cvtColor(frame_scale, cv2.COLOR_BGR2GRAY)
(h, w) = gray.shape
kp_list = detector.detect(gray)
kp_list, des_list = detector.compute(gray, kp_list)
if not (des_list_ref is None) and not (des_list is None) and len(des_list_ref) and len(des_list):
def update(dt):
direction_str = circle_node.getString("direction")
direction = 1.0
if direction_str == "right":
direction = -1.0
elif direction_str == "left":
direction = 1.0
else:
circle_node.setString("direction", "left")
if pos_node.hasChild("longitude_deg") and \
pos_node.hasChild("latitude_deg"):
pos_lon = pos_node.getFloat("longitude_deg")
pos_lat = pos_node.getFloat("latitude_deg")
else:
# no valid current position, bail out.
return
if circle_node.hasChild("longitude_deg") and \
circle_node.hasChild("latitude_deg"):
# we have a valid circle center
center_lon = circle_node.getFloat("longitude_deg")
center_lat = circle_node.getFloat("latitude_deg")
else:
# we have a valid position, but no valid circle center, use
# current position. (sanity fallback)
circle_node.setFloat("longitude_deg", pos_lon)
circle_node.setFloat("latitude_deg", pos_lat)
circle_node.setFloat("radius_m", 100.0)
circle_node.setString("direction", "left")
center_lon = pos_lon
center_lat = pos_lat
# compute course and distance to center of target circle
# fixme: should reverse this and direction sense to match 'land.py' and make more sense
(course_deg, rev_deg, dist_m) = \
wgs84.geo_inverse( pos_lat, pos_lon, center_lat, center_lon )
# compute ideal ground course to be on the circle perimeter if at
# ideal radius
ideal_crs = course_deg + direction * 90
if ideal_crs > 360.0: ideal_crs -= 360.0
if ideal_crs < 0.0: ideal_crs += 360.0
# (in)sanity check
if circle_node.hasChild("radius_m"):
radius_m = circle_node.getFloat("radius_m")
if radius_m < 35: radius_m = 35
else:
radius_m = 100
circle_node.setFloat("radius_m", 100.0)
# compute a target ground course based on our actual radius
# distance
target_crs = ideal_crs
if dist_m < radius_m:
# inside circle, adjust target heading to expand our
# circling radius
offset_deg = direction * 90.0 * (1.0 - dist_m / radius_m)
target_crs += offset_deg
elif dist_m > radius_m:
# outside circle, adjust target heading to tighten our
# circling radius
offset_dist = dist_m - radius_m
if offset_dist > radius_m: offset_dist = radius_m
offset_deg = direction * 90 * offset_dist / radius_m
target_crs -= offset_deg
if target_crs > 360.0: target_crs -= 360.0
if target_crs < 0.0: target_crs += 360.0
targets_node.setFloat("groundtrack_deg", target_crs)
# L1 'mathematical' response to error
L1_period = L1_node.getFloat("period") # gain
gs_mps = vel_node.getFloat("groundspeed_ms")
omegaA = sqrt_of_2 * math.pi / L1_period
VomegaA = gs_mps * omegaA
course_error = orient_node.getFloat("groundtrack_deg") - target_crs
# wrap to +/- 180
if course_error < -180.0: course_error += 360.0
if course_error > 180.0: course_error -= 360.0
# clamp to +/-90
if course_error < -90.0: course_error = -90.0
if course_error > 90.0: course_error = 90.0
targets_node.setFloat("course_error_deg", course_error)
# accel: is the lateral acceleration we need to compensate for
# heading error
accel = 2.0 * math.sin(course_error * d2r) * VomegaA
# circling acceleration needed for our current distance from center
if dist_m > 0.1:
turn_accel = direction * gs_mps * gs_mps / dist_m
else:
turn_accel = 0.0
# allow a crude fudge factor for non-straight airframes or imu
# mounting errors. This is essentially the bank angle that yields
# zero turn rate
bank_bias_deg = L1_node.getFloat("bank_bias_deg")
# compute desired acceleration = acceleration required for course
# correction + acceleration required to maintain turn at current
# distance from center.
total_accel = accel + turn_accel
target_bank = -math.atan(total_accel / gravity)
target_bank_deg = target_bank * r2d + bank_bias_deg
bank_limit_deg = L1_node.getFloat("bank_limit_deg")
if target_bank_deg < -bank_limit_deg + bank_bias_deg:
target_bank_deg = -bank_limit_deg + bank_bias_deg
if target_bank_deg > bank_limit_deg + bank_bias_deg:
target_bank_deg = bank_limit_deg + bank_bias_deg
targets_node.setFloat("roll_deg", target_bank_deg)
route_node.setFloat("wp_dist_m", dist_m)
if gs_mps > 0.1:
route_node.setFloat("wp_eta_sec", dist_m / gs_mps)
else:
route_node.setFloat("wp_eta_sec", 0.0)
def make_pix4d(image_dir, force_altitude=None, force_heading=None, yaw_from_groundtrack=False):
if not force_altitude and camera.camera_node.getString("make") == "DJI" and camera.camera_node.getString("model") in ["FC330", "FC6310", "FC6310S"]:
# test for Phantom 4 Pro v2.0 camera which lies about it's altitude
log("Detected these images are from a Phantom 4 which lies about it's")
log("altitude. Please rerun the script with the --force-altitude option to")
log("override the incorrect goetag altitude with your best estimate of the")
log("true gps altitude of the flight altitude (in meters).")
log("Sorry for the inconvenience!")
quit()
# load list of images
files = []
for file in os.listdir(image_dir):
if fnmatch.fnmatch(file, "*.jpg") or fnmatch.fnmatch(file, "*.JPG"):
files.append(file)
files.sort()
# save some work if true
images_have_yaw = False
images = []
# read image exif timestamp (and convert to unix seconds)
for file in files:
full_name = os.path.join(image_dir, file)
# print(full_name)
lon_deg, lat_deg, alt_m, unixtime, yaw_deg, pitch_deg, roll_deg = exif.get_pose(full_name)
line = [file, lat_deg, lon_deg]
if not force_altitude:
line.append(alt_m)
else:
line.append(force_altitude)
if roll_deg is None:
line.append(0) # assume zero roll
else:
line.append(roll_deg)
if pitch_deg is None:
line.append(0) # assume zero pitch
else:
line.append(pitch_deg)
if force_heading is not None:
line.append(force_heading)
elif yaw_deg is not None:
images_have_yaw = True
line.append(yaw_deg)
else:
# no yaw info found in metadata
line.append(0)
images.append(line)
if (not force_heading and not images_have_yaw) or yaw_from_groundtrack:
print("do yaw from ground track")
# do extra work to estimate yaw heading from gps ground track
from rcUAS import wgs84
for i in range(len(images)):
if i > 0:
prev = images[i-1]
else:
prev = None
cur = images[i]
if i < len(images)-1:
next = images[i+1]
else:
next = None
if not prev is None:
(prev_hdg, rev_course, prev_dist) = \
wgs84.geo_inverse( prev[1], prev[2], cur[1], cur[2] )
else:
prev_hdg = 0.0
prev_dist = 0.0
if not next is None:
(next_hdg, rev_course, next_dist) = \
wgs84.geo_inverse( cur[1], cur[2], next[1], next[2] )
else:
next_hdg = 0.0
next_dist = 0.0
prev_hdgx = math.cos(prev_hdg*d2r)
prev_hdgy = math.sin(prev_hdg*d2r)
next_hdgx = math.cos(next_hdg*d2r)
next_hdgy = math.sin(next_hdg*d2r)
avg_hdgx = (prev_hdgx*prev_dist + next_hdgx*next_dist) / (prev_dist + next_dist)
avg_hdgy = (prev_hdgy*prev_dist + next_hdgy*next_dist) / (prev_dist + next_dist)
avg_hdg = math.atan2(avg_hdgy, avg_hdgx)*r2d
if avg_hdg < 0:
avg_hdg += 360.0
#print("%d %.2f %.1f %.2f %.1f %.2f" % (i, prev_hdg, prev_dist, next_hdg, next_dist, avg_hdg))
images[i][6] = avg_hdg
# sanity check
output_file = os.path.join(image_dir, "pix4d.csv")
if os.path.exists(output_file):
log(output_file, "exists, please rename it and rerun this script.")
quit()
log("Creating pix4d image pose file:", output_file, "images:", len(files))
# traverse the image list and create output csv file
with open(output_file, 'w') as csvfile:
writer = csv.DictWriter( csvfile,
fieldnames=["File Name",
"Lat (decimal degrees)",
"Lon (decimal degrees)",
"Alt (meters MSL)",
"Roll (decimal degrees)",
"Pitch (decimal degrees)",
"Yaw (decimal degrees)"] )
writer.writeheader()
for line in images:
image = line[0]
lat_deg = line[1]
lon_deg = line[2]
alt_m = line[3]
roll_deg = line[4]
pitch_deg = line[5]
yaw_deg = line[6]
writer.writerow( { "File Name": os.path.basename(image),
"Lat (decimal degrees)": "%.10f" % lat_deg,
"Lon (decimal degrees)": "%.10f" % lon_deg,
"Alt (meters MSL)": "%.2f" % alt_m,
"Roll (decimal degrees)": "%.2f" % roll_deg,
"Pitch (decimal degrees)": "%.2f" % pitch_deg,
"Yaw (decimal degrees)": "%.2f" % yaw_deg } )
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(dt):
global current_wp
global acquired
reposition()
nav_course = 0.0
nav_dist_m = 0.0
direct_dist = 0.0
request = route_node.getString('route_request')
if len(request):
result = ''
if build_str(request):
swap()
reposition(force=True)
result = 'success: ' + request
dribble(reset=True)
else:
result = 'failed: ' + request
route_node.setString('request_result', result)
route_node.setString('route_request', '')
route_node.setInt('route_size', len(active_route))
if len(active_route) > 0:
if gps_node.getFloat("data_age") < 10.0:
# track current waypoint of route (only!) if we have
# recent gps data
# route start up logic: if start_mode == first_wpt
# then there is nothing to do, we simply continue to
# track wpt 0 if that is the current waypoint. If
# start_mode == 'first_leg', then if we are tracking
# wpt 0, increment it so we track the 2nd waypoint
# along the first leg. If only a 1 point route is
# given along with first_leg startup behavior, then
# don't do that again, force some sort of sane route
# parameters instead!
start_mode = route_node.getString('start_mode')
if start_mode == 'first_leg' and current_wp == 0:
if len(active_route) > 1:
current_wp += 1
else:
route_node.setString('start_mode', 'first_wpt')
route_node.setString('follow_mode', 'direct')
L1_period = L1_node.getFloat('period')
L1_damping = L1_node.getFloat('damping')
gs_mps = vel_node.getFloat('groundspeed_ms')
groundtrack_deg = orient_node.getFloat('groundtrack_deg')
tas_kt = wind_node.getFloat("true_airspeed_kt")
tas_mps = tas_kt * kt2mps
prev = get_previous_wp()
wp = get_current_wp()
# compute direct-to course and distance
pos_lon = pos_node.getFloat("longitude_deg")
pos_lat = pos_node.getFloat("latitude_deg")
(direct_course, rev_course, direct_dist) = \
wgs84.geo_inverse( pos_lat, pos_lon, wp.lat_deg, wp.lon_deg )
#print pos_lat, pos_lon, ":", wp.lat_deg, wp.lon_deg
#print ' course to:', direct_course, 'dist:', direct_dist
# compute leg course and distance
(leg_course, rev_course, leg_dist) = \
wgs84.geo_inverse( prev.lat_deg, prev.lon_deg,
wp.lat_deg, wp.lon_deg )
#print prev.lat_deg, prev.lon_deg, " ", wp.lat_deg, wp.lon_deg
#print ' leg course:', leg_course, 'dist:', leg_dist
# difference between ideal (leg) course and direct course
angle = leg_course - direct_course
if angle < -180.0: angle += 360.0
elif angle > 180.0: angle -= 360.0
# compute cross-track error
angle_rad = angle * d2r
xtrack_m = math.sin(angle_rad) * direct_dist
dist_m = math.cos(angle_rad) * direct_dist
# print("lc: %.1f dc: %.1f a: %.1f xc: %.1f dd: %.1f" % (leg_course, direct_course, angle, xtrack_m, direct_dist))
route_node.setFloat( 'xtrack_dist_m', xtrack_m )
route_node.setFloat( 'projected_dist_m', dist_m )
# default distance for waypoint acquisition = direct
# distance to the target waypoint. This can be
# overridden later by leg following and replaced with
# distance remaining along the leg.
nav_dist_m = direct_dist
follow_mode = route_node.getString('follow_mode')
completion_mode = route_node.getString('completion_mode')
if follow_mode == 'direct':
# steer direct to
nav_course = direct_course
elif follow_mode == 'xtrack_direct_hdg':
# cross track steering depricated. See
# route_mgr.cxx in the historical archives for
# reference code.
pass
elif follow_mode == 'xtrack_leg_hdg':
# cross track steering depricated. See
# route_mgr.cxx in the historical archives for
# reference code.
pass
elif follow_mode == 'leader':
# scale our L1_dist (something like a target heading
# gain) proportional to ground speed
L1_dist = (1.0 / math.pi) * L1_damping * L1_period * gs_mps
wangle = 0.0
if L1_dist < 1.0:
# ground really small or negative (problem?!?)
L1_dist = 1.0
if L1_dist <= abs(xtrack_m):
# beyond L1 distance, steer as directly toward
# leg as allowed
wangle = 0.0
else:
# steer towards imaginary point projected onto
# the route leg L1_distance ahead of us
wangle = math.acos(abs(xtrack_m) / L1_dist) * r2d
if wangle < 30.0: wangle = 30.0
if xtrack_m > 0.0:
nav_course = direct_course + angle - 90.0 + wangle
else:
nav_course = direct_course + angle + 90.0 - wangle
# print("x: %.1f dc: %.1f a: %.1f wa: %.1f nc: %.1f" % (xtrack_m, direct_course, angle, wangle, nav_course))
if acquired:
nav_dist_m = dist_m
else:
# direct to first waypoint until we've
# acquired this route
nav_course = direct_course
nav_dist_m = direct_dist
# printf('direct=%.1f angle=%.1f nav=%.1f L1=%.1f xtrack=%.1f wangle=%.1f nav_dist=%.1f\n', direct_course, angle, nav_course, L1_dist, xtrack_m, wangle, nav_dist_m)
gs_mps = vel_node.getFloat('groundspeed_ms')
if gs_mps > 0.1 and abs(nav_dist_m) > 0.1:
wp_eta_sec = nav_dist_m / gs_mps
else:
wp_eta_sec = 99.0 # just any sorta big value
route_node.setFloat( 'wp_eta_sec', dist_m / gs_mps )
route_node.setFloat( 'wp_dist_m', direct_dist )
if nav_course < 0.0: nav_course += 360.0
if nav_course > 360.0: nav_course -= 360.0
targets_node.setFloat( 'groundtrack_deg', nav_course )
# allow a crude fudge factor for non-straight airframes or
# imu mounting errors. This is essentially the bank angle
# that yields zero turn rate
bank_bias_deg = L1_node.getFloat("bank_bias_deg");
# target bank angle computed here
target_bank_deg = 0.0
# heading error is computed with wind triangles so this is
# the actual body heading error, not the ground track
# error, thus Vomega is computed with tas_mps, not gs_mps
omegaA = sqrt_of_2 * math.pi / L1_period
#VomegaA = gs_mps * omegaA
#course_error = orient_node.getFloat('groundtrack_deg') \
# - nav_course
VomegaA = tas_mps * omegaA
# print 'gt:', groundtrack_deg, 'nc:', nav_course, 'error:', groundtrack_deg - nav_course
hdg_error = wind_heading_error(groundtrack_deg, nav_course)
# clamp to +/-90 so we still get max turn input when flying directly away from the heading.
if hdg_error < -90.0: hdg_error = -90.0
if hdg_error > 90.0: hdg_error = 90.0
targets_node.setFloat( 'wind_heading_error_deg', hdg_error )
accel = 2.0 * math.sin(hdg_error * d2r) * VomegaA
target_bank_deg = -math.atan( accel / gravity )*r2d + bank_bias_deg
bank_limit_deg = L1_node.getFloat('bank_limit_deg')
if target_bank_deg < -bank_limit_deg + bank_bias_deg:
target_bank_deg = -bank_limit_deg + bank_bias_deg
if target_bank_deg > bank_limit_deg + bank_bias_deg:
target_bank_deg = bank_limit_deg + bank_bias_deg
targets_node.setFloat( 'roll_deg', target_bank_deg )
# estimate distance remaining to completion of route
if dist_valid:
dist_remaining_m = nav_dist_m + \
get_remaining_distance_from_next_waypoint()
route_node.setFloat('dist_remaining_m', dist_remaining_m)
#if comms_node.getBool('display_on'):
# print 'next leg: %.1f to end: %.1f wpt=%d of %d' % (nav_dist_m, dist_remaining_m, current_wp, len(active_route))
# logic to mark completion of leg and move to next leg.
if completion_mode == 'loop':
if wp_eta_sec < 1.0:
acquired = True
increment_current_wp()
elif completion_mode == 'circle_last_wpt':
if wp_eta_sec < 1.0:
acquired = True
if current_wp < len(active_route) - 1:
increment_current_wp()
else:
wp = get_current()
# FIXME: NEED TO GO TO CIRCLE MODE HERE SOME HOW!!!
#mission_mgr.request_task_circle(wp.get_target_lon(),
# wp.get_target_lat(),
# 0.0, 0.0)
elif completion_mode == 'extend_last_leg':
if wp_eta_sec < 1.0:
acquired = True
if current_wp < len(active_route) - 1:
increment_current_wp()
else:
# follow the last leg forever
pass
# publish current target waypoint
route_node.setInt('target_waypoint_idx', current_wp)
# if ( display_on ) {
# printf('route dist = %0f\n', dist_remaining_m)
# }
else:
pass
# FIXME: we've been commanded to follow a route, but no
# route has been defined.
# We are in ill-defined territory, should we do some sort
# of circle of our home position?
# FIXME: need to go to circle mode somehow here!!!!
# mission_mgr.request_task_circle()
# dribble active route into property tree
dribble()