예제 #1
0
def load(csv_file):
    result = {}
    result['imu'] = []
    result['gps'] = []
    result['air'] = []
    result['filter'] = []
    #result['pilot'] = []
    result['act'] = []
    result['ap'] = []

    last_gps_time = -1.0
    with open(csv_file, 'rb') as f:
        reader = csv.DictReader(f)
        for row in reader:
            if row['TIME_StartTime'] == '':
                # empty time
                continue

            imu = IMUdata()
            imu.time = float(row['TIME_StartTime']) / 1000000.0
            imu.p = float(row['IMU_GyroX'])
            imu.q = float(row['IMU_GyroY'])
            imu.r = float(row['IMU_GyroZ'])
            imu.ax = float(row['IMU_AccX'])
            imu.ay = float(row['IMU_AccY'])
            imu.az = float(row['IMU_AccZ'])
            hx = float(row['IMU_MagX'])
            hy = float(row['IMU_MagY'])
            hz = float(row['IMU_MagZ'])
            hf = np.array([hx, hy, hz])
            norm = np.linalg.norm(hf)
            hf /= norm
            imu.hx = hf[0]
            imu.hy = hf[1]
            imu.hz = hf[2]
            #print imu.hx, imu.hy, imu.hz
            imu.temp = 15.0
            result['imu'].append(imu)

            if row['GPS_GPSTime'] != '':
                gps = GPSdata()
                gps.time = imu.time
                gps.unix_sec = float(row['GPS_GPSTime']) / 1000000.0
                gps.lat = float(row['GPS_Lat'])
                gps.lon = float(row['GPS_Lon'])
                gps.alt = float(row['GPS_Alt'])
                gps.vn = float(row['GPS_VelN'])
                gps.ve = float(row['GPS_VelE'])
                gps.vd = float(row['GPS_VelD'])
                gps.sats = int(row['GPS_nSat'])
                if gps.sats >= 5 and gps.unix_sec > last_gps_time:
                    result['gps'].append(gps)
                last_gps_time = gps.unix_sec

            if row['SENS_BaroPres'] != '':
                air = Airdata()
                air.time = imu.time
                air.static_press = float(row['SENS_BaroPres'])
                if 'AIR1_DiffPres' in row and row['AIR1_DiffPres'] != '':
                    air.diff_press = float(row['AIR1_DiffPres'])
                else:
                    air.diff_press = 0.0
                if 'AIRS_Temp' in row and row['AIRS_Temp'] != '':
                    air.temp = float(row['AIRS_Temp'])
                else:
                    air.temp = 0.0
                if 'AIRS_IAS' in row and row['AIRS_IAS'] != '':
                    air.airspeed = float(row['AIRS_IAS']) * mps2kt
                else:
                    air.airspeed = 0.0
                if 'AIR1_BaroAlt' in row and row['AIR1_BaroAlt'] != '':
                    air.alt_press = float(row['AIR1_BaroAlt'])
                else:
                    air.alt_press = 0.0
                if 'GPS_Alt' in row and row['GPS_Alt'] != '':
                    air.alt_true = float(row['GPS_Alt'])
                else:
                    air.alt_true = 0.0
                result['air'].append(air)

            if row['GPOS_Lat'] != '':
                nav = NAVdata()
                nav.time = imu.time
                nav.lat = float(row['GPOS_Lat']) * d2r
                nav.lon = float(row['GPOS_Lon']) * d2r
                nav.alt = float(row['GPOS_Alt'])
                nav.vn = float(row['GPOS_VelN'])
                nav.ve = float(row['GPOS_VelE'])
                nav.vd = float(row['GPOS_VelD'])
                nav.phi = float(row['ATT_Roll'])
                nav.the = float(row['ATT_Pitch'])
                psi = float(row['ATT_Yaw'])
                if psi > 180.0:
                    psi = psi - 360.0
                if psi < -180.0:
                    psi = psi + 360.0
                nav.psi = psi
                result['filter'].append(nav)

            if row['GPSP_Alt'] != '':
                ap = APdata()
                ap.time = imu.time
                ap.hdg = my_float(row['ATSP_YawSP']) * r2d
                ap.roll = my_float(row['ATSP_RollSP']) * r2d
                ap.alt = my_float(row['GPSP_Alt']) * m2ft
                ap.pitch = my_float(row['ATSP_PitchSP']) * r2d
                ap.speed = my_float(row['TECS_AsSP']) * mps2kt
                result['ap'].append(ap)

            if row['OUT0_Out0'] != '':
                act = Controldata()
                ch0 = (float(row['OUT0_Out0']) - 1500) / 500
                ch1 = (float(row['OUT0_Out1']) - 1500) / 500
                ch2 = (float(row['OUT0_Out2']) - 1000) / 1000
                ch3 = (float(row['OUT0_Out3']) - 1500) / 500
                #print ch0, ch1, ch2, ch3
                act.time = imu.time
                act.aileron = (ch0 - ch1)
                act.elevator = -(ch0 + ch1)
                act.throttle = ch2
                act.rudder = ch3
                act.gear = 0.0
                act.flaps = 0.0
                act.aux1 = 0.0
                act.auto_manual = 0.0
                result['act'].append(act)

    return result
