예제 #1
0
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)
예제 #2
0
파일: logbook.py 프로젝트: tajisoft/cuav
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)
예제 #3
0
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)
예제 #4
0
파일: logbook.py 프로젝트: CanberraUAV/cuav
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)
예제 #5
0
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
예제 #6
0
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))
예제 #7
0
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))
예제 #8
0
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
예제 #9
0
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