def get_vicon_pose(self, object_name, segment_name):
# get position in mm. Coordinates are in NED
vicon_pos = self.vicon.get_segment_global_translation(
object_name, segment_name)
if vicon_pos is None:
# Object is not in view
return None, None, None, None
vicon_quat = Quaternion(
self.vicon.get_segment_global_quaternion(object_name,
segment_name))
pos_ned = Vector3(vicon_pos * 0.001)
euler = vicon_quat.euler
roll, pitch, yaw = euler[0], euler[1], euler[2]
yaw = math.radians(mavextra.wrap_360(math.degrees(yaw)))
return pos_ned, roll, pitch, yaw
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
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
def main():
print sys.argv
parser = argparse.ArgumentParser()
parser.add_argument("--baudrate",
type=int,
help="master port baud rate",
default=57600)
parser.add_argument("--master", required=True, help="serial device")
parser.add_argument("--port",
required=True,
type=int,
help="port for server")
parser.add_argument("--logfile", help="location to log information")
parser.add_argument("--mavlog", required=True, help="tlog dump location")
parser.add_argument(
"--report-timeout",
type=int,
default=-1,
help=
"seconds to wait before reporting attack back to system. -1 means don't report"
)
parser.add_argument("lat", type=float, help="latitude of point to protect")
parser.add_argument("lon",
type=float,
help="longitude of point ot protect")
parser.add_argument("radius",
type=float,
help="radius around point to protect")
args = parser.parse_args()
lgr.info("parsed arguments")
enabled = False
attack_happened = False
# connect to mav
with closing(mavutil.mavlink_connection(args.master,
baud=args.baudrate)) as master:
lgr.info("connect to master")
with open(args.mavlog, "w"):
pass
with closing(
mavutil.mavlink_connection(args.mavlog,
write=True,
input=False)) as logger:
lgr.info("connected to mav log file")
# data variables here
# make up a fake gps coordinate to calculations
protect = FakeGPS(args.lat * 1.0e7, args.lon * 1.0e7, 0, 10)
restore_mode = None
triggered = False
time_of_attack = None
with closing(socket.socket(socket.AF_INET,
socket.SOCK_STREAM)) as server:
server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
try:
server.bind(("0.0.0.0", args.port))
except IOError as e:
lgr.exception("failed to connect to socket")
raise
lgr.info("connected to socket")
server.listen(1)
lgr.info("reply from socket")
conn, addr = server.accept()
lgr.info("established connection over socket")
with closing(conn) as conn:
with closing(conn.makefile()) as rc:
lgr.info("full connection")
while True:
lgr.info("waiting to read")
to_read, _, _ = select.select([master.fd, rc], [],
[], 1)
lgr.info("read to read")
for f in to_read:
# check to see if this is a mavlink packet or a message from the server
if f == master.fd:
lgr.info("received MAVLink message")
# we are the mav
loc = master.recv_match(type='GPS_RAW_INT')
if loc is None:
continue
if not enabled:
continue
# calculate distance and bearing from protected point
dist = mavextra.distance_two(loc, protect)
deg = mavextra.wrap_360(
bearing(protect, loc))
print dist
# if we are in the 'cone of influence'
if dist <= args.radius and master.motors_armed(
):
print "Incoming target at %.2f degrees and %.2f meters away." % (
deg, dist)
# we need to protect the point!
attack_happened = True
if time_of_attack is None:
time_of_attack = time.time()
# but first back up current mode
# if restore_mode is None:
# master.recv_match(type='SYS_STATUS')
# restore_mode = master.flightmode
# # set mode for brake
# master.set_mode(17)
# logger.set_mode(17)
# if the attack hasn't occurred yet
if not triggered:
print "triggered\n"
val = 0xFFFFFFFF
master.param_set_send('j', 50)
logger.param_set_send('j', 50)
triggered = True
triggered = True
# # We sleep for a moment to help the MAV slow down
# time.sleep(1)
# print "go"
# elif restore_mode is not None:
# # We are finished
# print 'Done protecting'
# master.set_mode(restore_mode)
# logger.set_mode(restore_mode)
# restore_mode = None
else:
lgr.info(
"received message from test harness")
# message from the server
msg = f.readline().strip().upper()
print msg
lgr.info("TH message: %s", msg)
if msg == "START":
enabled = True
elif msg == "EXIT":
print "we are done"
logger.close()
break
elif msg == "CHECK":
if attack_happened:
rc.write("YES\n")
rc.flush()
else:
rc.write("NO\n")
rc.flush()
# print "waiting for heartbeat"
# master.recv_match(type="HEARTBEAT", blocking=True)
# print "got heartbeat"
# check to see if we should be reporting
if time_of_attack is not None:
if (args.report_timeout >= 0
and time.time() - time_of_attack >
args.report_timeout):
rc.write("ATTACK\n")
rc.flush()
args.report_timeout = -1