예제 #2
0
def load(mat_filename):
    # Name of .mat file that exists in the directory defined above and
    # has the flight_data and flight_info structures
    filepath = mat_filename

    # Load Flight Data: ## IMPORTANT to have the .mat file in the
    # flight_data and flight_info structures for this function ##
    data = sio.loadmat(filepath, struct_as_record=False, squeeze_me=True)
    print 'Loaded Data Summary'
    print '* File: %s' % filepath.split(os.path.sep)[-1]
    try:
        flight_data, flight_info = data['flight_data'], data['flight_info']
        print('* Date: %s' % flight_info.date)
        print('* Aircraft: %s' % flight_info.aircraft)
    except KeyError:
        print 'KeyError'
        # Convert from Python dictionary to struct-like before
        flight_data = dict2struct()
        for k in data:
            exec("flight_data.%s = data['%s']" % (k, k))
    del(data)

    # Add both names for pitch: the and theta
    try:
        flight_data.theta = flight_data.the
    except AttributeError:
        pass

    # Fill in time data
    t = flight_data.time
    print 't:', t

    # Magnetometer data - not used hence don't trust
    hm  = np.vstack((flight_data.hx, -flight_data.hy, -flight_data.hz)).T

    # Geri mag calibration
    mag_affine = np.array(
        [[ 4.3374191736,  0.9527668819,  0.2106929615,  0.0649324241],
         [-3.7617930866,  6.2839014058, -3.5707398622,  2.1616165305],
         [-0.8688354599,  0.2205650877,  4.7466115481, -2.7041600008],
         [ 0.          ,  0.          ,  0.          ,  1.          ]]
    )

    # Populate IMU Data
    imu = np.vstack((t, flight_data.p, flight_data.q, flight_data.r, 
                     flight_data.ax, flight_data.ay, flight_data.az,
                     hm[:,0], hm[:,1], hm[:,2])).T

    # Note that accelerometer and gyro measurements logged by UAV
    # after 11/17/2011 flight (seemingly, see 
    # http://trac.umnaem.webfactional.com/wiki/FlightReports/2011_11_17)
    # have the nav-estimated bias removed before datalogging. So to work with raw
    # imu-data, we add back the on-board estimated biases.
    if not FLAG_UNBIASED_IMU:
        try:
            imu[:, 1:4] += np.vstack((flight_data.p_bias, 
                                      flight_data.q_bias, 
                                      flight_data.r_bias)).T

            imu[:, 4:7] += np.vstack((flight_data.ax_bias,
                                      flight_data.ay_bias,
                                      flight_data.az_bias)).T
        except AttributeError:
            print('Note: On board estimated bias not found.')

    # Air Data
    ias = flight_data.ias # indicated airspeed (m/s)
    h = flight_data.h

    # Populate GPS sensor data
    try:
        vn = flight_data.gps_vn
    except:
        vn = flight_data.vn
    try:
        ve = flight_data.gps_ve
    except:
        ve = flight_data.ve
    try:
        vd = flight_data.gps_vd
    except:
        vd = flight_data.vd
    lat = flight_data.lat
    lon = flight_data.lon
    alt = flight_data.alt

    # kstart set to when the navigation filter used onboard the aircraft
    # was initialized and this is accomplished by detecting when navlat is
    # no longer 0.0. This choice of kstart will ensure the filter being
    # tested is using the same initialization time step as the onboard
    # filter allowing for apples to apples comparisons.
    kstart = (abs(flight_data.navlat) > 0.0).tolist().index(True)
    k = kstart
    print('Initialized at Time: %.2f s (k=%i)' % (t[k], k))

    # Set previous value of GPS altitude to 0.0. This will be used to
    # trigger GPS newData flag which is commonly used in our
    # navigation filters for deciding if the GPS data has been
    # updated. However, in python we have no log of newData
    # (typically). So a comparison of current GPS altitude to the
    # previous epoch's GPS altitude is used to determine if GPS has
    # been updated.
    last_gps_alt = -9999.9

    # create data structures for ekf processing
    result = {}
    result['imu'] = []
    result['gps'] = []
    result['air'] = []
    result['filter'] = []
    
    k = kstart
    while k < len(t):
        p, q, r = imu[k, 1:4]
        ax, ay, az = imu[k, 4:7]
        hx, hy, hz = imu[k, 7:10]
        
        s = [hx, hy, hz, 1.0]
        #hf = np.dot(mag_affine, s)
        hf = s

        imu_pt = IMUdata()
        imu_pt.time = float(t[k])
        imu_pt.p = float(p)
        imu_pt.q = float(q)
        imu_pt.r = float(r)
        imu_pt.ax = float(ax)
        imu_pt.ay = float(ay)
        imu_pt.az = float(az)
        #imu_pt.hx = hx
        #imu_pt.hy = hy
        #imu_pt.hz = hz
        imu_pt.hx = float(hf[0])
        imu_pt.hy = float(hf[1])
        imu_pt.hz = float(hf[2])
        imu_pt.temp = 15.0
        result['imu'].append(imu_pt)

        if abs(alt[k] - last_gps_alt) > 0.0001:
            last_gps_alt = alt[k]
            gps_pt = GPSdata()
            gps_pt.time = float(t[k])
            #gps_pt.status = int(status)
            gps_pt.unix_sec = float(t[k])
            gps_pt.lat = float(lat[k])
            gps_pt.lon = float(lon[k])
            gps_pt.alt = float(alt[k])
            gps_pt.vn = float(vn[k])
            gps_pt.ve = float(ve[k])
            gps_pt.vd = float(vd[k])
            gps_pt.sats = 8     # force a reasonable value (not logged?)
            result['gps'].append(gps_pt)

        air_pt = Airdata()
        air_pt.time = float(t[k])
        air_pt.airspeed = float(flight_data.ias[k]*mps2kt)
        air_pt.altitude = float(flight_data.h[k])
        result['air'].append(air_pt)
        
        nav = NAVdata()
        nav.time = float(t[k])
        nav.lat = float(flight_data.navlat[k])
        nav.lon = float(flight_data.navlon[k])
        nav.alt = float(flight_data.navalt[k])
        nav.vn = float(flight_data.navvn[k])
        nav.ve = float(flight_data.navve[k])
        nav.vd = float(flight_data.navvd[k])
        nav.phi = float(flight_data.phi[k])
        nav.the = float(flight_data.theta[k])
        nav.psi = float(flight_data.psi[k])
        result['filter'].append(nav)

        k += 1

    dir = os.path.dirname(mat_filename)
    print 'dir:', dir
    
    filename = os.path.join(dir, 'imu-0.txt')
    f = open(filename, 'w')
    for imupt in result['imu']:
        line = [ '%.5f' % imupt.time, '%.4f' % imupt.p, '%.4f' % imupt.q, '%.4f' % imupt.r, '%.4f' % imupt.ax, '%.4f' % imupt.ay, '%.4f' % imupt.az, '%.4f' % imupt.hx, '%.4f' % imupt.hy, '%.4f' % imupt.hz, '%.4f' % imupt.temp, '0' ]
        f.write(','.join(line) + '\n')

    filename = os.path.join(dir, 'gps-0.txt')
    f = open(filename, 'w')
    for gpspt in result['gps']:
        line = [ '%.5f' % gpspt.time, '%.10f' % gpspt.lat, '%.10f' % gpspt.lon, '%.4f' % gpspt.alt, '%.4f' % gpspt.vn, '%.4f' % gpspt.ve, '%.4f' % gpspt.vd, '%.4f' % gpspt.time, '8', '0' ]
        f.write(','.join(line) + '\n')

    filename = os.path.join(dir, 'filter-0.txt')
    f = open(filename, 'w')
    r2d = 180.0 / math.pi
    for filtpt in result['filter']:
        line = [ '%.5f' % filtpt.time, '%.10f' % filtpt.lat, '%.10f' % filtpt.lon, '%.4f' % filtpt.alt, '%.4f' % filtpt.vn, '%.4f' % filtpt.ve, '%.4f' % filtpt.vd, '%.4f' % (filtpt.phi*r2d), '%.4f' % (filtpt.the*r2d), '%.4f' % (filtpt.psi*r2d), '0' ]
        f.write(','.join(line) + '\n')

    return result
