def find_spawns(loc, offsets):
lat, lon, alt, heading = loc
spawns = {}
for k in offsets:
(x, y, z, head) = offsets[k]
if head is None:
head = heading
g = mavextra.gps_offset(lat, lon, x, y)
spawns[k] = ",".join([str(g[0]), str(g[1]), str(alt + z), str(head)])
return spawns
def find_new_spawn(loc, file_path):
(lat, lon, alt, heading) = loc.split(",")
swarminit_filepath = os.path.join(autotest_dir, "swarminit.txt")
for path2 in [file_path, swarminit_filepath]:
if os.path.isfile(path2):
with open(path2, 'r') as swd:
next(swd)
for lines in swd:
if len(lines) == 0:
continue
(instance, offset) = lines.split("=")
if ((int)(instance) == (int)(cmd_opts.instance)):
(x, y, z, head) = offset.split(",")
g = mavextra.gps_offset((float)(lat), (float)(lon), (float)(x), (float)(y))
loc = str(g[0])+","+str(g[1])+","+str(alt+z)+","+str(head)
return loc
g = mavextra.gps_newpos((float)(lat), (float)(lon), 90, 20*(int)(cmd_opts.instance))
loc = str(g[0])+","+str(g[1])+","+str(alt)+","+str(heading)
return loc
def find_new_spawn(loc, file_path):
(lat, lon, alt, heading) = loc.split(",")
swarminit_filepath = os.path.join(find_autotest_dir(), "swarminit.txt")
for path2 in [file_path, swarminit_filepath]:
if os.path.isfile(path2):
with open(path2, 'r') as swd:
next(swd)
for lines in swd:
if len(lines) == 0:
continue
(instance, offset) = lines.split("=")
if ((int)(instance) == (int)(cmd_opts.instance)):
(x, y, z, head) = offset.split(",")
g = mavextra.gps_offset((float)(lat), (float)(lon), (float)(x), (float)(y))
loc = str(g[0])+","+str(g[1])+","+str(alt+z)+","+str(head)
return loc
g = mavextra.gps_newpos((float)(lat), (float)(lon), 90, 20*(int)(cmd_opts.instance))
loc = str(g[0])+","+str(g[1])+","+str(alt)+","+str(heading)
return loc
def gps_input_send(self, time, pos_ned, yaw, gps_vel):
time_us = int(time * 1.0e6)
gps_lat, gps_lon = mavextra.gps_offset(self.vicon_settings.origin_lat,
self.vicon_settings.origin_lon,
pos_ned.y, pos_ned.x)
gps_alt = self.vicon_settings.origin_alt - pos_ned.z
gps_week, gps_week_ms = get_gps_time(time)
if self.vicon_settings.gps_nsats >= 6:
fix_type = 3
else:
fix_type = 1
yaw_cd = int(mavextra.wrap_360(math.degrees(yaw)) * 100)
if yaw_cd == 0:
# the yaw extension to GPS_INPUT uses 0 as no yaw support
yaw_cd = 36000
self.master.mav.gps_input_send(time_us, 0, 0, gps_week_ms, gps_week,
fix_type, int(gps_lat * 1.0e7),
int(gps_lon * 1.0e7), gps_alt, 1.0, 1.0,
gps_vel.x, gps_vel.y, gps_vel.z, 0.2,
1.0, 1.0, self.vicon_settings.gps_nsats,
yaw_cd)
def main_loop():
'''loop getting vicon data'''
global vicon
name = vicon.get_subject_name(0)
segname = vicon.get_subject_root_segment_name(name)
last_msg_time = time.time()
last_gps_pos = None
now = time.time()
last_origin_send = now
now_ms = int(now * 1000)
last_gps_send_ms = now_ms
last_gps_send = now
gps_period_ms = 1000 // args.gps_rate
while True:
if args.send_zero:
time.sleep(0.05)
else:
vicon.get_frame()
now = time.time()
now_ms = int(now * 1000)
if args.rate is not None:
dt = now - last_msg_time
if dt < 1.0 / args.rate:
continue
last_msg_time = now
# get position as ENU mm
if args.send_zero:
pos_enu = [0.0, 0.0, 0.0]
else:
pos_enu = vicon.get_segment_global_translation(name, segname)
if pos_enu is None:
continue
# convert to NED meters
pos_ned = [pos_enu[0] * 0.001, pos_enu[1] * 0.001, -pos_enu[2] * 0.001]
# get orientation in euler, converting to ArduPilot conventions
if args.send_zero:
quat = [0.0, 0.0, 0.0, 1.0]
else:
quat = vicon.get_segment_global_quaternion(name, segname)
q = Quaternion([quat[3], quat[0], quat[1], quat[2]])
euler = q.euler
roll, pitch, yaw = euler[2], euler[1], euler[0]
yaw -= math.pi * 0.5
yaw_cd = int(mavextra.wrap_360(math.degrees(yaw)) * 100)
if yaw_cd == 0:
# the yaw extension to GPS_INPUT uses 0 as no yaw support
yaw_cd = 36000
if args.debug > 0:
print("Pose: [%.3f, %.3f, %.3f] [%.2f %.2f %.2f]" %
(pos_ned[0], pos_ned[1], pos_ned[2], math.degrees(roll),
math.degrees(pitch), math.degrees(yaw)))
time_us = int(now * 1.0e6)
if now - last_origin_send > 1 and not args.gps_only:
# send a heartbeat msg
mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS,
mavutil.mavlink.MAV_AUTOPILOT_GENERIC, 0, 0,
0)
# send origin at 1Hz
mav.mav.set_gps_global_origin_send(args.target_sysid,
int(origin[0] * 1.0e7),
int(origin[1] * 1.0e7),
int(origin[2] * 1.0e3), time_us)
last_origin_send = now
if args.gps_rate > 0 and now_ms - last_gps_send_ms > gps_period_ms:
'''send GPS data at the specified rate, trying to align on the given period'''
if last_gps_pos is None:
last_gps_pos = pos_ned
gps_lat, gps_lon = mavextra.gps_offset(origin[0], origin[1],
pos_ned[1], pos_ned[0])
gps_alt = origin[2] - pos_ned[2]
gps_dt = now - last_gps_send
gps_vel = [(pos_ned[0] - last_gps_pos[0]) / gps_dt,
(pos_ned[1] - last_gps_pos[1]) / gps_dt,
(pos_ned[2] - last_gps_pos[2]) / gps_dt]
last_gps_pos = pos_ned
gps_week, gps_week_ms = get_gps_time(now)
if args.gps_nsats >= 6:
fix_type = 3
else:
fix_type = 1
mav.mav.gps_input_send(time_us, 0, 0, gps_week_ms, gps_week,
fix_type, int(gps_lat * 1.0e7),
int(gps_lon * 1.0e7), gps_alt, 1.0, 1.0,
gps_vel[0], gps_vel[1], gps_vel[2], 0.2,
1.0, 1.0, args.gps_nsats, yaw_cd)
last_gps_send_ms = (now_ms // gps_period_ms) * gps_period_ms
last_gps_send = now
# send VISION_POSITION_ESTIMATE
if not args.gps_only:
# we force mavlink1 to avoid the covariances which seem to make the packets too large
# for the mavesp8266 wifi bridge
mav.mav.global_vision_position_estimate_send(time_us,
pos_ned[0],
pos_ned[1],
pos_ned[2],
roll,
pitch,
yaw,
force_mavlink1=True)
def thread_loop(self):
'''background processing'''
name = None
last_pos = None
last_frame_num = None
while True:
vicon = self.vicon
if vicon is None:
time.sleep(0.1)
continue
if not name:
vicon.get_frame()
name = vicon.get_subject_name(0)
if name is None:
continue
segname = vicon.get_subject_root_segment_name(name)
print("Connected to subject '%s' segment '%s'" %
(name, segname))
last_msg_time = time.time()
last_gps_pos = None
now = time.time()
last_origin_send = now
now_ms = int(now * 1000)
last_gps_send_ms = now_ms
last_gps_send = now
frame_rate = vicon.get_frame_rate()
frame_dt = 1.0 / frame_rate
last_rate = time.time()
frame_count = 0
print("Vicon frame rate %.1f" % frame_rate)
if self.vicon_settings.gps_rate > 0:
gps_period_ms = 1000 // self.vicon_settings.gps_rate
time.sleep(0.01)
vicon.get_frame()
now = time.time()
now_ms = int(now * 1000)
frame_num = vicon.get_frame_number()
frame_count += 1
if now - last_rate > 0.1:
rate = frame_count / (now - last_rate)
self.actual_frame_rate = 0.9 * self.actual_frame_rate + 0.1 * rate
last_rate = now
frame_count = 0
self.vel_filter.set_cutoff_frequency(
self.actual_frame_rate, self.vicon_settings.vel_filter_hz)
# get position in mm
pos_enu = vicon.get_segment_global_translation(name, segname)
if pos_enu is None:
continue
# convert to NED meters
pos_ned = Vector3(pos_enu[1] * 0.001, pos_enu[0] * 0.001,
-pos_enu[2] * 0.001)
if last_frame_num is None or frame_num - last_frame_num > 100 or frame_num <= last_frame_num:
last_frame_num = frame_num
last_pos = pos_ned
continue
dt = (frame_num - last_frame_num) * frame_dt
vel = (pos_ned - last_pos) * (1.0 / dt)
last_pos = pos_ned
last_frame_num = frame_num
filtered_vel = self.vel_filter.apply(vel)
if self.vicon_settings.vision_rate > 0:
dt = now - last_msg_time
if dt < 1.0 / self.vicon_settings.vision_rate:
continue
last_msg_time = now
# get orientation in euler, converting to ArduPilot conventions
quat = vicon.get_segment_global_quaternion(name, segname)
q = Quaternion([quat[3], quat[0], quat[1], quat[2]])
euler = q.euler
roll, pitch, yaw = euler[2], euler[1], euler[0]
yaw += math.radians(self.vicon_settings.yaw_offset)
yaw = math.radians(mavextra.wrap_360(math.degrees(yaw)))
self.pos = pos_ned
self.att = [
math.degrees(roll),
math.degrees(pitch),
math.degrees(yaw)
]
self.frame_count += 1
yaw_cd = int(mavextra.wrap_360(math.degrees(yaw)) * 100)
if yaw_cd == 0:
# the yaw extension to GPS_INPUT uses 0 as no yaw support
yaw_cd = 36000
time_us = int(now * 1.0e6)
if now - last_origin_send > 1 and self.vicon_settings.vision_rate > 0:
for m, t, _ in self.master:
# send a heartbeat msg
m.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS,
mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
0, 0, 0)
# send origin at 1Hz
m.mav.set_gps_global_origin_send(
t, int(self.vicon_settings.origin_lat * 1.0e7),
int(self.vicon_settings.origin_lon * 1.0e7),
int(self.vicon_settings.origin_alt * 1.0e3), time_us)
last_origin_send = now
if self.vicon_settings.gps_rate > 0 and now_ms - last_gps_send_ms > gps_period_ms:
'''send GPS data at the specified rate, trying to align on the given period'''
if last_gps_pos is None:
last_gps_pos = pos_ned
gps_lat, gps_lon = mavextra.gps_offset(
self.vicon_settings.origin_lat,
self.vicon_settings.origin_lon, pos_ned.y, pos_ned.x)
gps_alt = self.vicon_settings.origin_alt - pos_ned.z
gps_dt = now - last_gps_send
gps_vel = filtered_vel
last_gps_pos = pos_ned
gps_week, gps_week_ms = get_gps_time(now)
if self.vicon_settings.gps_nsats >= 6:
fix_type = 3
else:
fix_type = 1
mav.mav.gps_input_send(time_us, 0, 0, gps_week_ms, gps_week,
fix_type, int(gps_lat * 1.0e7),
int(gps_lon * 1.0e7), gps_alt, 1.0, 1.0,
gps_vel.x, gps_vel.y, gps_vel.z, 0.2,
1.0, 1.0, self.vicon_settings.gps_nsats,
yaw_cd)
last_gps_send_ms = (now_ms // gps_period_ms) * gps_period_ms
last_gps_send = now
self.gps_count += 1
if self.vicon_settings.vision_rate > 0:
# send VISION_POSITION_ESTIMATE
# we force mavlink1 to avoid the covariances which seem to make the packets too large
# for the mavesp8266 wifi bridge
mav.mav.global_vision_position_estimate_send(
time_us,
pos_ned.x,
pos_ned.y,
pos_ned.z,
roll,
pitch,
yaw,
force_mavlink1=True)
self.vision_count += 1
now = time.time()
while True:
pose = client.simGetVehiclePose()
now = time.time()
now_ms = int(now * 1000)
time_us = int(now * 1.0e6)
fix_type = 6
position = pose.position
pos_ned = Vector3(position.x_val, position.y_val, -1 * position.z_val)
gps_week, gps_week_ms = get_gps_time(now)
gps_lat, gps_lon = mavextra.gps_offset(-35.363261,
149.165230,
pos_ned.y, pos_ned.x)
gps_alt = pos_ned.z
sat_visible = 20
ignore_flags = mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VEL_HORIZ | mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VEL_VERT
gps_vel = Vector3(0, 0, 0)
msg = vehicle.message_factory.gps_input_encode(time_us, 0, ignore_flags, gps_week_ms, gps_week, fix_type,
int(gps_lat*1e7),int(gps_lon*1e7),gps_alt,
1.0, 1.0,
gps_vel.x, gps_vel.y, gps_vel.z,
0.2, 1.0, 1.0,
sat_visible,
) # Satellites visible
def thread_loop(self):
'''background processing'''
name = None
while True:
vicon = self.vicon
if vicon is None:
time.sleep(0.1)
continue
if not name:
vicon.get_frame()
name = vicon.get_subject_name(0)
if name is None:
continue
segname = vicon.get_subject_root_segment_name(name)
print("Connected to subject '%s' segment '%s'" %
(name, segname))
last_msg_time = time.time()
last_gps_pos = None
now = time.time()
last_origin_send = now
now_ms = int(now * 1000)
last_gps_send_ms = now_ms
last_gps_send = now
if self.vicon_settings.gps_rate > 0:
gps_period_ms = 1000 // self.vicon_settings.gps_rate
time.sleep(0.01)
vicon.get_frame()
mav = self.master
now = time.time()
now_ms = int(now * 1000)
if self.vicon_settings.vision_rate > 0:
dt = now - last_msg_time
if dt < 1.0 / self.vicon_settings.vision_rate:
continue
last_msg_time = now
# get position in mm
pos_enu = vicon.get_segment_global_translation(name, segname)
if pos_enu is None:
continue
# convert to NED meters
pos_ned = [
pos_enu[1] * 0.001, pos_enu[0] * 0.001, -pos_enu[2] * 0.001
]
# get orientation in euler, converting to ArduPilot conventions
quat = vicon.get_segment_global_quaternion(name, segname)
q = Quaternion([quat[3], quat[0], quat[1], quat[2]])
euler = q.euler
roll, pitch, yaw = euler[2], euler[1], euler[0]
yaw -= math.pi * 0.5
yaw = math.radians(mavextra.wrap_360(math.degrees(yaw)))
self.pos = pos_ned
self.att = [
math.degrees(roll),
math.degrees(pitch),
math.degrees(yaw)
]
self.frame_count += 1
yaw_cd = int(mavextra.wrap_360(math.degrees(yaw)) * 100)
if yaw_cd == 0:
# the yaw extension to GPS_INPUT uses 0 as no yaw support
yaw_cd = 36000
time_us = int(now * 1.0e6)
if now - last_origin_send > 1 and self.vicon_settings.vision_rate > 0:
# send a heartbeat msg
mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS,
mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
0, 0, 0)
# send origin at 1Hz
mav.mav.set_gps_global_origin_send(
self.target_system,
int(self.vicon_settings.origin_lat * 1.0e7),
int(self.vicon_settings.origin_lon * 1.0e7),
int(self.vicon_settings.origin_alt * 1.0e3), time_us)
last_origin_send = now
if self.vicon_settings.gps_rate > 0 and now_ms - last_gps_send_ms > gps_period_ms:
'''send GPS data at the specified rate, trying to align on the given period'''
if last_gps_pos is None:
last_gps_pos = pos_ned
gps_lat, gps_lon = mavextra.gps_offset(
self.vicon_settings.origin_lat,
self.vicon_settings.origin_lon, pos_ned[1], pos_ned[0])
gps_alt = self.vicon_settings.origin_alt - pos_ned[2]
gps_dt = now - last_gps_send
gps_vel = [(pos_ned[0] - last_gps_pos[0]) / gps_dt,
(pos_ned[1] - last_gps_pos[1]) / gps_dt,
(pos_ned[2] - last_gps_pos[2]) / gps_dt]
last_gps_pos = pos_ned
gps_week, gps_week_ms = get_gps_time(now)
if self.vicon_settings.gps_nsats >= 6:
fix_type = 3
else:
fix_type = 1
mav.mav.gps_input_send(time_us, 0, 0, gps_week_ms, gps_week,
fix_type, int(gps_lat * 1.0e7),
int(gps_lon * 1.0e7), gps_alt, 1.0, 1.0,
gps_vel[0], gps_vel[1], gps_vel[2], 0.2,
1.0, 1.0, self.vicon_settings.gps_nsats,
yaw_cd)
last_gps_send_ms = (now_ms // gps_period_ms) * gps_period_ms
last_gps_send = now
self.gps_count += 1
if self.vicon_settings.vision_rate > 0:
# send VISION_POSITION_ESTIMATE
# we force mavlink1 to avoid the covariances which seem to make the packets too large
# for the mavesp8266 wifi bridge
mav.mav.global_vision_position_estimate_send(
time_us,
pos_ned[0],
pos_ned[1],
pos_ned[2],
roll,
pitch,
yaw,
force_mavlink1=True)
self.vision_count += 1
