find best magnetometer offset fit to a log file

def magfit(logfile):
    '''find best magnetometer offset fit to a log file'''
    print("Processing log %s" % filename)
    mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)

    flying = False
    gps_heading = 0.0

    data = []

    # get the current mag offsets
    m = mlog.recv_match(type='SENSOR_OFFSETS',condition=args.condition)
    offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)

    attitude = mlog.recv_match(type='ATTITUDE',condition=args.condition)

    # now gather all the data
    while True:
        m = mlog.recv_match(condition=args.condition)
        if m is None:
            break
        if m.get_type() == "GPS_RAW":
            # flying if groundspeed more than 5 m/s
            flying = (m.v > args.minspeed and m.fix_type == 2)
            gps_heading = m.hdg
        if m.get_type() == "GPS_RAW_INT":
            # flying if groundspeed more than 5 m/s
            flying = (m.vel/100 > args.minspeed and m.fix_type == 3)
            gps_heading = m.cog/100
        if m.get_type() == "ATTITUDE":
            attitude = m
        if m.get_type() == "SENSOR_OFFSETS":
            # update current offsets
            offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
        if not flying:
            continue
        if m.get_type() == "RAW_IMU":
            data.append((m.xmag - offsets.x, m.ymag - offsets.y, m.zmag - offsets.z, attitude.roll, attitude.pitch, gps_heading))
    print("Extracted %u data points" % len(data))
    print("Current offsets: %s" % offsets)
    ofs2 = fit_data(data)
    print("Declination estimate: %.1f" % ofs2[-1])
    new_offsets = vec3(ofs2[0], ofs2[1], ofs2[2])
    a = [[ofs2[3], ofs2[4], ofs2[5]],
         [ofs2[6], ofs2[7], ofs2[8]],
         [ofs2[9], ofs2[10], ofs2[11]]]
    print(a)
    print("New offsets    : %s" % new_offsets)