예제 #3
0
파일: aura.py 프로젝트: AuraUAS/nav-devel
def load(flight_dir, recalibrate=None):
    result = {}

    # load imu/gps data files
    imu_file = flight_dir + "/imu-0.txt"
    imucal_json = flight_dir + "/imucal.json"
    imucal_xml = flight_dir + "/imucal.xml"
    gps_file = flight_dir + "/gps-0.txt"
    air_file = flight_dir + "/air-0.txt"
    filter_file = flight_dir + "/filter-0.txt"
    pilot_file = flight_dir + "/pilot-0.txt"
    act_file = flight_dir + "/act-0.txt"
    ap_file = flight_dir + "/ap-0.txt"
    imu_bias_file = flight_dir + "/imubias.txt"

    # HEY: in the latest aura code, calibrated magnetometer is logged,
    # not raw magnetometer, so we don't need to correct here.  We
    # could 'back correct' if we wanted original values for some
    # reason (using the calibration matrix saved with each flight
    # log.)
    
    # # vireo_01
    # mag_affine = np.array(
    #     [[ 1.6207467043,  0.0228992488,  0.0398638786,  0.1274248748],
    #      [-0.0169905025,  1.7164397411, -0.0001290047, -0.1140304977],
    #      [ 0.0424979948, -0.0038515935,  1.7193766423, -0.1449816095],
    #      [ 0.          ,  0.          ,  0.          ,  1.          ]]
    # )

    # Tyr
    # mag_affine = np.array(
    #     [[ 1.810,  0.109,  0.285, -0.237],
    #      [-0.078,  1.931, -0.171, -0.060],
    #      [ 0.008,  0.109,  1.929,  0.085],
    #      [ 0.   ,  0.   ,  0.   ,  1.      ]]
    # )

    # telemaster apm2_101
    mag_affine = np.array(
        [[ 0.0026424919,  0.0001334248,  0.0000984977, -0.2235908546],
         [-0.000081925 ,  0.0026419229,  0.0000751835, -0.0010757621],
         [ 0.0000219407,  0.0000560341,  0.002541171 ,  0.040221458 ],
         [ 0.          ,  0.          ,  0.          ,  1.          ]]
    )
    
    # skywalker apm2_105
    # mag_affine = np.array(
    #      [[ 0.0025834778, 0.0001434776, 0.0001434961, -0.7900775707 ],
    #       [-0.0001903118, 0.0024796553, 0.0001365238,  0.1881142449 ],
    #       [ 0.0000556437, 0.0001329724, 0.0023791184,  0.1824851582, ],
    #       [ 0.0000000000, 0.0000000000, 0.0000000000,  1.0000000000  ]]
    # )

    #np.set_printoptions(precision=10,suppress=True)
    #print mag_affine
    
    result['imu'] = []
    fimu = fileinput.input(imu_file)
    for line in fimu:
        tokens = re.split('[,\s]+', line.rstrip())
        #s = [float(tokens[7]), float(tokens[8]), float(tokens[9]), 1.0]
        #hf = np.dot(mag_affine, s)
        #print hf
        imu = IMUdata()
        imu.time = float(tokens[0])
        imu.p = float(tokens[1])
        imu.q = float(tokens[2])
        imu.r = float(tokens[3])
        imu.ax = float(tokens[4])
        imu.ay = float(tokens[5])
        imu.az = float(tokens[6])
        imu.hx = float(tokens[7])
        imu.hy = float(tokens[8])
        imu.hz = float(tokens[9])
        imu.temp = float(tokens[10])
        result['imu'].append( imu )

    result['gps'] = []
    fgps = fileinput.input(gps_file)
    last_time = -1.0
    for line in fgps:
        # Note: the aura logs unix time of the gps record, not tow,
        # but for the purposes of the insgns algorithm, it's only
        # important to have a properly incrementing clock, it doesn't
        # really matter what the zero reference point of time is.
        tokens = re.split('[,\s]+', line.rstrip())
        time = float(tokens[0])
        sats = int(tokens[8])
        if sats >= 5 and time > last_time:
            gps = GPSdata()
            gps.time = time
            gps.unix_sec = float(tokens[7])
            gps.lat = float(tokens[1])
            gps.lon = float(tokens[2])
            gps.alt = float(tokens[3])
            gps.vn = float(tokens[4])
            gps.ve = float(tokens[5])
            gps.vd = float(tokens[6])
            gps.sats = sats
            result['gps'].append(gps)
        last_time = time

    if os.path.exists(air_file):
        result['air'] = []
        fair = fileinput.input(air_file)
        for line in fair:
            tokens = re.split('[,\s]+', line.rstrip())
            air = Airdata()
            air.time = float(tokens[0])
            air.static_press = float(tokens[1])
            air.diff_press = 0.0    # not directly available in flight log
            air.temp = float(tokens[2])
            air.airspeed = float(tokens[3])
            air.alt_press = float(tokens[4])
            air.alt_true = float(tokens[5])
            result['air'].append( air )

    # load filter records if they exist (for comparison purposes)
    result['filter'] = []
    ffilter = fileinput.input(filter_file)
    for line in ffilter:
        tokens = re.split('[,\s]+', line.rstrip())
        lat = float(tokens[1])
        lon = float(tokens[2])
        if abs(lat) > 0.0001 and abs(lon) > 0.0001:
            nav = NAVdata()
            nav.time = float(tokens[0])
            nav.lat = lat*d2r
            nav.lon = lon*d2r
            nav.alt = float(tokens[3])
            nav.vn = float(tokens[4])
            nav.ve = float(tokens[5])
            nav.vd = float(tokens[6])
            nav.phi = float(tokens[7])*d2r
            nav.the = float(tokens[8])*d2r
            psi = float(tokens[9])
            if psi > 180.0:
                psi = psi - 360.0
            if psi < -180.0:
                psi = psi + 360.0
            nav.psi = psi*d2r
            result['filter'].append(nav)

    if os.path.exists(pilot_file):
        result['pilot'] = []
        fpilot = fileinput.input(pilot_file)
        for line in fpilot:
            tokens = re.split('[,\s]+', line.rstrip())
            pilot = Controldata()
            pilot.time = float(tokens[0])
            pilot.aileron = float(tokens[1])
            pilot.elevator = float(tokens[2])
            pilot.throttle = float(tokens[3])
            pilot.rudder = float(tokens[4])
            pilot.gear = float(tokens[5])
            pilot.flaps = float(tokens[6])
            pilot.aux1 = float(tokens[7])
            pilot.auto_manual = float(tokens[8])
            result['pilot'].append(pilot)

    if os.path.exists(act_file):
        result['act'] = []
        fact = fileinput.input(act_file)
        for line in fact:
            tokens = re.split('[,\s]+', line.rstrip())
            act = Controldata()
            act.time = float(tokens[0])
            act.aileron = float(tokens[1])
            act.elevator = float(tokens[2])
            act.throttle = float(tokens[3])
            act.rudder = float(tokens[4])
            act.gear = float(tokens[5])
            act.flaps = float(tokens[6])
            act.aux1 = float(tokens[7])
            act.auto_manual = float(tokens[8])
            result['act'].append(act)

    if os.path.exists(ap_file):
        result['ap'] = []
        fap = fileinput.input(ap_file)
        for line in fap:
            tokens = re.split('[,\s]+', line.rstrip())
            ap = APdata()
            ap.time = float(tokens[0])
            ap.hdg = float(tokens[1])
            ap.roll = float(tokens[2])
            ap.alt = float(tokens[3])
            ap.pitch = float(tokens[5])
            ap.speed = float(tokens[7])
            #ap.flight_time = float(tokens[8])
            #ap.target_wp = int(tokens[9])
            #ap.wp_lon = float(tokens[10])
            #ap.wp_lat = float(tokens[11])
            #ap.wp_index = int(tokens[12])
            #ap.route_size = int(tokens[13])
            result['ap'].append(ap)

    cal = imucal.Calibration()
    if os.path.exists(imucal_json):
        imucal_file = imucal_json
    elif os.path.exists(imucal_xml):
        imucal_file = imucal_xml
    else:
        imucal_file = None
    if imucal_file:
        cal.load(imucal_file)
        print 'back correcting imu data (to get original raw values)'
        result['imu'] = cal.back_correct(result['imu'])

    if recalibrate:
        print 'recalibrating imu data using alternate calibration file:', recalibrate
        rcal = imucal.Calibration()
        rcal.load(recalibrate)
        result['imu'] = rcal.correct(result['imu'])

    return result
