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