def flight_time(logfile):
'''work out flight time for a log file'''
print("Processing log %s" % filename)
mlog = mavutil.mavlink_connection(filename)
in_air = False
start_time = 0.0
total_time = 0.0
total_dist = 0.0
t = None
last_msg = None
last_time_usec = None
while True:
m = mlog.recv_match(type=['GPS', 'GPS_RAW_INT'],
condition=args.condition)
if m is None:
if in_air:
total_time += time.mktime(t) - start_time
if total_time > 0:
print("Flight time : %u:%02u" %
(int(total_time) / 60, int(total_time) % 60))
return (total_time, total_dist)
if m.get_type() == 'GPS_RAW_INT':
groundspeed = m.vel * 0.01
status = m.fix_type
time_usec = m.time_usec
else:
groundspeed = m.Spd
status = m.Status
time_usec = m.TimeUS
if status < 3:
continue
t = time.localtime(m._timestamp)
if groundspeed > args.groundspeed and not in_air:
print("In air at %s (percent %.0f%% groundspeed %.1f)" %
(time.asctime(t), mlog.percent, groundspeed))
in_air = True
start_time = time.mktime(t)
elif groundspeed < args.groundspeed and in_air:
print(
"On ground at %s (percent %.1f%% groundspeed %.1f time=%.1f seconds)"
% (time.asctime(t), mlog.percent, groundspeed,
time.mktime(t) - start_time))
in_air = False
total_time += time.mktime(t) - start_time
if last_msg is None or time_usec > last_time_usec or time_usec + 30e6 < last_time_usec:
if last_msg is not None:
total_dist += distance_two(last_msg, m)
last_msg = m
last_time_usec = time_usec
return (total_time, total_dist)
def flight_time(logbook, logfile, argcondition, argmindist, argmintime):
'''work out flight time for a log file'''
print("Processing log %s" % logfile)
mlog = mavutil.mavlink_connection(logfile)
in_air = False
start_time = None
total_time = 0.0
total_dist = 0.0
t = None
last_msg = None
aircraft = logfile.split('/')[0]
first_takeoff = None
status = 0
while True:
m = mlog.recv_match(type=['GPS', 'GPS_RAW_INT', 'VFR_HUD'],
condition=argcondition)
if m is None:
if in_air:
total_time += time.mktime(t) - start_time
if total_time > 0:
print("Flight time %s %u:%02u" %
(logfile, int(total_time) / 60, int(total_time) % 60))
if in_air and total_dist >= argmindist * 1000 and total_time >= argmintime * 60:
add_log_entry(logbook, logfile, start_time, total_time,
total_dist)
return (total_time, total_dist)
if not mlog.motors_armed():
continue
if m.get_type() == 'GPS_RAW_INT':
groundspeed = m.vel * 0.01
status = m.fix_type
elif m.get_type() == 'GPS':
groundspeed = m.Spd
status = m.Status
elif m.get_type() == 'VFR_HUD':
throttle = m.throttle
if status < 3:
continue
t = time.localtime(m._timestamp)
if not in_air:
in_air = True
start_time = time.mktime(t)
if m.get_type() in ['GPS', 'GPS_RAW_INT']:
if last_msg is not None:
total_dist += distance_two(last_msg, m)
last_msg = m
if in_air and total_dist >= argmindist * 1000 and total_time >= argmintime * 60:
add_log_entry(logfile, start_time, total_time, total_dist)
return (total_time, total_dist)
def flight_time(logfile):
'''work out flight time for a log file'''
print(("Processing log %s" % filename))
mlog = mavutil.mavlink_connection(filename)
in_air = False
start_time = 0.0
total_time = 0.0
total_dist = 0.0
t = None
last_msg = None
while True:
m = mlog.recv_match(
type=[
'GPS',
'GPS_RAW_INT'],
condition=args.condition)
if m is None:
if in_air:
total_time += time.mktime(t) - start_time
if total_time > 0:
print(("Flight time : %u:%02u" %
(int(total_time) /
60, int(total_time) %
60)))
return (total_time, total_dist)
if m.get_type() == 'GPS_RAW_INT':
groundspeed = m.vel * 0.01
status = m.fix_type
else:
groundspeed = m.Spd
status = m.Status
if status < 3:
continue
t = time.localtime(m._timestamp)
if groundspeed > args.groundspeed and not in_air:
print(("In air at %s (percent %.0f%% groundspeed %.1f)" %
(time.asctime(t), mlog.percent, groundspeed)))
in_air = True
start_time = time.mktime(t)
elif groundspeed < args.groundspeed and in_air:
print(("On ground at %s (percent %.1f%% groundspeed %.1f time=%.1f seconds)" % (
time.asctime(t), mlog.percent, groundspeed, time.mktime(t) - start_time)))
in_air = False
total_time += time.mktime(t) - start_time
if last_msg is not None:
total_dist += distance_two(last_msg, m)
last_msg = m
return (total_time, total_dist)
def flight_time(logbook, logfile, argcondition, argmindist, argmintime):
'''work out flight time for a log file'''
print("Processing log %s" % logfile)
mlog = mavutil.mavlink_connection(logfile)
in_air = False
start_time = None
total_time = 0.0
total_dist = 0.0
t = None
last_msg = None
aircraft = logfile.split('/')[0]
first_takeoff = None
status = 0
while True:
m = mlog.recv_match(type=['GPS','GPS_RAW_INT','VFR_HUD'], condition=argcondition)
if m is None:
if in_air:
total_time += time.mktime(t) - start_time
if total_time > 0:
print("Flight time %s %u:%02u" % (logfile, int(total_time)/60, int(total_time)%60))
if in_air and total_dist >= argmindist*1000 and total_time >= argmintime*60:
add_log_entry(logbook, logfile, start_time, total_time, total_dist)
return (total_time, total_dist)
if not mlog.motors_armed():
continue
if m.get_type() == 'GPS_RAW_INT':
groundspeed = m.vel*0.01
status = m.fix_type
elif m.get_type() == 'GPS':
groundspeed = m.Spd
status = m.Status
elif m.get_type() == 'VFR_HUD':
throttle = m.throttle
if status < 3:
continue
t = time.localtime(m._timestamp)
if not in_air:
in_air = True
start_time = time.mktime(t)
if m.get_type() in ['GPS','GPS_RAW_INT']:
if last_msg is not None:
total_dist += distance_two(last_msg, m)
last_msg = m
if in_air and total_dist >= argmindist*1000 and total_time >= argmintime*60:
add_log_entry(logfile, start_time, total_time, total_dist)
return (total_time, total_dist)
def PrintSummary(logfile):
'''Calculate some interesting datapoints of the file'''
# Open the log file
mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps, dialect=args.dialect)
autonomous_sections = 0 # How many different autonomous sections there are
autonomous = False # Whether the vehicle is currently autonomous at this point in the logfile
auto_time = 0.0 # The total time the vehicle was autonomous/guided (seconds)
start_time = None # The datetime of the first received message (seconds since epoch)
total_dist = 0.0 # The total ground distance travelled (meters)
last_gps_msg = None # The first GPS message received
first_gps_msg = None # The last GPS message received
true_time = None # Track the first timestamp found that corresponds to a UNIX timestamp
types = set(['HEARTBEAT', 'GPS_RAW_INT'])
while True:
m = mlog.recv_match(condition=args.condition, type=types)
# If there's no match, it means we're done processing the log.
if m is None:
break
# Ignore any failed messages
if m.get_type() == 'BAD_DATA':
continue
# Keep track of the latest timestamp for various calculations
timestamp = getattr(m, '_timestamp', 0.0)
# Log the first message time
if start_time is None:
start_time = timestamp
# Log the first timestamp found that is a true timestamp. We first try
# to get the groundstation timestamp from the log directly. If that fails
# we try to find a message that outputs a Unix time-since-epoch.
if true_time is None:
if not args.notimestamps and timestamp >= 1230768000:
true_time = timestamp
elif 'time_unix_usec' in m.__dict__ and m.time_unix_usec >= 1230768000:
true_time = m.time_unix_usec * 1.0e-6
elif 'time_usec' in m.__dict__ and m.time_usec >= 1230768000:
true_time = m.time_usec * 1.0e-6
# Track the vehicle's speed and status
if m.get_type() == 'GPS_RAW_INT':
# Ignore GPS messages without a proper fix
if m.fix_type < 3 or m.lat == 0 or m.lon == 0:
continue
# Log the first GPS location found, being sure to skip GPS fixes
# that are bad (at lat/lon of 0,0)
if first_gps_msg is None:
first_gps_msg = m
# Track the distance travelled, being sure to skip GPS fixes
# that are bad (at lat/lon of 0,0)
if last_gps_msg is None or m.time_usec > last_gps_msg.time_usec or m.time_usec+30e6 < last_gps_msg.time_usec:
if last_gps_msg is not None:
total_dist += distance_two(last_gps_msg, m)
# Save this GPS message to do simple distance calculations with
last_gps_msg = m
elif m.get_type() == 'HEARTBEAT':
if m.type == mavutil.mavlink.MAV_TYPE_GCS:
continue
if (m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED or
m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_AUTO_ENABLED) and not autonomous:
autonomous = True
autonomous_sections += 1
start_auto_time = timestamp
elif (not m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED and
not m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_AUTO_ENABLED) and autonomous:
autonomous = False
auto_time += timestamp - start_auto_time
# If there were no messages processed, say so
if start_time is None:
print("ERROR: No messages found.")
return
# If the vehicle ends in autonomous mode, make sure we log the total time
if autonomous:
auto_time += timestamp - start_auto_time
# Compute the total logtime, checking that timestamps are 2009 or newer for validity
# (MAVLink didn't exist until 2009)
if true_time:
start_time_str = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(true_time))
print("Log started at about {}".format(start_time_str))
else:
print("Warning: No absolute timestamp found in datastream. No starting time can be provided for this log.")
# Print location data
if last_gps_msg is not None:
first_gps_position = (first_gps_msg.lat / 1e7, first_gps_msg.lon / 1e7)
last_gps_position = (last_gps_msg.lat / 1e7, last_gps_msg.lon / 1e7)
print("Travelled from ({0[0]}, {0[1]}) to ({1[0]}, {1[1]})".format(first_gps_position, last_gps_position))
print("Total distance : {:0.2f}m".format(total_dist))
else:
print("Warning: No GPS data found, can't give position summary.")
# Print out the rest of the results.
total_time = timestamp - start_time
print("Total time (mm:ss): {:3.0f}:{:02.0f}".format(total_time / 60, total_time % 60))
# The autonomous time should be good, as a steady HEARTBEAT is required for MAVLink operation
print("Autonomous sections: {}".format(autonomous_sections))
if autonomous_sections > 0:
print("Autonomous time (mm:ss): {:3.0f}:{:02.0f}".format(auto_time / 60, auto_time % 60))
totals.time += total_time
totals.distance += total_dist
totals.flights += 1
def PrintSummary(logfile):
'''Calculate some interesting datapoints of the file'''
# Open the log file
mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps, dialect=args.dialect)
autonomous_sections = 0 # How many different autonomous sections there are
autonomous = False # Whether the vehicle is currently autonomous at this point in the logfile
auto_time = 0.0 # The total time the vehicle was autonomous/guided (seconds)
start_time = None # The datetime of the first received message (seconds since epoch)
total_dist = 0.0 # The total ground distance travelled (meters)
last_gps_msg = None # The first GPS message received
first_gps_msg = None # The last GPS message received
while True:
m = mlog.recv_match(condition=args.condition)
# If there's no match, it means we're done processing the log.
if m is None:
break
# Ignore any failed messages
if m.get_type() == 'BAD_DATA':
continue
# Keep track of the latest timestamp for various calculations
timestamp = getattr(m, '_timestamp', 0.0)
# Log the first message time
if start_time is None:
start_time = timestamp
# Track the vehicle's speed and status
if m.get_type() == 'GPS_RAW_INT':
# Ignore GPS messages without a proper fix
if m.fix_type < 3 or m.lat == 0 or m.lon == 0:
continue
# Log the first GPS location found, being sure to skip GPS fixes
# that are bad (at lat/lon of 0,0)
if first_gps_msg is None:
first_gps_msg = m
# Track the distance travelled, being sure to skip GPS fixes
# that are bad (at lat/lon of 0,0)
if last_gps_msg is not None:
total_dist += distance_two(last_gps_msg, m)
# Save this GPS message to do simple distance calculations with
last_gps_msg = m
elif m.get_type() == 'HEARTBEAT':
if (m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED or
m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_AUTO_ENABLED) and autonomous == False:
autonomous = True
autonomous_sections += 1
start_auto_time = timestamp
elif (not m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED and
not m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_AUTO_ENABLED) and autonomous == True:
autonomous = False
auto_time += timestamp - start_auto_time
# If there were no messages processed, say so
if start_time is None:
print("ERROR: No messages found.")
return
# If the vehicle ends in autonomous mode, make sure we log the total time
if autonomous == True:
auto_time += timestamp - start_auto_time
# Compute the total logtime, checking that timestamps are 2009 or newer for validity
# (MAVLink didn't exist until 2009)
if start_time >= 1230768000:
start_time_str = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(start_time))
end_time_str = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(timestamp))
print("Timespan from {} to {}".format(start_time_str, end_time_str))
else:
print(first_gps_msg.time_usec)
print(last_gps_msg.time_usec)
print("ERROR: Invalid timestamps found.")
# Print location data
if last_gps_msg is not None:
first_gps_position = (first_gps_msg.lat / 1e7, first_gps_msg.lon / 1e7)
last_gps_position = (last_gps_msg.lat / 1e7, last_gps_msg.lon / 1e7)
print("Travelled from ({0[0]}, {0[1]}) to ({1[0]}, {1[1]})".format(first_gps_position, last_gps_position))
print("Total distance : {:0.2f}m".format(total_dist))
else:
print("ERROR: No GPS data found.")
# Print out the rest of the results.
total_time = timestamp - start_time
print("Total time : {}:{:02}".format(int(total_time)/60, int(total_time)%60))
# The autonomous time should be good, as a steady HEARTBEAT is required for MAVLink operation
print("Autonomous sections : {}".format(autonomous_sections))
if autonomous_sections > 0:
print("Autonomous time : {}:{:02}".format(int(auto_time)/60, int(auto_time)%60))
def PrintSummary(logfile):
'''Calculate some interesting datapoints of the file'''
# Open the log file
mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps, dialect=args.dialect)
autonomous_sections = 0 # How many different autonomous sections there are
autonomous = False # Whether the vehicle is currently autonomous at this point in the logfile
auto_time = 0.0 # The total time the vehicle was autonomous/guided (seconds)
start_time = None # The datetime of the first received message (seconds since epoch)
total_dist = 0.0 # The total ground distance travelled (meters)
last_gps_msg = None # The first GPS message received
first_gps_msg = None # The last GPS message received
while True:
m = mlog.recv_match(condition=args.condition)
# If there's no match, it means we're done processing the log.
if m is None:
break
# Ignore any failed messages
if m.get_type() == 'BAD_DATA':
continue
# Keep track of the latest timestamp for various calculations
timestamp = getattr(m, '_timestamp', 0.0)
# Log the first message time
if start_time is None:
start_time = timestamp
# Track the vehicle's speed and status
if m.get_type() == 'GPS_RAW_INT':
# Ignore GPS messages without a proper fix
if m.fix_type < 3 or m.lat == 0 or m.lon == 0:
continue
# Log the first GPS location found, being sure to skip GPS fixes
# that are bad (at lat/lon of 0,0)
if first_gps_msg is None:
first_gps_msg = m
# Track the distance travelled, being sure to skip GPS fixes
# that are bad (at lat/lon of 0,0)
if last_gps_msg is not None:
total_dist += distance_two(last_gps_msg, m)
# Save this GPS message to do simple distance calculations with
last_gps_msg = m
elif m.get_type() == 'HEARTBEAT':
if (m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED or
m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_AUTO_ENABLED) and autonomous == False:
autonomous = True
autonomous_sections += 1
start_auto_time = timestamp
elif (not m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED and
not m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_AUTO_ENABLED) and autonomous == True:
autonomous = False
auto_time += timestamp - start_auto_time
# If there were no messages processed, say so
if start_time is None:
print("ERROR: No messages found.")
return
# If the vehicle ends in autonomous mode, make sure we log the total time
if autonomous == True:
auto_time += timestamp - start_auto_time
# Compute the total logtime, checking that timestamps are 2009 or newer for validity
# (MAVLink didn't exist until 2009)
if start_time >= 1230768000:
start_time_str = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(start_time))
end_time_str = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(timestamp))
print("Timespan from {} to {}".format(start_time_str, end_time_str))
else:
print(first_gps_msg.time_usec)
print(last_gps_msg.time_usec)
print("ERROR: Invalid timestamps found.")
# Print location data
if last_gps_msg is not None:
first_gps_position = (first_gps_msg.lat / 1e7, first_gps_msg.lon / 1e7)
last_gps_position = (last_gps_msg.lat / 1e7, last_gps_msg.lon / 1e7)
print("Travelled from ({0[0]}, {0[1]}) to ({1[0]}, {1[1]})".format(first_gps_position, last_gps_position))
print("Total distance : {:0.2f}m".format(total_dist))
else:
print("ERROR: No GPS data found.")
# Print out the rest of the results.
total_time = timestamp - start_time
print("Total time : {}:{:02}".format(int(total_time)/60, int(total_time)%60))
# The autonomous time should be good, as a steady HEARTBEAT is required for MAVLink operation
print("Autonomous sections : {}".format(autonomous_sections))
if autonomous_sections > 0:
print("Autonomous time : {}:{:02}".format(int(auto_time)/60, int(auto_time)%60))
def PrintSummary(logfile):
'''Calculate some interesting datapoints of the file'''
# Open the log file
mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps, dialect=args.dialect)
autonomous_sections = 0 # How many different autonomous sections there are
autonomous = False # Whether the vehicle is currently autonomous at this point in the logfile
auto_time = 0.0 # The total time the vehicle was autonomous/guided (seconds)
start_time = None # The datetime of the first received message (seconds since epoch)
total_dist = 0.0 # The total ground distance travelled (meters)
last_gps_msg = None # The first GPS message received
first_gps_msg = None # The last GPS message received
true_time = None # Track the first timestamp found that corresponds to a UNIX timestamp
while True:
m = mlog.recv_match(condition=args.condition)
# If there's no match, it means we're done processing the log.
if m is None:
break
# Ignore any failed messages
if m.get_type() == 'BAD_DATA':
continue
# Keep track of the latest timestamp for various calculations
timestamp = getattr(m, '_timestamp', 0.0)
# Log the first message time
if start_time is None:
start_time = timestamp
# Log the first timestamp found that is a true timestamp. We first try
# to get the groundstation timestamp from the log directly. If that fails
# we try to find a message that outputs a Unix time-since-epoch.
if true_time is None:
if not args.notimestamps and timestamp >= 1230768000:
true_time = timestamp
elif 'time_unix_usec' in m.__dict__ and m.time_unix_usec >= 1230768000:
true_time = m.time_unix_usec * 1.0e-6
elif 'time_usec' in m.__dict__ and m.time_usec >= 1230768000:
true_time = m.time_usec * 1.0e-6
# Track the vehicle's speed and status
if m.get_type() == 'GPS_RAW_INT':
# Ignore GPS messages without a proper fix
if m.fix_type < 3 or m.lat == 0 or m.lon == 0:
continue
# Log the first GPS location found, being sure to skip GPS fixes
# that are bad (at lat/lon of 0,0)
if first_gps_msg is None:
first_gps_msg = m
# Track the distance travelled, being sure to skip GPS fixes
# that are bad (at lat/lon of 0,0)
if last_gps_msg is None or m.time_usec > last_gps_msg.time_usec or m.time_usec+30e6 < last_gps_msg.time_usec:
if last_gps_msg is not None:
total_dist += distance_two(last_gps_msg, m)
# Save this GPS message to do simple distance calculations with
last_gps_msg = m
elif m.get_type() == 'HEARTBEAT':
if m.type == mavutil.mavlink.MAV_TYPE_GCS:
continue
if (m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED or
m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_AUTO_ENABLED) and autonomous == False:
autonomous = True
autonomous_sections += 1
start_auto_time = timestamp
elif (not m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED and
not m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_AUTO_ENABLED) and autonomous == True:
autonomous = False
auto_time += timestamp - start_auto_time
# If there were no messages processed, say so
if start_time is None:
print("ERROR: No messages found.")
return
# If the vehicle ends in autonomous mode, make sure we log the total time
if autonomous == True:
auto_time += timestamp - start_auto_time
# Compute the total logtime, checking that timestamps are 2009 or newer for validity
# (MAVLink didn't exist until 2009)
if true_time:
start_time_str = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(true_time))
print("Log started at about {}".format(start_time_str))
else:
print("Warning: No absolute timestamp found in datastream. No starting time can be provided for this log.")
# Print location data
if last_gps_msg is not None:
first_gps_position = (first_gps_msg.lat / 1e7, first_gps_msg.lon / 1e7)
last_gps_position = (last_gps_msg.lat / 1e7, last_gps_msg.lon / 1e7)
print("Travelled from ({0[0]}, {0[1]}) to ({1[0]}, {1[1]})".format(first_gps_position, last_gps_position))
print("Total distance : {:0.2f}m".format(total_dist))
else:
print("Warning: No GPS data found, can't give position summary.")
# Print out the rest of the results.
total_time = timestamp - start_time
print("Total time (mm:ss): {:3.0f}:{:02.0f}".format(total_time / 60, total_time % 60))
# The autonomous time should be good, as a steady HEARTBEAT is required for MAVLink operation
print("Autonomous sections: {}".format(autonomous_sections))
if autonomous_sections > 0:
print("Autonomous time (mm:ss): {:3.0f}:{:02.0f}".format(auto_time / 60, auto_time % 60))
totals.time += total_time
totals.distance += total_dist
totals.flights += 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