예제 #4
0
def load(csv_base):
    result = {}

    comb_path = csv_base + '_sensor_combined_0.csv'
    gps_path = csv_base + '_vehicle_gps_position_0.csv'
    att_path = csv_base + '_vehicle_attitude_0.csv'
    pos_path = csv_base + '_vehicle_global_position_0.csv'

    result['imu'] = []
    with open(comb_path, 'rb') as f:
        reader = csv.DictReader(f)
        for row in reader:
            imu = IMUdata()
            imu.time = float(row['timestamp']) / 1000000.0
            imu.p = float(row['gyro_rad[0]'])
            imu.q = float(row['gyro_rad[1]'])
            imu.r = float(row['gyro_rad[2]'])
            imu.ax = float(row['accelerometer_m_s2[0]'])
            imu.ay = float(row['accelerometer_m_s2[1]'])
            imu.az = float(row['accelerometer_m_s2[2]'])
            hx = float(row['magnetometer_ga[0]'])
            hy = float(row['magnetometer_ga[0]'])
            hz = float(row['magnetometer_ga[0]'])
            hf = np.array([hx, hy, hz])
            norm = np.linalg.norm(hf)
            hf /= norm
            imu.hx = hf[0]
            imu.hy = hf[1]
            imu.hz = hf[2]
            #print imu.hx, imu.hy, imu.hz
            imu.temp = 15.0
            result['imu'].append(imu)

    result['gps'] = []
    with open(gps_path, 'rb') as f:
        reader = csv.DictReader(f)
        for row in reader:
            gps = GPSdata()
            gps.time = float(row['timestamp']) / 1000000.0
            gps.unix_sec = float(row['time_utc_usec']) / 1000000.0
            gps.lat = float(row['lat']) / 1e7
            gps.lon = float(row['lon']) / 1e7
            gps.alt = float(row['alt']) / 1e3
            # print gps.lat, gps.lon, gps.alt
            gps.vn = float(row['vel_n_m_s'])
            gps.ve = float(row['vel_e_m_s'])
            gps.vd = float(row['vel_d_m_s'])
            gps.sats = int(row['satellites_used'])
            if gps.sats >= 5:
                result['gps'].append(gps)

    # result['air'] = []
    # with open(gps_path, 'rb') as f:
    #     reader = csv.DictReader(f)
    #     for row in reader:
    #         air = Airdata()
    #         air.time = imu.time
    #         air.static_press = float(row['SENS_BaroPres'])
    #         air.diff_press = float(row['SENS_DiffPres'])
    #         air.temp = float(row['AIRS_AirTemp'])
    #         air.airspeed = float(row['AIRS_IndSpeed'])
    #         air.alt_press = float(row['SENS_BaroAlt'])
    #         air.alt_true = gps.alt
    #         result['air'].append( air )

    att = []
    with open(att_path, 'rb') as f:
        reader = csv.DictReader(f)
        for row in reader:
            time = float(row['timestamp']) / 1000000.0
            q = [
                float(row['q[0]']),
                float(row['q[1]']),
                float(row['q[2]']),
                float(row['q[3]'])
            ]
            # either of these will work ...
            (roll, pitch, yaw) = px4_quat2euler(q)
            att.append([time, yaw, pitch, roll])

    pos = []
    with open(pos_path, 'rb') as f:
        reader = csv.DictReader(f)
        for row in reader:
            time = float(row['timestamp']) / 1000000.0
            lat = float(row['lat']) * d2r
            lon = float(row['lon']) * d2r
            alt = float(row['alt'])
            vn = float(row['vel_n'])
            ve = float(row['vel_e'])
            vd = float(row['vel_d'])
            pos.append([time, lat, lon, alt, vn, ve, vd])
    pos_array = np.array(pos)

    lat_interp = interpolate.interp1d(pos_array[:, 0],
                                      pos_array[:, 1],
                                      bounds_error=False,
                                      fill_value='extrapolate')
    lon_interp = interpolate.interp1d(pos_array[:, 0],
                                      pos_array[:, 2],
                                      bounds_error=False,
                                      fill_value='extrapolate')
    alt_interp = interpolate.interp1d(pos_array[:, 0],
                                      pos_array[:, 3],
                                      bounds_error=False,
                                      fill_value='extrapolate')
    vn_interp = interpolate.interp1d(pos_array[:, 0],
                                     pos_array[:, 4],
                                     bounds_error=False,
                                     fill_value=0.0)
    ve_interp = interpolate.interp1d(pos_array[:, 0],
                                     pos_array[:, 5],
                                     bounds_error=False,
                                     fill_value=0.0)
    vd_interp = interpolate.interp1d(pos_array[:, 0],
                                     pos_array[:, 6],
                                     bounds_error=False,
                                     fill_value=0.0)

    result['filter'] = []
    for a in att:
        nav = NAVdata()
        nav.time = a[0]
        nav.lat = float(lat_interp(nav.time))
        nav.lon = float(lon_interp(nav.time))
        nav.alt = float(alt_interp(nav.time))
        nav.vn = float(vn_interp(nav.time))
        nav.ve = float(ve_interp(nav.time))
        nav.vd = float(vd_interp(nav.time))
        nav.phi = a[3]
        nav.the = a[2]
        nav.psi = a[1]
        result['filter'].append(nav)

    #result['pilot'] = []
    #result['act'] = []
    #result['ap'] = []

    return result
예제 #5
0
def load(mat_filename):
    # Name of .mat file that exists in the directory defined above and
    # has the flight_data and flight_info structures
    filepath = mat_filename

    # Load Flight Data: ## IMPORTANT to have the .mat file in the
    # flight_data and flight_info structures for this function ##
    data = sio.loadmat(filepath, struct_as_record=False, squeeze_me=True)
    print 'Loaded Data Summary'
    print '* File: %s' % filepath.split(os.path.sep)[-1]
    try:
        flight_data, flight_info = data['flight_data'], data['flight_info']
        print('* Date: %s' % flight_info.date)
        print('* Aircraft: %s' % flight_info.aircraft)
    except KeyError:
        print 'KeyError'
        # Convert from Python dictionary to struct-like before
        flight_data = dict2struct()
        for k in data:
            exec("flight_data.%s = data['%s']" % (k, k))
    del (data)

    # Add both names for pitch: the and theta
    try:
        flight_data.theta = flight_data.the
    except AttributeError:
        pass

    # Fill in time data
    t = flight_data.time
    print 't:', t

    # Magnetometer data - not used hence don't trust
    hm = np.vstack((flight_data.hx, -flight_data.hy, -flight_data.hz)).T

    # Geri mag calibration
    mag_affine = np.array(
        [[4.3374191736, 0.9527668819, 0.2106929615, 0.0649324241],
         [-3.7617930866, 6.2839014058, -3.5707398622, 2.1616165305],
         [-0.8688354599, 0.2205650877, 4.7466115481, -2.7041600008],
         [0., 0., 0., 1.]])

    # Populate IMU Data
    imu = np.vstack(
        (t, flight_data.p, flight_data.q, flight_data.r, flight_data.ax,
         flight_data.ay, flight_data.az, hm[:, 0], hm[:, 1], hm[:, 2])).T

    # Note that accelerometer and gyro measurements logged by UAV
    # after 11/17/2011 flight (seemingly, see
    # http://trac.umnaem.webfactional.com/wiki/FlightReports/2011_11_17)
    # have the nav-estimated bias removed before datalogging. So to work with raw
    # imu-data, we add back the on-board estimated biases.
    if not FLAG_UNBIASED_IMU:
        try:
            imu[:, 1:4] += np.vstack(
                (flight_data.p_bias, flight_data.q_bias, flight_data.r_bias)).T

            imu[:, 4:7] += np.vstack((flight_data.ax_bias, flight_data.ay_bias,
                                      flight_data.az_bias)).T
        except AttributeError:
            print('Note: On board estimated bias not found.')

    # Air Data
    ias = flight_data.ias  # indicated airspeed (m/s)
    h = flight_data.h

    # Populate GPS sensor data
    try:
        vn = flight_data.gps_vn
    except:
        vn = flight_data.vn
    try:
        ve = flight_data.gps_ve
    except:
        ve = flight_data.ve
    try:
        vd = flight_data.gps_vd
    except:
        vd = flight_data.vd
    lat = flight_data.lat
    lon = flight_data.lon
    alt = flight_data.alt

    # kstart set to when the navigation filter used onboard the aircraft
    # was initialized and this is accomplished by detecting when navlat is
    # no longer 0.0. This choice of kstart will ensure the filter being
    # tested is using the same initialization time step as the onboard
    # filter allowing for apples to apples comparisons.
    kstart = (abs(flight_data.navlat) > 0.0).tolist().index(True)
    k = kstart
    print('Initialized at Time: %.2f s (k=%i)' % (t[k], k))

    # Set previous value of GPS altitude to 0.0. This will be used to
    # trigger GPS newData flag which is commonly used in our
    # navigation filters for deciding if the GPS data has been
    # updated. However, in python we have no log of newData
    # (typically). So a comparison of current GPS altitude to the
    # previous epoch's GPS altitude is used to determine if GPS has
    # been updated.
    last_gps_alt = -9999.9

    # create data structures for ekf processing
    result = {}
    result['imu'] = []
    result['gps'] = []
    result['air'] = []
    result['filter'] = []

    k = kstart
    while k < len(t):
        p, q, r = imu[k, 1:4]
        ax, ay, az = imu[k, 4:7]
        hx, hy, hz = imu[k, 7:10]

        s = [hx, hy, hz, 1.0]
        #hf = np.dot(mag_affine, s)
        hf = s

        imu_pt = IMUdata()
        imu_pt.time = float(t[k])
        imu_pt.p = float(p)
        imu_pt.q = float(q)
        imu_pt.r = float(r)
        imu_pt.ax = float(ax)
        imu_pt.ay = float(ay)
        imu_pt.az = float(az)
        #imu_pt.hx = hx
        #imu_pt.hy = hy
        #imu_pt.hz = hz
        imu_pt.hx = float(hf[0])
        imu_pt.hy = float(hf[1])
        imu_pt.hz = float(hf[2])
        imu_pt.temp = 15.0
        result['imu'].append(imu_pt)

        if abs(alt[k] - last_gps_alt) > 0.0001:
            last_gps_alt = alt[k]
            gps_pt = GPSdata()
            gps_pt.time = float(t[k])
            #gps_pt.status = int(status)
            gps_pt.unix_sec = float(t[k])
            gps_pt.lat = float(lat[k])
            gps_pt.lon = float(lon[k])
            gps_pt.alt = float(alt[k])
            gps_pt.vn = float(vn[k])
            gps_pt.ve = float(ve[k])
            gps_pt.vd = float(vd[k])
            gps_pt.sats = 8  # force a reasonable value (not logged?)
            result['gps'].append(gps_pt)

        air_pt = Airdata()
        air_pt.time = float(t[k])
        air_pt.airspeed = float(flight_data.ias[k] * mps2kt)
        air_pt.altitude = float(flight_data.h[k])
        result['air'].append(air_pt)

        nav = NAVdata()
        nav.time = float(t[k])
        nav.lat = float(flight_data.navlat[k])
        nav.lon = float(flight_data.navlon[k])
        nav.alt = float(flight_data.navalt[k])
        nav.vn = float(flight_data.navvn[k])
        nav.ve = float(flight_data.navve[k])
        nav.vd = float(flight_data.navvd[k])
        nav.phi = float(flight_data.phi[k])
        nav.the = float(flight_data.theta[k])
        nav.psi = float(flight_data.psi[k])
        result['filter'].append(nav)

        k += 1

    dir = os.path.dirname(mat_filename)
    print 'dir:', dir

    filename = os.path.join(dir, 'imu-0.txt')
    f = open(filename, 'w')
    for imupt in result['imu']:
        line = [
            '%.5f' % imupt.time,
            '%.4f' % imupt.p,
            '%.4f' % imupt.q,
            '%.4f' % imupt.r,
            '%.4f' % imupt.ax,
            '%.4f' % imupt.ay,
            '%.4f' % imupt.az,
            '%.4f' % imupt.hx,
            '%.4f' % imupt.hy,
            '%.4f' % imupt.hz,
            '%.4f' % imupt.temp, '0'
        ]
        f.write(','.join(line) + '\n')

    filename = os.path.join(dir, 'gps-0.txt')
    f = open(filename, 'w')
    for gpspt in result['gps']:
        line = [
            '%.5f' % gpspt.time,
            '%.10f' % gpspt.lat,
            '%.10f' % gpspt.lon,
            '%.4f' % gpspt.alt,
            '%.4f' % gpspt.vn,
            '%.4f' % gpspt.ve,
            '%.4f' % gpspt.vd,
            '%.4f' % gpspt.time, '8', '0'
        ]
        f.write(','.join(line) + '\n')

    filename = os.path.join(dir, 'filter-0.txt')
    f = open(filename, 'w')
    r2d = 180.0 / math.pi
    for filtpt in result['filter']:
        line = [
            '%.5f' % filtpt.time,
            '%.10f' % filtpt.lat,
            '%.10f' % filtpt.lon,
            '%.4f' % filtpt.alt,
            '%.4f' % filtpt.vn,
            '%.4f' % filtpt.ve,
            '%.4f' % filtpt.vd,
            '%.4f' % (filtpt.phi * r2d),
            '%.4f' % (filtpt.the * r2d),
            '%.4f' % (filtpt.psi * r2d), '0'
        ]
        f.write(','.join(line) + '\n')

    return result
예제 #6
0
def load(mat_filename):
    # Name of .mat file that exists in the directory defined above and
    # has the flight_data and flight_info structures
    filepath = mat_filename

    # Load Flight Data: ## IMPORTANT to have the .mat file in the
    # flight_data and flight_info structures for this function ##
    data = sio.loadmat(filepath, struct_as_record=False, squeeze_me=True)
    for keys in data:
        print keys

    # print 'Loaded Data Summary'
    # print '* File: %s' % filepath.split(os.path.sep)[-1]

    # # Fill in time data
    # t = flight_data.time
    # print 't:', t

    # # Magnetometer data - not used hence don't trust
    # hm  = np.vstack((flight_data.hx, -flight_data.hy, -flight_data.hz)).T

    # # Geri mag calibration
    # mag_affine = np.array(
    #     [[ 4.3374191736,  0.9527668819,  0.2106929615,  0.0649324241],
    #      [-3.7617930866,  6.2839014058, -3.5707398622,  2.1616165305],
    #      [-0.8688354599,  0.2205650877,  4.7466115481, -2.7041600008],
    #      [ 0.          ,  0.          ,  0.          ,  1.          ]]
    # )

    # # Populate IMU Data
    # imu = np.vstack((t, flight_data.p, flight_data.q, flight_data.r,
    #                  flight_data.ax, flight_data.ay, flight_data.az,
    #                  hm[:,0], hm[:,1], hm[:,2])).T

    # # Note that accelerometer and gyro measurements logged by UAV
    # # after 11/17/2011 flight (seemingly, see
    # # http://trac.umnaem.webfactional.com/wiki/FlightReports/2011_11_17)
    # # have the nav-estimated bias removed before datalogging. So to work with raw
    # # imu-data, we add back the on-board estimated biases.
    # if not FLAG_UNBIASED_IMU:
    #     try:
    #         imu[:, 1:4] += np.vstack((flight_data.p_bias,
    #                                   flight_data.q_bias,
    #                                   flight_data.r_bias)).T

    #         imu[:, 4:7] += np.vstack((flight_data.ax_bias,
    #                                   flight_data.ay_bias,
    #                                   flight_data.az_bias)).T
    #     except AttributeError:
    #         print('Note: On board estimated bias not found.')

    # # Air Data
    # ias = flight_data.ias # indicated airspeed (m/s)
    # h = flight_data.h

    # # Populate GPS sensor data
    # try:
    #     vn = flight_data.gps_vn
    # except:
    #     vn = flight_data.vn
    # try:
    #     ve = flight_data.gps_ve
    # except:
    #     ve = flight_data.ve
    # try:
    #     vd = flight_data.gps_vd
    # except:
    #     vd = flight_data.vd
    # lat = flight_data.lat
    # lon = flight_data.lon
    # alt = flight_data.alt

    # # kstart set to when the navigation filter used onboard the aircraft
    # # was initialized and this is accomplished by detecting when navlat is
    # # no longer 0.0. This choice of kstart will ensure the filter being
    # # tested is using the same initialization time step as the onboard
    # # filter allowing for apples to apples comparisons.
    # kstart = (abs(flight_data.navlat) > 0.0).tolist().index(True)
    # k = kstart
    # print('Initialized at Time: %.2f s (k=%i)' % (t[k], k))

    # # Set previous value of GPS altitude to 0.0. This will be used to
    # # trigger GPS newData flag which is commonly used in our
    # # navigation filters for deciding if the GPS data has been
    # # updated. However, in python we have no log of newData
    # # (typically). So a comparison of current GPS altitude to the
    # # previous epoch's GPS altitude is used to determine if GPS has
    # # been updated.
    # last_gps_alt = -9999.9

    # create data structures for ekf processing
    result = {}
    result['imu'] = []
    result['gps'] = []
    result['air'] = []
    result['filter'] = []

    last_gps_lon = -9999.0
    last_gps_lat = -9999.0

    for i in range(len(data['Time_s'])):
        # p, q, r = imu[k, 1:4]
        # ax, ay, az = imu[k, 4:7]
        # hx, hy, hz = imu[k, 7:10]

        s = [
            data['IMU_Hx_uT'][i], data['IMU_Hy_uT'][i], data['IMU_Hz_uT'][i],
            1.0
        ]
        # hf = np.dot(mag_affine, s) # apply mag calibration
        hf = s

        imu_pt = IMUdata()
        imu_pt.time = data['Time_s'][i]
        imu_pt.p = data['IMU_Gx_rads'][i]
        imu_pt.q = data['IMU_Gy_rads'][i]
        imu_pt.r = data['IMU_Gz_rads'][i]
        imu_pt.ax = data['IMU_Ax_mss'][i]
        imu_pt.ay = data['IMU_Ay_mss'][i]
        imu_pt.az = data['IMU_Az_mss'][i]
        imu_pt.hx = float(hf[0])
        imu_pt.hy = float(hf[1])
        imu_pt.hz = float(hf[2])
        imu_pt.temp = data['IMU_Temp_C'][i]
        result['imu'].append(imu_pt)

        lat = data['GPS_Latitude'][i] * r2d
        lon = data['GPS_Longitude'][i] * r2d
        #print lon,lat
        if abs(lat - last_gps_lat) > 0.0000000001 or abs(
                lon - last_gps_lon) > 0.0000000000001:
            last_gps_lat = lat
            last_gps_lon = lon
            gps_pt = GPSdata()
            gps_pt.time = data['Time_s'][i]
            #gps_pt.status = something
            gps_pt.unix_sec = data['Time_s'][
                i]  # hack an incrementing time here
            gps_pt.lat = lat
            gps_pt.lon = lon
            gps_pt.alt = data['GPS_Altitude'][i]
            gps_pt.vn = data['GPS_North_Velocity'][i]
            gps_pt.ve = data['GPS_East_Velocity'][i]
            gps_pt.vd = data['GPS_Down_Velocity'][i]
            gps_pt.sats = int(data['GPS_NumberSatellites'][i])
            result['gps'].append(gps_pt)

        pitot_calibrate = 1.0
        diff_pa = data['Pitot_DiffPressure_Pressure_Pa'][i]
        if diff_pa < 0:
            diff_pa = 0.0
        static_pa = data['Pitot_StaticPressure_Pressure_Pa'][i]
        airspeed_mps = math.sqrt(2 * diff_pa / 1.225) * pitot_calibrate
        P0 = 1013.25
        T = 15.0
        P = static_pa / 100.0  # convert to mbar
        tmp1 = math.pow((P0 / P), 1.0 / 5.257) - 1.0
        alt_m = (tmp1 * (T + 273.15)) / 0.0065

        air_pt = Airdata()
        air_pt.time = data['Time_s'][i]
        air_pt.airspeed = airspeed_mps * mps2kt
        air_pt.altitude = alt_m
        result['air'].append(air_pt)

        # nav = NAVdata()
        # nav.time = float(t[k])
        # nav.lat = float(flight_data.navlat[k])
        # nav.lon = float(flight_data.navlon[k])
        # nav.alt = float(flight_data.navalt[k])
        # nav.vn = float(flight_data.navvn[k])
        # nav.ve = float(flight_data.navve[k])
        # nav.vd = float(flight_data.navvd[k])
        # nav.phi = float(flight_data.phi[k])
        # nav.the = float(flight_data.theta[k])
        # nav.psi = float(flight_data.psi[k])
        # result['filter'].append(nav)

    dir = os.path.dirname(mat_filename)
    print 'dir:', dir

    filename = os.path.join(dir, 'imu-0.txt')
    f = open(filename, 'w')
    for imupt in result['imu']:
        line = [
            '%.5f' % imupt.time,
            '%.4f' % imupt.p,
            '%.4f' % imupt.q,
            '%.4f' % imupt.r,
            '%.4f' % imupt.ax,
            '%.4f' % imupt.ay,
            '%.4f' % imupt.az,
            '%.4f' % imupt.hx,
            '%.4f' % imupt.hy,
            '%.4f' % imupt.hz,
            '%.4f' % imupt.temp, '0'
        ]
        f.write(','.join(line) + '\n')

    filename = os.path.join(dir, 'gps-0.txt')
    f = open(filename, 'w')
    for gpspt in result['gps']:
        line = [
            '%.5f' % gpspt.time,
            '%.10f' % gpspt.lat,
            '%.10f' % gpspt.lon,
            '%.4f' % gpspt.alt,
            '%.4f' % gpspt.vn,
            '%.4f' % gpspt.ve,
            '%.4f' % gpspt.vd,
            '%.4f' % gpspt.time, '8', '0'
        ]
        f.write(','.join(line) + '\n')

    if 'filter' in result:
        filename = os.path.join(dir, 'filter-0.txt')
        f = open(filename, 'w')
        for filtpt in result['filter']:
            line = [
                '%.5f' % filtpt.time,
                '%.10f' % filtpt.lat,
                '%.10f' % filtpt.lon,
                '%.4f' % filtpt.alt,
                '%.4f' % filtpt.vn,
                '%.4f' % filtpt.ve,
                '%.4f' % filtpt.vd,
                '%.4f' % (filtpt.phi * r2d),
                '%.4f' % (filtpt.the * r2d),
                '%.4f' % (filtpt.psi * r2d), '0'
            ]
            f.write(','.join(line) + '\n')

    return result
예제 #7
0
def load(flight_dir, recalibrate=None):
    result = {}

    # load imu/gps data files
    imu_file = flight_dir + "/imu-0.txt"
    imucal_json = flight_dir + "/imucal.json"
    imucal_xml = flight_dir + "/imucal.xml"
    gps_file = flight_dir + "/gps-0.txt"
    air_file = flight_dir + "/air-0.txt"
    filter_file = flight_dir + "/filter-0.txt"
    filter_post = flight_dir + "/filter-post.txt"
    pilot_file = flight_dir + "/pilot-0.txt"
    act_file = flight_dir + "/act-0.txt"
    ap_file = flight_dir + "/ap-0.txt"
    imu_bias_file = flight_dir + "/imubias.txt"

    # HEY: in the latest aura code, calibrated magnetometer is logged,
    # not raw magnetometer, so we don't need to correct here.  We
    # could 'back correct' if we wanted original values for some
    # reason (using the calibration matrix saved with each flight
    # log.)

    # # vireo_01
    # mag_affine = np.array(
    #     [[ 1.6207467043,  0.0228992488,  0.0398638786,  0.1274248748],
    #      [-0.0169905025,  1.7164397411, -0.0001290047, -0.1140304977],
    #      [ 0.0424979948, -0.0038515935,  1.7193766423, -0.1449816095],
    #      [ 0.          ,  0.          ,  0.          ,  1.          ]]
    # )

    # Tyr
    # mag_affine = np.array(
    #     [[ 1.810,  0.109,  0.285, -0.237],
    #      [-0.078,  1.931, -0.171, -0.060],
    #      [ 0.008,  0.109,  1.929,  0.085],
    #      [ 0.   ,  0.   ,  0.   ,  1.      ]]
    # )

    # telemaster apm2_101
    mag_affine = np.array(
        [[0.0026424919, 0.0001334248, 0.0000984977, -0.2235908546],
         [-0.000081925, 0.0026419229, 0.0000751835, -0.0010757621],
         [0.0000219407, 0.0000560341, 0.002541171, 0.040221458],
         [0., 0., 0., 1.]])

    # skywalker apm2_105
    # mag_affine = np.array(
    #      [[ 0.0025834778, 0.0001434776, 0.0001434961, -0.7900775707 ],
    #       [-0.0001903118, 0.0024796553, 0.0001365238,  0.1881142449 ],
    #       [ 0.0000556437, 0.0001329724, 0.0023791184,  0.1824851582, ],
    #       [ 0.0000000000, 0.0000000000, 0.0000000000,  1.0000000000  ]]
    # )

    #np.set_printoptions(precision=10,suppress=True)
    #print mag_affine

    result['imu'] = []
    fimu = fileinput.input(imu_file)
    for line in fimu:
        tokens = re.split('[,\s]+', line.rstrip())
        #s = [float(tokens[7]), float(tokens[8]), float(tokens[9]), 1.0]
        #hf = np.dot(mag_affine, s)
        #print hf
        imu = IMUdata()
        imu.time = float(tokens[0])
        imu.p = float(tokens[1])
        imu.q = float(tokens[2])
        imu.r = float(tokens[3])
        imu.ax = float(tokens[4])
        imu.ay = float(tokens[5])
        imu.az = float(tokens[6])
        imu.hx = float(tokens[7])
        imu.hy = float(tokens[8])
        imu.hz = float(tokens[9])
        imu.temp = float(tokens[10])
        result['imu'].append(imu)

    result['gps'] = []
    fgps = fileinput.input(gps_file)
    last_time = -1.0
    for line in fgps:
        # Note: the aura logs unix time of the gps record, not tow,
        # but for the purposes of the insgns algorithm, it's only
        # important to have a properly incrementing clock, it doesn't
        # really matter what the zero reference point of time is.
        tokens = re.split('[,\s]+', line.rstrip())
        time = float(tokens[0])
        sats = int(tokens[8])
        if sats >= 5 and time > last_time:
            gps = GPSdata()
            gps.time = time
            gps.unix_sec = float(tokens[7])
            gps.lat = float(tokens[1])
            gps.lon = float(tokens[2])
            gps.alt = float(tokens[3])
            gps.vn = float(tokens[4])
            gps.ve = float(tokens[5])
            gps.vd = float(tokens[6])
            gps.sats = sats
            result['gps'].append(gps)
        last_time = time

    if os.path.exists(air_file):
        result['air'] = []
        fair = fileinput.input(air_file)
        for line in fair:
            tokens = re.split('[,\s]+', line.rstrip())
            air = Airdata()
            air.time = float(tokens[0])
            air.static_press = float(tokens[1])
            air.diff_press = 0.0  # not directly available in flight log
            air.temp = float(tokens[2])
            air.airspeed = float(tokens[3])
            air.alt_press = float(tokens[4])
            air.alt_true = float(tokens[5])
            result['air'].append(air)

    # load filter records if they exist (for comparison purposes)
    result['filter'] = []
    ffilter = fileinput.input(filter_file)
    for line in ffilter:
        tokens = re.split('[,\s]+', line.rstrip())
        lat = float(tokens[1])
        lon = float(tokens[2])
        if abs(lat) > 0.0001 and abs(lon) > 0.0001:
            nav = NAVdata()
            nav.time = float(tokens[0])
            nav.lat = lat * d2r
            nav.lon = lon * d2r
            nav.alt = float(tokens[3])
            nav.vn = float(tokens[4])
            nav.ve = float(tokens[5])
            nav.vd = float(tokens[6])
            nav.phi = float(tokens[7]) * d2r
            nav.the = float(tokens[8]) * d2r
            psi = float(tokens[9])
            if psi > 180.0:
                psi = psi - 360.0
            if psi < -180.0:
                psi = psi + 360.0
            nav.psi = psi * d2r
            result['filter'].append(nav)

    # load filter (post process) records if they exist (for comparison
    # purposes)
    if os.path.exists(filter_post):
        result['filter_post'] = []
        ffilter = fileinput.input(filter_post)
        for line in ffilter:
            tokens = re.split('[,\s]+', line.rstrip())
            lat = float(tokens[1])
            lon = float(tokens[2])
            if abs(lat) > 0.0001 and abs(lon) > 0.0001:
                nav = NAVdata()
                nav.time = float(tokens[0])
                nav.lat = lat * d2r
                nav.lon = lon * d2r
                nav.alt = float(tokens[3])
                nav.vn = float(tokens[4])
                nav.ve = float(tokens[5])
                nav.vd = float(tokens[6])
                nav.phi = float(tokens[7]) * d2r
                nav.the = float(tokens[8]) * d2r
                psi = float(tokens[9])
                if psi > 180.0:
                    psi = psi - 360.0
                if psi < -180.0:
                    psi = psi + 360.0
                nav.psi = psi * d2r
                result['filter_post'].append(nav)

    if os.path.exists(pilot_file):
        result['pilot'] = []
        fpilot = fileinput.input(pilot_file)
        for line in fpilot:
            tokens = re.split('[,\s]+', line.rstrip())
            pilot = Controldata()
            pilot.time = float(tokens[0])
            pilot.aileron = float(tokens[1])
            pilot.elevator = float(tokens[2])
            pilot.throttle = float(tokens[3])
            pilot.rudder = float(tokens[4])
            pilot.gear = float(tokens[5])
            pilot.flaps = float(tokens[6])
            pilot.aux1 = float(tokens[7])
            pilot.auto_manual = float(tokens[8])
            result['pilot'].append(pilot)

    if os.path.exists(act_file):
        result['act'] = []
        fact = fileinput.input(act_file)
        for line in fact:
            tokens = re.split('[,\s]+', line.rstrip())
            act = Controldata()
            act.time = float(tokens[0])
            act.aileron = float(tokens[1])
            act.elevator = float(tokens[2])
            act.throttle = float(tokens[3])
            act.rudder = float(tokens[4])
            act.gear = float(tokens[5])
            act.flaps = float(tokens[6])
            act.aux1 = float(tokens[7])
            act.auto_manual = float(tokens[8])
            result['act'].append(act)

    if os.path.exists(ap_file):
        result['ap'] = []
        fap = fileinput.input(ap_file)
        for line in fap:
            tokens = re.split('[,\s]+', line.rstrip())
            ap = APdata()
            ap.time = float(tokens[0])
            ap.hdg = float(tokens[1])
            ap.roll = float(tokens[2])
            ap.alt = float(tokens[3])
            ap.pitch = float(tokens[5])
            ap.speed = float(tokens[7])
            #ap.flight_time = float(tokens[8])
            #ap.target_wp = int(tokens[9])
            #ap.wp_lon = float(tokens[10])
            #ap.wp_lat = float(tokens[11])
            #ap.wp_index = int(tokens[12])
            #ap.route_size = int(tokens[13])
            result['ap'].append(ap)

    cal = imucal.Calibration()
    if os.path.exists(imucal_json):
        imucal_file = imucal_json
    elif os.path.exists(imucal_xml):
        imucal_file = imucal_xml
    else:
        imucal_file = None
    if imucal_file:
        cal.load(imucal_file)
        print 'back correcting imu data (to get original raw values)'
        result['imu'] = cal.back_correct(result['imu'])

    if recalibrate:
        print 'recalibrating imu data using alternate calibration file:', recalibrate
        rcal = imucal.Calibration()
        rcal.load(recalibrate)
        result['imu'] = rcal.correct(result['imu'])

    return result
예제 #8
0
def load(csv_base):
    result = {}

    comb_path = csv_base + '_sensor_combined_0.csv'
    gps_path = csv_base + '_vehicle_gps_position_0.csv'
    att_path = csv_base + '_vehicle_attitude_0.csv'
    pos_path = csv_base + '_vehicle_global_position_0.csv'
    ap_path = csv_base + '_vehicle_attitude_setpoint_0.csv'
    air_path = csv_base + '_airspeed_0.csv'
    act_path = csv_base + '_actuator_outputs_0.csv'
    filter_post = csv_base + '_filter_post.txt'

    result['imu'] = []
    with open(comb_path, 'rb') as f:
        reader = csv.DictReader(f)
        for row in reader:
            imu = IMUdata()
            imu.time = float(row['timestamp']) / 1000000.0
            imu.p = float(row['gyro_rad[0]'])
            imu.q = float(row['gyro_rad[1]'])
            imu.r = float(row['gyro_rad[2]'])
            imu.ax = float(row['accelerometer_m_s2[0]'])
            imu.ay = float(row['accelerometer_m_s2[1]'])
            imu.az = float(row['accelerometer_m_s2[2]'])
            hx = float(row['magnetometer_ga[0]'])
            hy = float(row['magnetometer_ga[0]'])
            hz = float(row['magnetometer_ga[0]'])
            hf = np.array([hx, hy, hz])
            norm = np.linalg.norm(hf)
            hf /= norm
            imu.hx = hf[0]
            imu.hy = hf[1]
            imu.hz = hf[2]
            #print imu.hx, imu.hy, imu.hz
            imu.temp = 15.0
            result['imu'].append(imu)

    result['gps'] = []
    with open(gps_path, 'rb') as f:
        reader = csv.DictReader(f)
        for row in reader:
            gps = GPSdata()
            gps.time = float(row['timestamp']) / 1000000.0
            gps.unix_sec = float(row['time_utc_usec']) / 1000000.0
            gps.lat = float(row['lat']) / 1e7
            gps.lon = float(row['lon']) / 1e7
            gps.alt = float(row['alt']) / 1e3
            # print gps.lat, gps.lon, gps.alt
            gps.vn = float(row['vel_n_m_s'])
            gps.ve = float(row['vel_e_m_s'])
            gps.vd = float(row['vel_d_m_s'])
            gps.sats = int(row['satellites_used'])
            if gps.sats >= 5:
                result['gps'].append(gps)

    result['air'] = []
    with open(air_path, 'rb') as f:
        reader = csv.DictReader(f)
        for row in reader:
            air = Airdata()
            air.time = float(row['timestamp']) / 1000000.0
            #air.static_press = float(row['SENS_BaroPres'])
            air.diff_press = float(row['differential_pressure_filtered_pa'])
            air.temp = float(row['air_temperature_celsius'])
            air.airspeed = float(row['indicated_airspeed_m_s']) * mps2kt
            #air.alt_press = float(row['SENS_BaroAlt'])
            air.alt_true = gps.alt
            result['air'].append(air)

    att = []
    with open(att_path, 'rb') as f:
        reader = csv.DictReader(f)
        for row in reader:
            time = float(row['timestamp']) / 1000000.0
            q = [
                float(row['q[0]']),
                float(row['q[1]']),
                float(row['q[2]']),
                float(row['q[3]'])
            ]
            # either of these will work ...
            (roll, pitch, yaw) = px4_quat2euler(q)
            att.append([time, yaw, pitch, roll])

    pos = []
    with open(pos_path, 'rb') as f:
        reader = csv.DictReader(f)
        for row in reader:
            time = float(row['timestamp']) / 1000000.0
            lat = float(row['lat']) * d2r
            lon = float(row['lon']) * d2r
            alt = float(row['alt'])
            vn = float(row['vel_n'])
            ve = float(row['vel_e'])
            vd = float(row['vel_d'])
            pos.append([time, lat, lon, alt, vn, ve, vd])
    pos_array = np.array(pos)

    lat_interp = interpolate.interp1d(pos_array[:, 0],
                                      pos_array[:, 1],
                                      bounds_error=False,
                                      fill_value='extrapolate')
    lon_interp = interpolate.interp1d(pos_array[:, 0],
                                      pos_array[:, 2],
                                      bounds_error=False,
                                      fill_value='extrapolate')
    alt_interp = interpolate.interp1d(pos_array[:, 0],
                                      pos_array[:, 3],
                                      bounds_error=False,
                                      fill_value='extrapolate')
    vn_interp = interpolate.interp1d(pos_array[:, 0],
                                     pos_array[:, 4],
                                     bounds_error=False,
                                     fill_value=0.0)
    ve_interp = interpolate.interp1d(pos_array[:, 0],
                                     pos_array[:, 5],
                                     bounds_error=False,
                                     fill_value=0.0)
    vd_interp = interpolate.interp1d(pos_array[:, 0],
                                     pos_array[:, 6],
                                     bounds_error=False,
                                     fill_value=0.0)

    if os.path.exists(ap_path):
        result['ap'] = []
        with open(ap_path, 'rb') as f:
            reader = csv.DictReader(f)
            for row in reader:
                ap = APdata()
                ap.time = float(row['timestamp']) / 1000000.0
                ap.hdg = float(row['yaw_body']) * r2d
                ap.roll = float(row['roll_body']) * r2d
                ap.pitch = float(row['pitch_body']) * r2d
                ap.alt = 0
                ap.speed = 0
                #ap.alt = float(row['GPSP_Alt']) * m2ft
                #ap.speed = float(row['TECS_AsSP']) * mps2kt
                result['ap'].append(ap)

    result['filter'] = []
    for a in att:
        nav = NAVdata()
        nav.time = a[0]
        nav.lat = float(lat_interp(nav.time))
        nav.lon = float(lon_interp(nav.time))
        nav.alt = float(alt_interp(nav.time))
        nav.vn = float(vn_interp(nav.time))
        nav.ve = float(ve_interp(nav.time))
        nav.vd = float(vd_interp(nav.time))
        nav.phi = a[3]
        nav.the = a[2]
        nav.psi = a[1]
        result['filter'].append(nav)

    #result['pilot'] = []
    #result['act'] = []
    #result['ap'] = []

    # load filter (post process) records if they exist (for comparison
    # purposes)
    if os.path.exists(filter_post):
        print "found filter_post.txt file"
        result['filter_post'] = []
        ffilter = fileinput.input(filter_post)
        for line in ffilter:
            tokens = re.split('[,\s]+', line.rstrip())
            lat = float(tokens[1])
            lon = float(tokens[2])
            if abs(lat) > 0.0001 and abs(lon) > 0.0001:
                nav = NAVdata()
                nav.time = float(tokens[0])
                nav.lat = lat * d2r
                nav.lon = lon * d2r
                nav.alt = float(tokens[3])
                nav.vn = float(tokens[4])
                nav.ve = float(tokens[5])
                nav.vd = float(tokens[6])
                nav.phi = float(tokens[7]) * d2r
                nav.the = float(tokens[8]) * d2r
                psi = float(tokens[9])
                if psi > 180.0:
                    psi = psi - 360.0
                if psi < -180.0:
                    psi = psi + 360.0
                nav.psi = psi * d2r
                result['filter_post'].append(nav)
    if os.path.exists(act_path):
        result['act'] = []
        with open(act_path, 'rb') as f:
            reader = csv.DictReader(f)
            for row in reader:
                act = Controldata()
                act.time = float(row['timestamp']) / 1000000.0
                ch = [0] * 8
                for i in range(len(ch)):
                    pwm = float(row['output[%d]' % i])
                    ch[i] = (pwm - 1500) / 500
                act.aileron = ch[0]
                act.elevator = ch[1]
                act.throttle = ch[2]
                act.rudder = ch[3]
                act.gear = 0
                act.flaps = 0
                act.aux1 = 0
                act.auto_manual = 0
                result['act'].append(act)

    return result